From 11d448226dfa69e40076980eda49049265d4a662 Mon Sep 17 00:00:00 2001
From: giorgos <giorgos@eowyn.lan>
Date: Fri, 3 Jun 2022 18:01:44 +0300
Subject: [PATCH] Changed main.ino to test.ino, added values to the PID
 constants.

---
 main.ino                                      |  40 ------
 .../pis-example.ino                           |  37 +++---
 qtr-test.ino => qtr-test/qtr-test.ino         |   0
 test/test.ino                                 | 115 ++++++++++++++++++
 4 files changed, 131 insertions(+), 61 deletions(-)
 delete mode 100644 main.ino
 rename pis-example.ino => pis-example/pis-example.ino (79%)
 rename qtr-test.ino => qtr-test/qtr-test.ino (100%)
 create mode 100644 test/test.ino

diff --git a/main.ino b/main.ino
deleted file mode 100644
index ccd8c46..0000000
--- a/main.ino
+++ /dev/null
@@ -1,40 +0,0 @@
-int MODE = 8;
-
-int left_motor_in = 6;
-int left_motor_out = 9;
-
-int right_motor_in = 3;
-int right_motor_out = 5;
-
-void setup() {
-  Serial.begin(9600);
-  Serial.println("Start");
-  init_motor();
-}
-
-void loop() {
-  left_motor_go(250);
-  right_motor_go(250);
-}
-  
-void init_motor(){
-  pinMode(left_motor_in, OUTPUT);
-  pinMode(left_motor_out, OUTPUT);
-  
-  pinMode(right_motor_in, OUTPUT);
-  pinMode(right_motor_out, OUTPUT);
-
-  pinMode(MODE, OUTPUT);
-  digitalWrite(MODE, HIGH);
-  }
-
-//range:0<value<255
-void left_motor_go(int motor_speed){
-  analogWrite(left_motor_in, motor_speed);
-  digitalWrite(left_motor_out, LOW);
- }
-//range:0<value<255
-void right_motor_go(int motor_speed){
-  analogWrite(right_motor_in, motor_speed);
-  digitalWrite(right_motor_out, LOW);
-}
diff --git a/pis-example.ino b/pis-example/pis-example.ino
similarity index 79%
rename from pis-example.ino
rename to pis-example/pis-example.ino
index daf97e6..b2547cd 100644
--- a/pis-example.ino
+++ b/pis-example/pis-example.ino
@@ -5,7 +5,7 @@ const uint8_t SensorCount = 8;
 uint16_t sensorValues[SensorCount];
 
 float Kp = 0.07; //set up the constants value
-float Ki = 0.001;
+float Ki = 0.0008;
 float Kd = 0.6;
 int P;
 int I;
@@ -14,7 +14,7 @@ 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
+//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;
@@ -22,8 +22,8 @@ const uint8_t basespeedb = 100;
 
 //Set up the drive motor carrier pins
 int mode = 8;
-int aphase = 6;
-int aenbl = 9;
+int aphase = 9;
+int aenbl = 6;
 int bphase = 5;
 int benbl = 3;
 
@@ -35,9 +35,7 @@ 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.setSensorPins((const uint8_t[]){10, 11, 12, 14, 15, 16, 18, 19}, SensorCount);
   qtr.setEmitterPin(7);//LEDON PIN
 
   pinMode(mode, OUTPUT);
@@ -52,7 +50,7 @@ void setup() {
 
   boolean Ok = false;
   while (Ok == false) { //the loop won't start until the robot is calibrated
-    if (digitalRead(buttoncalibrate) == HIGH) {
+    if(digitalRead(buttoncalibrate) == HIGH) {
       calibration(); //calibrate the robot for 10 seconds
       Ok = true;
     }
@@ -70,9 +68,9 @@ void calibration() {
 }
 
 void loop() {
-  if (digitalRead(buttonstart) == HIGH) {
-    onoff = ! onoff;
-    if (onoff = true) {
+  if(digitalRead(buttonstart) == HIGH) {
+    onoff =! onoff;
+    if(onoff = true) {
       delay(1000);//a delay when the robot starts
     }
     else {
@@ -81,16 +79,14 @@ void loop() {
   }
   if (onoff == true) {
     PID_control();
-    Serial.print(">>>>>PID-control");
   }
   else {
-    forward_brake(0, 0);
-    Serial.print(">>>>>forward-brake");
+    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, HIGH);
+  digitalWrite(aphase, LOW);
   digitalWrite(bphase, HIGH);
   analogWrite(aenbl, posa);
   analogWrite(benbl, posb);
@@ -103,11 +99,11 @@ void PID_control() {
   I = I + error;
   D = error - lastError;
   lastError = error;
-  int motorspeed = P * Kp + I * Ki + D * Kd;
-
+  int motorspeed = P*Kp + I*Ki + D*Kd;
+  
   int motorspeeda = basespeeda + motorspeed;
   int motorspeedb = basespeedb - motorspeed;
-
+  
   if (motorspeeda > maxspeeda) {
     motorspeeda = maxspeeda;
   }
@@ -119,8 +115,7 @@ void PID_control() {
   }
   if (motorspeedb < 0) {
     motorspeedb = 0;
-  }
-  Serial.print(motorspeeda);Serial.print(" ");Serial.println(motorspeedb);
-  Serial.print(position);
+  } 
+  //Serial.print(motorspeeda);Serial.print(" ");Serial.println(motorspeedb);
   forward_brake(motorspeeda, motorspeedb);
 }
diff --git a/qtr-test.ino b/qtr-test/qtr-test.ino
similarity index 100%
rename from qtr-test.ino
rename to qtr-test/qtr-test.ino
diff --git a/test/test.ino b/test/test.ino
new file mode 100644
index 0000000..8bdfcd3
--- /dev/null
+++ b/test/test.ino
@@ -0,0 +1,115 @@
+
+#include <QTRSensors.h>
+
+QTRSensors qtr;
+
+const uint8_t SensorCount = 8;
+uint16_t sensorValues[SensorCount];
+
+
+int MODE = 8;
+
+int left_motor_in = 6;
+int left_motor_out = 9;
+
+int right_motor_in = 3;
+int right_motor_out = 5;
+
+void setup() {
+  Serial.begin(9600);
+  Serial.println("Start");
+  init_motor();
+  
+  qtr.setTypeRC();
+  qtr.setSensorPins((const uint8_t[]){10, 11, 12, 14, 15, 16, 18, 19}, SensorCount);
+  qtr.setEmitterPin(7);
+
+  delay(500);
+  pinMode(LED_BUILTIN, OUTPUT);
+  digitalWrite(LED_BUILTIN, HIGH);// turn on Arduino's LED to indicate we are in calibration mode
+  Serial.println("Starting Calibration");
+
+  // 2.5 ms RC read timeout (default) * 10 reads per calibrate() call
+  // = ~25 ms per calibrate() call.
+  // Call calibrate() 400 times to make calibration take about 10 seconds.
+  for (uint16_t i = 0; i < 400; i++)
+  {
+    qtr.calibrate();
+  }
+  digitalWrite(LED_BUILTIN, LOW); // turn off Arduino's LED to indicate we are through with calibration
+  Serial.println("Calibration finished");
+
+  // print the calibration minimum values measured when emitters were on
+  Serial.begin(9600);
+  for (uint8_t i = 0; i < SensorCount; i++)
+  {
+    Serial.print(qtr.calibrationOn.minimum[i]);
+    Serial.print(' ');
+  }
+  Serial.println();
+
+  // print the calibration maximum values measured when emitters were on
+  for (uint8_t i = 0; i < SensorCount; i++)
+  {
+    Serial.print(qtr.calibrationOn.maximum[i]);
+    Serial.print(' ');
+  }
+  Serial.println();
+  Serial.println();
+  delay(1000);
+}
+
+void loop() {
+  uint16_t position = qtr.readLineBlack(sensorValues);
+  
+   for (uint8_t i = 0; i < SensorCount; i++)
+      {
+        Serial.print(sensorValues[i]);
+        Serial.print('\t');
+      }
+      Serial.println(position);
+
+      delay(250);
+
+      if (position >= 3000 and position <= 4300) {
+        left_motor_go(200);
+        right_motor_go(200);
+        Serial.print(position);Serial.println("center");
+      }
+
+      if (position < 3000) {
+        left_motor_go(200);
+        right_motor_go(0);
+        Serial.print(position);Serial.println("left");
+      }
+
+      if (position > 4300) {
+        left_motor_go(0);
+        right_motor_go(200);
+        Serial.print(position);Serial.println("right");
+      }
+
+      
+}
+  
+void init_motor(){
+  pinMode(left_motor_in, OUTPUT);
+  pinMode(left_motor_out, OUTPUT);
+  
+  pinMode(right_motor_in, OUTPUT);
+  pinMode(right_motor_out, OUTPUT);
+
+  pinMode(MODE, OUTPUT);
+  digitalWrite(MODE, HIGH);
+  }
+
+//range:0<value<255
+void left_motor_go(int motor_speed){
+  analogWrite(left_motor_in, motor_speed);
+  digitalWrite(left_motor_out, LOW);
+ }
+//range:0<value<255
+void right_motor_go(int motor_speed){
+  analogWrite(right_motor_in, motor_speed);
+  digitalWrite(right_motor_out, LOW);
+}