122 řádky
2.7 KiB
C++
122 řádky
2.7 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.07; //set up the constants value
|
|
float Ki = 0.0008;
|
|
float Kd = 0.6;
|
|
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 = 9;
|
|
int aenbl = 6;
|
|
int bphase = 5;
|
|
int benbl = 3;
|
|
|
|
//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
|
|
|
|
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();
|
|
}
|
|
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);
|
|
}
|