control/control.ino

@@ -1,63 +0,0 @@
-#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

@@ -1,23 +0,0 @@
-#include "MeMCore.h"
-#include <Wire.h>
-
-MeGyro gyro;
-void setup()
-{
-  Serial.begin(115200);
-  gyro.begin();
-}
-
-void loop()
-{
-  gyro.update();
-  Serial.read();
-  Serial.print("X:");
-  Serial.print(gyro.getAngleX() );
-  Serial.print(" Y:");
-  Serial.print(gyro.getAngleY() );
-  Serial.print(" Z:");
-  Serial.println(gyro.getAngleZ() );
- 
-  delay(10);
-}