|
- //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);
- }
|