Please help me troubleshoot my code. My line following robot didnt work, even though it normally work when i used normal line follower code .
Im using two drv8833 motor driver for controling 4 3v dc motor and 5 tcrt5000 ir sensor
Here's my code
#define NUM_SENSORS 5
#define TIME_INTERVAL 10
#define LF1_Motor 8
#define LF2_Motor 7
#define LB1_Motor 6
#define LB2_Motor 5
#define RF1_Motor 12
#define RF2_Motor 11
#define RB1_Motor 10
#define RB2_Motor 9
int s[6], total, sensor_position;
int threshold = 600;
float avg;
int position[6] = {1, 2, 3, 4, 5};
int set_point = 3;
void setup() {
pinMode(LF1_Motor, OUTPUT);
pinMode(LF2_Motor, OUTPUT);
pinMode(LB1_Motor, OUTPUT);
pinMode(LB2_Motor, OUTPUT);
pinMode(RF1_Motor, OUTPUT);
pinMode(RF2_Motor, OUTPUT);
pinMode(RB1_Motor, OUTPUT);
pinMode(RB2_Motor, OUTPUT);
Stop();
Serial.begin(9600);
delay(2000);
}
void loop() {
//displayValue();
PID_LINE_FOLLOW();
}
void Sensor_reading(){
sensor_position = 0;
total = 0;
for(byte i = 0; i < 5; i++){//sensor data read from A1, A2, A3, A6, A7
if (i > 2){
s[i] = analogRead(i + 3);
}else{
s[i] = analogRead(i);
}
if (s[i] > threshold) s[i] = 1; //analog to digital conversion
else s[i] = 0;
sensor_position += s[i] * position[i];
total += s[i];
}
if (total) avg = sensor_position / total; //average value
}
void PID_LINE_FOLLOW(){
int kp = 50, kd = 500, PID_Value, P, D;
float error, previous_error;
int baseSpeed = 200, leftSpeed, rightSpeed, turnSpeed = 100;
char t;
while (1){
Sensor_reading();
error = set_point - avg;
D = kd * (error - previous_error);
P = error * kp;
PID_Value = P + D;
previous_error = error;
//adjusting motor speed
rightSpeed = baseSpeed - PID_Value;
leftSpeed = baseSpeed - PID_Value;
motor(leftSpeed, rightSpeed);
Sensor_reading();
if (total == 0){
if (t != 's'){
if (t =='r') motor(turnSpeed, -turnSpeed);
else motor(-turnSpeed, turnSpeed);
while (!s[2]) Sensor_reading();
}
}
if (s[0] && !s[4]) t = 'l';
if (s[4] && !s[0]) t = 'r';
else if (total == 5){
Sensor_reading();
if (total == 5){
motor(0, 0);
while (total == 5) Sensor_reading();
} else if (total == 0) t = 's';
}
}
}
void displayValue(){
for(byte i = 0; i < 5; i++){//sensor data read from A1, A2, A3, A6, A7
if (i > 2){
s[i] = analogRead(i + 3);
}else{
s[i] = analogRead(i);
}
Serial.print(String(s[i]) + " ");
}
Serial.println();
delay(50);
}
void motor(int a, int b){
if (a > 0){
digitalWrite(RF1_Motor, LOW);
digitalWrite(RF2_Motor, HIGH);
digitalWrite(RB1_Motor, LOW);
digitalWrite(RB2_Motor, HIGH);
}else{
a = -(a);
digitalWrite(RF1_Motor, HIGH);
digitalWrite(RF2_Motor, LOW);
digitalWrite(RB1_Motor, HIGH);
digitalWrite(RB2_Motor, LOW);
}
if (b > 0){
digitalWrite(LF1_Motor, LOW);
digitalWrite(LF2_Motor, HIGH);
digitalWrite(LB1_Motor, LOW);
digitalWrite(LB2_Motor, HIGH);
}else{
b = -(b);
digitalWrite(LF1_Motor, HIGH);
digitalWrite(LF2_Motor, LOW);
digitalWrite(LB1_Motor, HIGH);
digitalWrite(LB2_Motor, LOW);
}
if (a > 200) a = 200;
if (b > 200) b = 200;
analogWrite(RF1_Motor, a);
analogWrite(RF2_Motor, a);
analogWrite(LF1_Motor, b);
analogWrite(LF2_Motor, b);
analogWrite(RB1_Motor, a);
analogWrite(RB2_Motor, a);
analogWrite(LB1_Motor, b);
analogWrite(LB2_Motor, b);
}
void Stop(){
digitalWrite(RF1_Motor, LOW);
digitalWrite(RF2_Motor, LOW);
digitalWrite(RB1_Motor, LOW);
digitalWrite(RB2_Motor, LOW);
digitalWrite(LF1_Motor, LOW);
digitalWrite(LF2_Motor, LOW);
digitalWrite(LB1_Motor, LOW);
digitalWrite(LB2_Motor, LOW);
}