小车a_p程序i_d

#define dW digitalWrite
#define dR digitalRead

/* PIN DEFINITION */

#define BTN1  7
#define BTN2  6
#define LEDG  13

#define MOT1  2
#define MOT2  3
#define MOT3  4
#define MOT4  5

/* GLOBAL CONSTANTS */

#define WHITE  1
#define BLACK   0

#define COLOR   BLACK         //Line color

#define N_SENS        5       //Number of sensors
#define R_SENS        1000    //Sensor readings are mapped to this range
#define WEIGHT_UNIT   1000    //Unit for weighted average
#define RIGHT_DIR     1
#define LEFT_DIR      0
#define PRESS         0
#define RELEASE       1
#define CAL_SPEED     400     //Sensor calibration speed
#define CAL_TIME      300     //Calibration time
//最小的亮度比例，考虑到线的部分：百分之0.5
//Minimum brightness percentage to consider part of the line
#define P_LINE_MIN    0.5     


const float SPEED = 600;
const float KP = 0.4;
const float KD = 12;
const float KI = 0;

/* GLOBAL VARIABLES */

unsigned long ms = 0;
//SENSOR[0]=A0引脚
const int SENSOR[N_SENS] = { A0, A1, A2, A3, A4 };     //Arduino pins
//标定得到每个传感器的最大，最小值；可以是负数
int sens_max[N_SENS];  //Maximum value each sensor measures (calibrated)
int sens_min[N_SENS];  //Minimum value each sensor measures (calibrated)
int start = 0;
//位置线的数值，平均数，可以是小数点
float line_pos = 0;
float last_line_pos = 0;

void setup() {
  Serial.begin(9600);
  InitializeGPIO();
  //循环赋值，得到每个传感器的最大值，最小值，是初始值；
  for(int x = 0; x < N_SENS; x++){
      sens_max[x] = 0;
      sens_min[x] = 1023;
  }
  
}

//获取线的位置信息
//传入：颜色信息，上次的方向信息

float get_line_pos(int color, int last_dir){
    float line = 0;
    int line_detected = 0;
    float sens_scaled[N_SENS];//每个传感器的归化
    float avg_num = 0;  //Average numerator数值的平均数
    float avg_den = 0;  //Average denominator 分母的平均数
    

    for(int x = 0; x < N_SENS; x++)  //遍历全部传感器
    {
        //Scale from 0 to R_SENS
        //读取每个传感器引脚的模拟值，减去最小值，比如最小值是100，最大值是600
        //那么得到每个传感器的假如读取到的600，250，360，450，300
        //600-100=500  250-100=150
        sens_scaled[x] = analogRead(SENSOR[x]) - sens_min[x];
        //500*1000=500000；150*1000=150000；
        sens_scaled[x] *= R_SENS;
        //500000/(500)=1000；150000/500=300
        sens_scaled[x] /= (sens_max[x] - sens_min[x]);
        
      //得到公式：**********************重要
      //归化数值=[（读取数值-最小数值）*范围的最大值]/(范围最大值-范围最小值)
      //得到公式：**********************重要


//Reverse scale to go from R_SENS to 0
//****************************s1=1000,s2=300
//如果颜色是黑色，那么调整下范围，s1=0,s2=1000-300=700
        if(color == BLACK)
        {
            sens_scaled[x] = R_SENS - sens_scaled[x];     
        } 
//At least one sensor has to detect a line
//如果检测归化的数值，大于等于（1000）*（0.5/100）=5，检测到线
        if(sens_scaled[x] >= (float) R_SENS * ((float)P_LINE_MIN / 100.0))
        {  
            line_detected = 1;
        }
//数据的平均值，累加起来，归化的数值，乘以传感器的号码，再乘以1000???
//  0*0*1000=0;      700*1*1000=700000
        avg_num += sens_scaled[x] * x * WEIGHT_UNIT;
        avg_den += sens_scaled[x];        
    }


    if(line_detected == 1){
       //Weighted average  700000/700=1000
        line = avg_num / avg_den;                           
         //Change scale from 0 _ 4000 to -2000 _ 2000
         //改为对称的数据
         //1000-(1000*4/2)=-1000
        line = line - (WEIGHT_UNIT * (N_SENS - 1) / 2);     
        dW(LEDG, LOW);
    }else{
         //Use last direction to calculate error as the maximum value
        line = WEIGHT_UNIT * (N_SENS - 1) * last_dir;       
      //Change scale
        line = line - (WEIGHT_UNIT * (N_SENS - 1) / 2);     
       
        dW(LEDG, HIGH);
    }
    
    return line;
}

