Final square trajectroy code

control/control.ino

@@ -0,0 +1,63 @@
+#include "MeMCore.h"
+#include <Wire.h>
+const int Port_1 =9;
+const int Port_2 =10;
+
+MeDCMotor motor1(Port_1);
+MeDCMotor motor2(Port_2);
+MeUltrasonicSensor ultraSensor(PORT_3); /* Ultrasonic module can ONLY be connected to port 3, 4, 6, 7, 8 of base shield. */
+uint8_t motorSpeed =100;
+double distance;
+double threshold= 10;
+int Squarex[]={0,25,25,0};
+int Squarey[]={0,0,25,25};
+bool tmp=0;
+double angle =0.0;
+double angleDifference=0.0;
+double targetAngle = 90.0;
+
+MeGyro gyro;
+
+void setup()
+{
+Serial.begin(9600);
+gyro.begin();
+}
+
+void loop()
+{
+     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();
+      gyro.update();
+      angle = gyro.getAngleZ();  /* Sense the initial position of the robot before turning */
+      tmp = 1;                  /* update flag */
+     }
+
+     if (tmp == 1) {              /* if loop for rotation after translation */
+
+      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;          
+        }       
+     }
+}

gyro/gyro.ino

@@ -18,6 +18,6 @@ void loop()
   Serial.print(gyro.getAngleY() );
   Serial.print(" Z:");
   Serial.println(gyro.getAngleZ() );
-  
+ 
   delay(10);
 }