Control on hold

control/control.ino

@@ -0,0 +1,79 @@
+#include "MeMCore.h"
+#include <Wire.h>
+
+MeGyro gyro;
+MeDCMotor leftMotor(9);
+MeDCMotor rightMotor(10);
+
+uint8_t motorSpeed = 100;
+
+void setup() {
+
+  Serial.begin(115200);
+  gyro.begin();
+
+  leftMotor.run(0);
+  rightMotor.run(0);
+
+}
+
+void loop() {
+  //(0,0)
+  gyro.update();
+  goForward();
+  delay(6000);
+  stop();
+
+  //(10,0)
+  
+  float error;
+  float d_target = 90;
+  float p;
+  float v_e;
+  float v_r;
+  
+  while (error > 10){
+    gyro.update();
+    error = d_target - gyro.getAngleZ();
+    v_e = p*error;
+    v_r = -v_e;
+    goLeft(v_r);
+  }
+  stop();
+  
+  //(10,10)
+  //(0,10)
+
+}
+
+void goForward(){
+
+  leftMotor.run(-motorSpeed);
+  rightMotor.run(motorSpeed);
+  delay(1000);  
+
+}
+
+void goBackward(){
+
+  leftMotor.run(motorSpeed);
+  rightMotor.run(-motorSpeed);
+  delay(100);  
+
+}
+
+void goLeft(float rotateSpeed){
+
+  leftMotor.run(rotateSpeed);
+  rightMotor.run(rotateSpeed);
+  delay(100);
+
+}
+
+void stop(){
+
+  leftMotor.stop();
+  rightMotor.stop();
+  delay(100);
+
+}

gyro/gyro.ino

@@ -12,11 +12,13 @@ void setup() {
 void loop() {
   gyro.update();
   Serial.read();
-  Serial.print("X: ");
+  Serial.print("X:");
   Serial.print(gyro.getAngleX());
-  Serial.print("Y: ");
+  Serial.print(",");
+  Serial.print("Y:");
   Serial.print(gyro.getAngleY());
-  Serial.print("Z: ");
+  Serial.print(",");
+  Serial.print("Z:");
   Serial.println(gyro.getAngleZ());
   delay(10);