From 1adaf8bc4946d87ace12e03896a0ab26834cd571 Mon Sep 17 00:00:00 2001
From: "ly.pechvattana" <ly.pechvattana@ecam.fr>
Date: Fri, 31 Mar 2023 16:42:05 +0700
Subject: [PATCH] Control on hold

---
 control/control.ino | 79 +++++++++++++++++++++++++++++++++++++++++++++
 gyro/gyro.ino       |  8 +++--
 2 files changed, 84 insertions(+), 3 deletions(-)
 create mode 100644 control/control.ino

diff --git a/control/control.ino b/control/control.ino
new file mode 100644
index 0000000..ed7ef08
--- /dev/null
+++ b/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);
+
+}
\ No newline at end of file
diff --git a/gyro/gyro.ino b/gyro/gyro.ino
index 222575f..8cab0fd 100644
--- a/gyro/gyro.ino
+++ b/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);