Add rally-test

2024-06-30 11:09:57 +03:00 · 2024-06-30 11:09:57 +03:00 · 33a33bfcac
parent 56f25b746b
commit 33a33bfcac
1 changed files with 136 additions and 0 deletions
--- a/136
+++ b/136
@ -0,0 +1,136 @@
+//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);
+}