//*****************************
//**********************************
float get_PID_correction(float line, float last_line, float kp, float kd, float ki){
  float proportional = line;
  float derivative = line - last_line;
  float integral = line + last_line;
  float correction = (kp * proportional + kd * derivative + ki * integral);

  return correction;
}
//From -1000 to 1000
void motorSpeed(int m1, int m2) {           
  int pwm1 = map(abs(m1), 0, 1000, 0, 255);
  int pwm2 = map(abs(m2), 0, 1000, 0, 255);
  pwm1 = (m1>0) ? 255-pwm1 : pwm1;
  pwm2 = (m2>=0) ? pwm2 : 255-pwm2; 
  analogWrite(MOT2, pwm1);
  analogWrite(MOT4, pwm2);
  digitalWrite(MOT1, (m1 > 0) ? HIGH : LOW);
  digitalWrite(MOT3, (m2 >= 0) ? LOW : HIGH);
}

void InitializeGPIO(){
  pinMode(BTN1, INPUT_PULLUP);
  pinMode(BTN2, INPUT_PULLUP);

  pinMode(LEDG, OUTPUT);

  pinMode(MOT1, OUTPUT);
  pinMode(MOT2, OUTPUT);
  pinMode(MOT3, OUTPUT);
  pinMode(MOT4, OUTPUT);

  for(int x = 0; x <= N_SENS; x++){
    pinMode(SENSOR[x], INPUT);
  }
}

void loop() {
  
  if(dR(BTN1) == PRESS){    //Calibrate sensors
      dW(LEDG, HIGH);
      while(dR(BTN1) == PRESS);
      delay(500);
      
      calibrate(CAL_TIME, CAL_SPEED, RIGHT_DIR);
      calibrate(CAL_TIME, CAL_SPEED, LEFT_DIR);
      calibrate(CAL_TIME, CAL_SPEED, RIGHT_DIR);
      calibrate(CAL_TIME, CAL_SPEED, LEFT_DIR);
  }
  
  if(dR(BTN2) == PRESS){              //Start race
      start = 1;
      while(dR(BTN2) == PRESS){
          dW(LEDG, millis()%100 < 50);
          if(dR(BTN1) == PRESS){      //Cancel race
              start = 0;              
          }
      }
      dW(LEDG, LOW);
      if(start == 1){         //Race not cancelled
          delay(100);         //Wait for hand to get out of the way
          while( dR(BTN1) == RELEASE || dR(BTN2) == RELEASE ){      //Press any button to exit, otherwise race
              race();
              delay(15);
          }
          delay(2000);
      }
  }
}

void race(void){
    last_line_pos = line_pos;
    line_pos = get_line_pos(COLOR, (last_line_pos>0));
        
    float PID_correction = get_PID_correction(line_pos, last_line_pos, KP, KD, KI);
    float max_correction = SPEED;                   
    //Can be changed to a lower value in order to limit the correction, needs to be at most SPEED
    
    if(PID_correction > 0){
        PID_correction = (PID_correction > max_correction)? max_correction : PID_correction;
        motorSpeed(SPEED, SPEED - PID_correction);
    }else{
        PID_correction = (PID_correction < -max_correction)? -max_correction : PID_correction;
        motorSpeed(SPEED + PID_correction, SPEED);
    }    
}

void calibrate(int cal_time, int cal_speed, int cal_dir){
  ms = millis();
  dW(LEDG, LOW);
  while((ms + cal_time) > millis()){
    dW(LEDG, millis()%100 < 50);        //Blink led
    if(cal_dir == RIGHT_DIR)  motorSpeed(cal_speed, -cal_speed);
    if(cal_dir == LEFT_DIR)  motorSpeed(-cal_speed, cal_speed);
    
    int sens_value[N_SENS];
    for(int x = 0; x < N_SENS; x++){
      sens_value[x] = analogRead(SENSOR[x]);
      sens_min[x] = (sens_value[x] < sens_min[x]) ? sens_value[x] : sens_min[x];
      sens_max[x] = (sens_value[x] > sens_max[x]) ? sens_value[x] : sens_max[x];
    }
  }
  motorSpeed(0, 0);
  dW(LEDG, HIGH);
}