system
12
const int motor3Pin = 3;
const int motor1Pin = 10;
const int motor2Pin = 6;
const int motor4Pin = 11;
int right_speed = 0;
int left_speed = 0;
long sensors[] = {0,0,0,0,0};
long sensors_average = 0;
int sensors_sum = 0;
int posn; //position
int proportional = 0;
long integral = 0;
int derivative = 0;
int last_proportional = 0;
float Kp = 2; // Havent decided the values yet
float Ki = 2;
float Kd = 2;
int error_value = 0;
int max_speed = 200;
int set_point = 1987;//the posn on the center of line, I have to decide the value yet
void setup(){
pinMode(motor1Pin, OUTPUT);
pinMode(motor2Pin, OUTPUT);
pinMode(motor3Pin, OUTPUT);
pinMode(motor4Pin, OUTPUT);
Serial.begin(9600);
}
void read_sensors () {
for (int i = 0; i < 5; i++) {
sensors[i] = analogRead(i);
sensors_average += sensors[i] * i * 1000;
sensors_sum += int(sensors[i]);
}
}
void pid_calc () {
posn = int(sensors_average / sensors_sum);
proportional = posn - set_point;
integral = integral + proportional;
derivative = proportional - last_proportional;
last_proportional = proportional;
error_value = int(proportional * Kp + integral * Ki + derivative * Kd);
}
void calc_turn(){
if (error_value < -256) {
error_value = -256;
}
if (error_value > 256) {
error_value = 256;
}
if (error_value < 0) {
right_speed = max_speed + error_value;
left_speed = max_speed;
}
else {
right_speed = max_speed;
left_speed = max_speed + error_value;
}
}
void motor_drive (int right_speed, int left_speed) {
analogWrite(motor1Pin, right_speed);
analogWrite(motor2Pin, left_speed);
delay(20);
}
void loop () {
read_sensors ();
pid_calc ();
calc_turn ();
motor_drive (right_speed, left_speed);
Serial.print(error_value);
Serial.print(" ");
Serial.print(right_speed);
Serial.print(" ");
Serial.print(left_speed);
Serial.println(" ");
}