/***********************************************************************
算法优点：采用8个光电传感器，可以平滑的转弯，不发生左右摆动。
见得比较多的小车寻机里都只用3、4个传感器，发生偏移就往另一个方向转向到头，
于是左右摆动严重。
缺点：我使用的是11v的锂电池，当电池满的时候小车运行没有任何问题，
但是如果电压低于8-7v（根据系统而定）这样就会由于电机的驱动电压不住导致转向不足，
所以在调试的时候出现小车越轨的情况先检查电压是否可以保证正常运作。
***********************************************************************/

#define REF_VALUE 4.5//当黑线在小车正中间的时候0 0 0 1 1 0 0 0，
//编码算法得到temp编码是9,黑线宽度3，两个传感器间距2.5，所以算出来的偏移量
//*bias = temp*1.0 / (*sum) - REF_VALUE;*bias = 0;不需偏移，保持正常速度，两电机速度相同
//REF_VALUE 是这么定下来的。也是根据系统而定（黑线宽度，传感器间距）

typedef struct motorModel {//电机结构(最初版本)
int proportion; // 斜率值，也即比例因子
int speedNormal;// 正常情况下的速度
int speed;    // 要发送的速度
} MOTORMODEL;
/***********************************************************************
初始化电机
***********************************************************************/
void init_motor(struct motorModel* motor, int proportion,int speedNormal)
{
motor->proportion = proportion;
motor->speedNormal = speedNormal;
motor->speed = 0;
}

/***********************************************************************
作者：谢受浪
更改：DreagonWoo。去掉与系统相关的代码，保证兼容性
根据从传感器读回来原始数据计算出偏移黑线值和在黑线上的传感器数目
sensorData:传感器数据（本系统使用8个光电传感器）
bias：偏移量（本函数就是计算这个偏移量）
sum：在黑线上的传感器数目
***********************************************************************/
int calc_sensor(unsigned char sensorData, float* bias, unsigned char* sum)
{
int i;
unsigned char temp, tempData;

// 用一个数组来记录sensorData中的每一个位
static unsigned char flagSensor[8] = {0};

tempData = sensorData;
for(i=0; i<8; i++) {//获取每一个传感器数据,根据传感器的摆放位置而定，是从左到右边，还是从有到左
   flagSensor[i] = sensorData & 0x01;
   sensorData >>= 1;
}

*sum = 0;
for(i=0; i<8; i++) {
   if(flagSensor[i])
    (*sum)++;
}

temp = 0;
for(i=0; i<8; i++) {//编码
   if(flagSensor[i])
    temp += (i+1);
}

if(temp != 0)
   *bias = temp*1.0 / (*sum) - REF_VALUE;//计算偏移量,本系统里偏移量0>向右边偏移，向左调整。

return 0;
}
/***********************************************************************
作者：谢受浪
更改：DreagonWoo。去掉与系统相关的代码，保证兼容性
设置速度左电机或右边电机速度
***********************************************************************/
void calc_motor(struct motorModel* motor, float bias)
{
motor->speed = (int)(motor->proportion * bias) + motor->speedNormal;
if(motor->speed > 255) motor->speed = 255;
if(motor->speed < -255) motor->speed = -255;
}

main()
{
int ret;//函数返回值
int biasValue,blackSum;
struct motorModel motor_L,    motor_R;
struct motorModel* pMotor_L = &motor_L;
struct motorModel* pMotor_R = &motor_R;

init_motor(pMotor_L, -65, 180);//左电机比例因子 -65，正常速度180（速度大小0-255，方向有正负）
init_motor(pMotor_R, 65, 180);
//ret = read(fd_sensor, &sensorData, sizeof(unsigned char));//与系统相关，更具系统而定
ret = calc_sensor(sensorData, &biasValue, &blackSum);
calc_motor(pMotor_L, biasValue);
calc_motor(pMotor_R, biasValue);
//.....发送电机数据
}