Hello there,
I am using a Arduino ATMEGA168 clone along with the 30:1 MP micrometal gear motors, the pololu tb6612fng motor driver, and of course, the Pololu QTR-8RC sensor. I have got the robot up and running, motor control flawless, and now i am shifting towards PID based line following. I have built some line followers, but they were not PID based, nor had i made use of the QTR sensors. I will post my code below, which, i don’t think is correct to some extent.
I went through the wonderful PDF explanation of using the QTR sensors, but am encountering problems on my way.
The line width for a competition i am taking part is 2.5 cm, therefore, i am using sensor 4 as the middle sensor, and sensors 1 and 2 as right sensors and 6 and 7 as left sensors.
Now the problem is, i have set my base motor speeds at a PWM value of 10. I serial print the final motor speeds ( m1speed and m2speed as mentioned in the PDF), they are sometimes at 5000, sometimes at -270, its all random. Surely, it has to do something with the code. Thus, the bot is completely random.
#include <QTRSensors.h>
#define KP .2
#define KD 5
#define M1_DEFAULT_SPEED 10
#define M2_DEFAULT_SPEED 10
#define MIDDLE_SENSOR 4
#define NUM_SENSORS 5 // number of sensors used
#define TIMEOUT 2500 // waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN 2 // emitter is controlled by digital pin 2
#define DEBUG 1 // set to 1 if serial debug output needed
#define rightMotor1 3
#define rightMotor2 4
#define rightMotorPWM 5
#define leftMotor1 12
#define leftMotor2 13
#define leftMotorPWM 11
#define motorPower 8
QTRSensorsRC qtrrc((unsigned char[]) { 14, 15, 16, 17, 18} ,NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
void setup()
{
pinMode(rightMotor1, OUTPUT);
pinMode(rightMotor2, OUTPUT);
pinMode(rightMotorPWM, OUTPUT);
pinMode(leftMotor1, OUTPUT);
pinMode(leftMotor2, OUTPUT);
pinMode(leftMotorPWM, OUTPUT);
pinMode(motorPower, OUTPUT);
delay(1000);
manual_calibration();
}
int lastError = 0;
int last_proportional = 0;
int integral = 0;
void loop()
{
unsigned int sensors[5];
int position = qtrrc.readLine(sensors);
int error = position - 2000;
Serial.print("position");
Serial.println(position);
Serial.print("error");
Serial.println(error);
int motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
int rightMotorSpeed = M2_DEFAULT_SPEED - motorSpeed;
int leftMotorSpeed = M1_DEFAULT_SPEED + motorSpeed;
Serial.print("rightMotorSpeed");
Serial.println(rightMotorSpeed);
Serial.print("leftmotorspeed");
Serial.println(leftMotorSpeed);
delay(1000);
{
digitalWrite(motorPower, HIGH); // move forward with appropriate speeds
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(motorPower, HIGH);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed);
}
}
void manual_calibration() {
int i;
for (i = 0; i < 250; i++) // the calibration will take a few seconds
{
qtrrc.calibrate(QTR_EMITTERS_ON);
delay(20);
}
if (DEBUG) { // if true, generate sensor dats via serial output
Serial.begin(9600);
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
}
}
One thing is doubt on this code is the move forward function inside the main loop? is that fine? is that the task of the bot to move forward with appropriate speeds, or please suggest me a better technique.
Also, please suggest me a sample code for automatic calibration which the 3pi does, first a left turn and a right turn, and zooooom. I understand that you make the bot turn right and left in calibration mode, but i am a bit confused, where to include this turning function? I don’t think my rough thought on this will work.
void setup() {
for (int i = 0; i < 400; i++) // make the calibration take about 10 seconds
{
qtrrc.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
turn left(); // 90 degree left turn
delay(500);
turn right(); // 180 degree right turn
delay(1000);
turn left(); 90 degree left turn to final position
delay(500);
}
stop(); // end of calibration
}
I know, i got it wrong though
And below is a serial print i get when the sensor is exactly above the black line, along with the radom motor speeds, and line positions and errors.
488 356 260 260 292
2500 2500 2500 2500 2500
position2247
error247
rmotightorspeed-1274
leftmotorspeed1294
position2186
error186
rmotightorspeed277
leftmotorspeed-257
position2274
error274
rmotightorspeed-484
leftmotorspeed504
position3788
error1788
rmotightorspeed-7917
leftmotorspeed7937
position4000
error2000
rmotightorspeed-1450
leftmotorspeed1470
position1682
error-318
rmotightorspeed11663
leftmotorspeed-11643
position2000
error0
rmotightorspeed-1580
leftmotorspeed1600
position2000
error0
rmotightorspeed10
leftmotorspeed10
as you can see, the line position is completely out of track, once the bot loses the track due to fast speed. Surprisingly, at the last serial reading, the error is 0, and motor speeds are at 10!! i didn’t notice it while experimenting, i was busy catching my mad line follower.
Is there a problem in the code? Or, am i fine to go and should i start tweaking the P and D values?
Last thing, i am using a 6v Ni-MH and boosted it to 9V using the pololu 9V boost regulator, it gets too hot, is it normal, it gets really hot? Just connected the Vin and gnd pins to +ve and -ve terminals of the battery, it gets really angry.
Thankyou for your time, and thankyou for such wonderful products.
Regards
Ashim