Question

#include <QTRSensors.h>
#define NUM_SENSORS             5  //Number of sensors used
#define NUM_SAMPLES_PER_SENSOR  4  //Average 4 analog samples per sensor reading
#define EMITTER_PIN             13  //Emitter is controlled by digital pin 2

QTRSensorsAnalog qtra((unsigned char[]) {0,1,2,3,4}, 
  NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS]; //Variable Array for sensor values
const double kp = .505; //Variable for adjusting KP Value
const double kd = 150; //Variable for adjusting KD Value
const int max_speed = 255; //Variable for the Maximum Speed of the Motors
unsigned int position = 0; //Variable for holding the calculated position
int set_point = 1500;//Variable for holding the value of the center of the line
int error = 0; //Variable for holding the error from center of line based on position
int last_error = 0; //Variable for holding the previous error
int white = 0; //Variable for reading the line over white or black
int max_difference = 80 ;
double spd_right; //Speed for the Right Motor
double spd_left; //Speed for the Left Motor
int derivative = 0; //Value for the derivative
int error_value = 0; //Value for the error_value calculated from the pd function
int key_s6 = 2;//Declaring Digital Push Button
int E1 = 5; //M1 Speed Control
int E2 = 6; //M2 Speed Control
int M1 = 4; //M1 Direction Control
int M2 = 7; //M2 Direction Control
int sensors_sum = 0;

//Setup Method that includes calibration
void setup(){
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(13, OUTPUT);
  calibration();
  set_Point();
  }//Ends Setup


void loop (){
  get_sum();
  white_or_black();
  pid_calc();
  adjust_control();
  set_motors();
}//Ends Loop*/

/*Method for calibrating set_point on individual tracks
*/
void set_Point(){
  qtra.readCalibrated(sensorValues);
  for (int i = 0; i <= 5; i++){
  set_point = sensorValues[i] + set_point;
  }//Ends For
}//Ends set_point Method

/*Method for Calibrating the sensors to the ambiance lighting and 
* varying contrasts of each individual track.
*/
void calibration (){
  for (int i = 0; i<250; i++){
    qtra.calibrate();
    delay(20);
  }//Ends For
}//Ends Calibration Method

/*Method for calculating the sum of all the sensors to be used in the 
* white_or_black method for determining the background color of the track
*/
void get_sum(){
  qtra.readCalibrated(sensorValues);
  for (int i = 0; i <= 5; i++){
  sensors_sum = sensorValues[i] + sensors_sum;
  }//Ends For
}//Ends get_sum Method

/*Method for determining the background color of the current portion of the 
* track based off of the value returned by get_sum and adjusting a variable 
* to follow a white of black line as necessary.
*/
void white_or_black(){
  if (sensors_sum > 2000){
    white = 1;
  }//Ends If
  else{
    white = 0;
  }//Ends Else
}//Ends white_or_black Method

/*Method for following the line based of the position, proportional, 
* and derivative
*/
void pid_calc(){
  position = qtra.readLine(sensorValues, QTR_EMITTERS_ON, white);
  error = position - set_point;
  derivative = error-last_error;
  last_error = error;
  error_value = int(error*kp+derivative*kd);
}//Ends pid_calc Method

void adjust_control(){
  if (error_value>max_difference){
    error_value = max_difference;
  }//Ends If
  else if(error_value < max_difference){
    error_value = -max_difference;
  }//Ends If
  if (error_value < 0 ){
    spd_right = max_speed + error_value;
    spd_left = max_speed;
  }//Ends If
  else{
    spd_right = max_speed;
    spd_left = max_speed - error_value;
  }//Ends Else
}//Ends adjust_control Method


void set_motors(){
  analogWrite(M1, HIGH);
  analogWrite(M2, HIGH);
  analogWrite(E1, constrain(spd_right, 0, max_speed));
  analogWrite(E2, constrain(spd_left, 0, max_speed));
}//Ends set_motors Method

void stop(){
  analogWrite(E1, 0);
  analogWrite(E2, 0);
}//Ends Stop Method

我是arduino语言的新手。这是我的第一个程序，我昨晚写了它。这是PD Line以下示例。我目前在调整电机时遇到问题。简单地说就是直截了当。我正在使用QTR模拟传感器阵列。我可能会切换到数字，因为我的罗密欧板上的一个模拟输入不起作用。有人对我的逻辑有任何建议或改进吗？

Answer 1

void set_Point(){
  qtra.readCalibrated(sensorValues);
  for (int i = 0; i < NUM_SENSORS; i++){
      set_point += sensorValues[i];
  }
}

关于这个循环的一个奇怪的事情是set_point在添加之前没有初始化（为零），并且在求和之后没有被分割（通过i == NUM_SENSORS- = 5）。

Arduino - PD - 线路跟随

1 个答案: