From 47c9f5f57aabd7e8f0917e463b43ea9f9a5a3217 Mon Sep 17 00:00:00 2001
From: Kilian DECLERCQ <kilian.declercq@ecam.fr>
Date: Sun, 10 Apr 2022 18:21:26 +0200
Subject: [PATCH] Final square trajectroy code

---
 control/control.ino | 57 +++++++++++++++++++++++++--------------------
 1 file changed, 32 insertions(+), 25 deletions(-)

diff --git a/control/control.ino b/control/control.ino
index c5b8192..378955f 100644
--- a/control/control.ino
+++ b/control/control.ino
@@ -12,45 +12,52 @@ double threshold= 10;
 int Squarex[]={0,25,25,0};
 int Squarey[]={0,0,25,25};
 bool tmp=0;
-double angle =0;
-double angle2=0;
+double angle =0.0;
+double angleDifference=0.0;
+double targetAngle = 90.0;
+
 MeGyro gyro;
 
 void setup()
-{Serial.begin(9600);
+{
+Serial.begin(9600);
 gyro.begin();
 }
 
 void loop()
 {
-     if(tmp == 0){
-      motorSpeed =100;
+     if(tmp == 0){          /* if-loop with flag to make the robot go 25 cm forward  */
+      motorSpeed = 100;
       motor1.run(-motorSpeed); /* value: between -255 and 255. */
       motor2.run(motorSpeed); /* value: between -255 and 255. */
       delay(2000);
       motor1.stop();
       motor2.stop();
-      tmp = 1;
+      gyro.update();
+      angle = gyro.getAngleZ();  /* Sense the initial position of the robot before turning */
+      tmp = 1;                  /* update flag */
      }
 
-      if (tmp == 1) {
-      gyro.update();
-      
-      Serial.read();
-      angle = 90 - gyro.getAngleZ();
-      Serial.println(gyro.getAngleZ());  
-           
-      motor1.run(2.8*angle); /* value: between -255 and 255. */
-      motor2.run(2.8*angle); /* value: between -255 and 255. */
-      
-      if (2.8*angle <=100){
+     if (tmp == 1) {              /* if loop for rotation after translation */
 
-          motor1.run(100); /* value: between -255 and 255. */
-          motor2.run(100);}
-          
-      if(abs(2.8*angle) <= 35){
-                tmp =0;}
-                }
-
-                
+      gyro.update();      
+      angleDifference = gyro.getAngleZ() - angle;  /* Calculate how much the robot turned */   
+       
+       if(angleDifference < targetAngle){ 
+        if (angleDifference < 0){
+          motorSpeed = 2.83*(targetAngle);
+         }
+        else{
+          motorSpeed = 2.83*(targetAngle-angleDifference); 
+         }               
+         motor1.run(motorSpeed);
+         motor2.run(motorSpeed);        
+        }
+        
+       if(angleDifference >= targetAngle){ /* if robot reaches 90°, stop rotation and update flag*/
+         motor1.stop();
+         motor2.stop();
+         tmp = 0;          
+        }       
+     }
 }