Hi everyone,
I'm making a line-follower robot, and my right motor won't run backwards in my code (in a test code I made before assembling the robot, it works fine). I'm using a Pololu TB6612FNG motor driver, a Pololu QTR-8A line-sensor, and a Teensy3.6 micro controller.
The left motor works fine and runs backwards.
Here is the test code (that works):
void setup() {
// put your setup code here, to run once:
pinMode(2, OUTPUT);
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
}
void loop() {
// put your main code here, to run repeatedly:
digitalWrite(2, LOW); //this is one control for the left motor: LOW = backwards, HIGH = forwards
digitalWrite(3, HIGH); //this is one control for the left motor: LOW = forwards, HIGH = backwards
digitalWrite(4, HIGH); //Set low to set motor driver in standby mode
digitalWrite(5, LOW); //this is one control for the right motor: LOW = backwards, HIGH = forwards
digitalWrite(6, HIGH); //this is one control for the right motor: LOW = forwards, HIGH = backwards
analogWrite(35, 65); // set motor speed for left motor
analogWrite(36, 65); // set motor speed for right motor
delay(1000);
}
And this is the code for my line-follower (This is the part of code where the problem should be. The whole code is in attachement (it's in different tabs)):
#define motorSTBY 4
#define lMotorControl1 2
#define lMotorControl2 3
#define rMotorControl1 5
#define rMotorControl2 6
#define lMotor 35
#define rMotor 36
void setup() {
pinMode(lMotorControl1, OUTPUT);
pinMode(lMotorControl2, OUTPUT);
pinMode(motorSTBY, OUTPUT);
pinMode(rMotorControl1, OUTPUT);
pinMode(lMotorControl2, OUTPUT);
}
void loop() {
// put your main code here, to run repeatedly:
Display();
TCS34725_1();
TCS34725_2();
Calibration();
baseSpeed = ((MaxMspeed - MinMspeed) / 2) + MinMspeed;
for (i=0; i<=7; i++) {
if (analogRead(lineSensor[i]) >= calibrationValue[i]) {
sensorValue[i] = 1000;
} else {
sensorValue[i] = 0;
}
}
// calculating the position of the line (a number between 0 and 70, where 35 is the middle):
pos = (0 * sensorValue[0] + 10 * sensorValue[1] + 20 * sensorValue[2] + 30 * sensorValue[3] + 40 * sensorValue[4] + 50 * sensorValue[5] + 60 * sensorValue[6] + 70 * sensorValue[7]) / (sensorValue[0] + sensorValue[1] + sensorValue[2] + sensorValue[3] + sensorValue[4] + sensorValue[5] + sensorValue[6] + sensorValue[7]);
// keep outputting 70 when the robot lose the line
if (pos == 0 && error > 0) {
pos = 70;
}
error = pos - 35;
correction = Kp * error + Kd * (error - lastError);
MspeedL = baseSpeed - correction;
MspeedR = baseSpeed + correction;
if (MspeedL > MaxMspeed) {
MspeedL = MaxMspeed;
}
if (MspeedL < MinMspeed) {
MspeedL -= 2 * MinMspeed;
digitalWrite(lMotorControl1, LOW);
digitalWrite(lMotorControl2, HIGH);
} else {
digitalWrite(lMotorControl1, HIGH);
digitalWrite(lMotorControl2, LOW);
}
if (MspeedR > MaxMspeed) {
MspeedR = MaxMspeed;
}
if (MspeedR < MinMspeed) {
MspeedR -= 2 * MinMspeed;
Serial.println(MspeedR);
Serial.println(abs(MspeedR));
digitalWrite(rMotorControl1, LOW);
digitalWrite(rMotorControl2, HIGH);
} else {
digitalWrite(rMotorControl1, HIGH);
digitalWrite(rMotorControl2, LOW);
}
analogWrite(lMotor, abs(MspeedL));
analogWrite(rMotor, abs(MspeedR));
if (b_start_stop == 1) {
digitalWrite(motorSTBY, HIGH);
// Serial.println("Motors running");
} else {
digitalWrite(motorSTBY, LOW);
// Serial.println("Motors off");
}
}
Any help would be appreciated.
Thanks in advance.
a_GLOBAL.ino (4.44 KB)
b_SETUP.ino (1.89 KB)
c_LOOP.ino (2.5 KB)
d_NEXTION.ino (7.22 KB)
e_CALIBRATION.ino (2.84 KB)
f_TCS34725.ino (4.92 KB)