So this is my first attempt at building a bot, and have it functioning but am trying to get it to run better. As I have a small competition running a figure 8 track against another bot. Currently using the QTR-3RC ran through an arduino to a pair of DRV8838 Single Brushed DC Motor Driver Carriers which control the 30:1 micro motors.
Here is code as is stands, any suggestions or advice as to what i could change to better optimize this bot?
// number of sensors used
#define NUM_SENSORS 3
// waits for 2500 us for sensor outputs to go low
//#define TIMEOUT 2500
// emitter is controlled by digital pin 2
//#define EMITTER_PIN 2
// motors pins
#define leftMotor1 7
#define leftMotor2 5
#define leftMotorPWM 6
#define rightMotor1 4
#define rightMotor2 2
#define rightMotorPWM 3
int lastError = 0;
// sensor connected through digital pins 9-11
QTRSensorsRC qtrrc((unsigned char[]) { 9, 10, 11}, NUM_SENSORS);
unsigned int sensorValues[NUM_SENSORS];
void setup() {
Serial.begin(9600);
pinMode(rightMotor1, OUTPUT);
pinMode(rightMotor2, OUTPUT);
pinMode(rightMotorPWM, OUTPUT);
pinMode(leftMotor1, OUTPUT);
pinMode(leftMotor2, OUTPUT);
pinMode(leftMotorPWM, OUTPUT);
// manualCalibration();
autoCalibration();
// wait for 2s to position the bot before entering the main loop
delay(2000);
if (DEBUG) {
debugCalibration();
}
}
void loop() {
int pid = calculatePid();
// calculate the new right motor speed
int rightMotorSpeed = rightBaseSpeed + pid;
// calculate the new left motor speed
int leftMotorSpeed = leftBaseSpeed - pid;
// prevent the motor from going beyond max speed
if (rightMotorSpeed > rightMaxSpeed ) {
rightMotorSpeed = rightMaxSpeed;
}
// prevent the motor from going beyond max speed
if (leftMotorSpeed > leftMaxSpeed ) {
leftMotorSpeed = leftMaxSpeed;
}
// keep the motor speed positive
if (rightMotorSpeed < 0) {
rightMotorSpeed = 0;
}
// keep the motor speed positive
if (leftMotorSpeed < 0) {
leftMotorSpeed = 0;
}
// adjust left motor speed
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, HIGH);
analogWrite(leftMotorPWM, leftMotorSpeed);
// adjust right motor speed
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, HIGH);
analogWrite(rightMotorPWM, rightMotorSpeed);
}
// calibrate for sometime by sliding the sensors across the line
void manualCalibration() {
for (int i = 0; i < 100; i++) {
qtrrc.calibrate();
delay(20);
}
}
void autoCalibration() {
for (int i = 0; i < 40; i++) {
// turn to the left and right to expose the sensors to the brightest and darkest readings that may be encountered
if (i < 10 || i >= 30) {
turnRight();
}
else {
turnLeft();
}
qtrrc.calibrate();
delay(20);
}
stopMotors();
}
void debugCalibration() {
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();
}
void turnLeft() {
// move left motor slow
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, HIGH);
analogWrite(leftMotorPWM, 15);
// right motor forward
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, HIGH);
analogWrite(rightMotorPWM, 50);
}
void turnRight() {
// move left motor forward
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, HIGH);
analogWrite(leftMotorPWM, 50);
// right motor slow
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, HIGH);
analogWrite(rightMotorPWM, 15);
}
void stopMotors() {
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);
}
int calculatePid() {
// get calibrated readings along with the line position
// refer to the QTR Sensors Arduino Library for more details on line position.
int position = qtrrc.readLine(sensorValues);
int error = position - 1000;
int pid = Kp * error + Kd * (error - lastError);
lastError = error;
return pid;
}
Thanks,
Doug