//Make sure to install the library #include QTRSensors qtr; const uint8_t SensorCount = 8; uint16_t sensorValues[SensorCount]; float Kp = 0.07; //set up the constants value float Ki = 0.001; 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 = 6; int aenbl = 9; 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(); Serial.print(">>>>>PID-control"); } else { forward_brake(0, 0); Serial.print(">>>>>forward-brake"); } } void forward_brake(int posa, int posb) { //set the appropriate values for aphase and bphase so that the robot goes straight digitalWrite(aphase, HIGH); 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); Serial.print(position); forward_brake(motorspeeda, motorspeedb); }