diff --git a/pid-example/pid-example.ino b/pid-example/pid-example.ino index 09a2d2e..d69957a 100644 --- a/pid-example/pid-example.ino +++ b/pid-example/pid-example.ino @@ -20,6 +20,10 @@ const uint8_t maxspeedb = 150; const uint8_t basespeeda = 100; const uint8_t basespeedb = 100; +// adding minspeed so it can reverse +const int minspeeda = -150; +const int minspeedb = -150; + //Set up the drive motor carrier pins int mode = 8; int aphase = 5; @@ -110,11 +114,11 @@ void PID_control() { if (motorspeedb > maxspeedb) { motorspeedb = maxspeedb; } - if (motorspeeda < 0) { - motorspeeda = 0; + if (motorspeeda < minspeeda) { + motorspeeda = minspeeda; } - if (motorspeedb < 0) { - motorspeedb = 0; + if (motorspeedb < minspeedb) { + motorspeedb = minspeedb; } //Serial.print(motorspeeda);Serial.print(" ");Serial.println(motorspeedb); forward_brake(motorspeeda, motorspeedb); diff --git a/rally-test.ino b/rally-test.ino deleted file mode 100644 index 43acd69..0000000 --- a/rally-test.ino +++ /dev/null @@ -1,136 +0,0 @@ -//Make sure to install the library -#include -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); -}