Line_Following-2022/rally-test.ino

137 lines
3.1 KiB
C++

//Make sure to install the library
#include <QTRSensors.h>
QTRSensors qtr;
const uint8_t SensorCount = 8;
uint16_t sensorValues[SensorCount];
float Kp = 0.09;//set up the constants value
float Ki = 0.0008;
float Kd = 0.34;
int P;
int I;
int D;
int lastError = 0;
boolean onoff = false;
//Increasing the maxspeed can damage the motors - at a value of 255 the 6V motors will receive 7,4 V
const uint8_t maxspeeda = 150;
const uint8_t maxspeedb = 150;
const uint8_t basespeeda = 100;
const uint8_t basespeedb = 100;
//Set up the drive motor carrier pins
int mode = 8;
int aphase = 5;
int aenbl = 3;
int bphase = 9;
int benbl = 6;
//Set up the buttons pins
int buttoncalibrate = 17; //pin A3
int buttonstart = 2;
void setup() {
Serial.begin(9600);
qtr.setTypeRC();
//Set up the sensor array pins
qtr.setSensorPins((const uint8_t[]){10, 11, 12, 14, 15, 16, 18, 19}, SensorCount);
qtr.setEmitterPin(7);//LEDON PIN
int timepassed = 0;
pinMode(mode, OUTPUT);
pinMode(aphase, OUTPUT);
pinMode(aenbl, OUTPUT);
pinMode(bphase, OUTPUT);
pinMode(benbl, OUTPUT);
digitalWrite(mode, HIGH);
delay(500);
pinMode(LED_BUILTIN, OUTPUT);
boolean Ok = false;
while (Ok == false) { //the loop won't start until the robot is calibrated
if(digitalRead(buttoncalibrate) == HIGH) {
calibration(); //calibrate the robot for 10 seconds
Ok = true;
}
}
forward_brake(0, 0);
}
void calibration() {
digitalWrite(LED_BUILTIN, HIGH);
for (uint16_t i = 0; i < 400; i++)
{
qtr.calibrate();
}
digitalWrite(LED_BUILTIN, LOW);
}
void loop() {
if(digitalRead(buttonstart) == HIGH) {
onoff =! onoff;
if(onoff = true) {
delay(1000);//a delay when the robot starts
}
else {
delay(50);
}
}
if (onoff == true) {
PID_control();
}
if ((sensors[0] > 750) && (sensors[1] > 750) && (sensors[2] > 750) && (sensors[3] > 750) && (sensors[4] > 750) && (sensors[5] > 750) && (sensors[6] > 750) && (sensors[7] > 750) && (sensors[8] > 750)) {
if (timepassed == 1) {
motorspeeda = 0;
motorspeedb = 0;
forward_brake(motorspeeda, motorspeedb);
}
else {
timepassed += 1;
}
}
else {
forward_brake(0,0);
}
}
void forward_brake(int posa, int posb) {
//set the appropriate values for aphase and bphase so that the robot goes straight
digitalWrite(aphase, LOW);
digitalWrite(bphase, HIGH);
analogWrite(aenbl, posa);
analogWrite(benbl, posb);
}
void PID_control() {
uint16_t position = qtr.readLineBlack(sensorValues);
int error = 3500 - position;
P = error;
I = I + error;
D = error - lastError;
lastError = error;
int motorspeed = P*Kp + I*Ki + D*Kd;
int motorspeeda = basespeeda + motorspeed;
int motorspeedb = basespeedb - motorspeed;
if (motorspeeda > maxspeeda) {
motorspeeda = maxspeeda;
}
if (motorspeedb > maxspeedb) {
motorspeedb = maxspeedb;
}
if (motorspeeda < 0) {
motorspeeda = 0;
}
if (motorspeedb < 0) {
motorspeedb = 0;
}
//Serial.print(motorspeeda);Serial.print(" ");Serial.println(motorspeedb);
forward_brake(motorspeeda, motorspeedb);
}