Im having problems with the intersections and the broken lines of the track were going to compete at,
I dont think PID control can recognize them, maybe im wrong. If anyone knows a solution or atleast someone who can point me in the right direction, please do.
Code:
#include <QTRSensors.h>
#define Kp 1.50
#define Kd 20
#define rightMaxSpeed 200 // max speed of the robot
#define leftMaxSpeed 200 // max speed of the robot
#define rightBaseSpeed 200
#define leftBaseSpeed 200
#define NUM_SENSORS 8
#define TIMEOUT 2500
#define EMITTER_PIN QTR_NO_EMITTER_PIN
#define rightMotor1 8
#define rightMotor2 11
#define rightMotorPWM 9
#define leftMotor1 12
#define leftMotor2 13
#define leftMotorPWM 10
QTRSensorsRC qtrrc((unsigned char[]) {
A8, A9, A10, A11, A12, A13, A14, A15
} , 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);
int i;
for (int i = 0; i < 100; i++)
{
/*
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, 170);
*/
qtrrc.calibrate();
delay(25);
}
wait();
delay(2000); // wait for 2s to position the bot before entering the main loop
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();
}
int lastError = 0;
void loop()
{
unsigned int sensors[8];
int intersectCounter=0;
int position = qtrrc.readLine(sensors);
int error = position-2500;
int motorSpeed = Kp * error + Kd * (error - lastError);
lastError = error;
int rightMotorSpeed = rightBaseSpeed + motorSpeed;
int leftMotorSpeed = leftBaseSpeed - motorSpeed;
if (rightMotorSpeed > rightMaxSpeed ) rightMotorSpeed = rightMaxSpeed; // prevent the motor from going beyond max speed
if (leftMotorSpeed > leftMaxSpeed ) leftMotorSpeed = leftMaxSpeed; // prevent the motor from going beyond max speed
if (rightMotorSpeed< 0) rightMotorSpeed = 0; // keep the motor speed positive
if (leftMotorSpeed < 0) leftMotorSpeed = 0; // keep the motor speed positive
{
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, rightMotorSpeed);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, leftMotorSpeed); // move forward with appropriate speeds
}
}
void wait() {
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
}
void moveForward(){
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
analogWrite(rightMotorPWM, 200);
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
analogWrite(leftMotorPWM, 200);
}