square path

control/control.ino

@@ -0,0 +1,56 @@
+#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;
+double angle2=0;
+MeGyro gyro;
+
+void setup()
+{Serial.begin(9600);
+gyro.begin();
+}
+
+void loop()
+{
+     if(tmp == 0){
+      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;
+     }
+
+      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){
+
+          motor1.run(100); /* value: between -255 and 255. */
+          motor2.run(100);}
+          
+      if(abs(2.8*angle) <= 35){
+                tmp =0;}
+                }
+
+                
+}