From bd723b38dfe1afaebc868a5e865b7b9653c20815 Mon Sep 17 00:00:00 2001
From: Lucas Marais <lucas.marais.wrk@gmail.com>
Date: Fri, 31 Mar 2023 12:01:54 +0200
Subject: [PATCH] control

---
 control/control.ino | 73 +++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 73 insertions(+)
 create mode 100644 control/control.ino

diff --git a/control/control.ino b/control/control.ino
new file mode 100644
index 0000000..0e74278
--- /dev/null
+++ b/control/control.ino
@@ -0,0 +1,73 @@
+#include"MeMCore.h"
+#include <Wire.h>
+
+
+MeGyro gyro;
+MeDCMotor motor1(9);
+MeDCMotor motor2(10);
+int points[4][2]={{0,0},{0,25},{25,25},{25,0}};
+
+void setup() {
+  Serial.begin(115200);
+  gyro.begin();
+
+}
+
+void loop() {
+  
+  for(int i = 0;i<4;i++){
+    goToPoint(i);
+  }
+}
+
+void goToPoint(int i){
+  bool state = true;
+  while(state){
+    turn90();
+    runToPoint(i);
+  }
+}
+
+void runToPoint(int i){
+  float v0 = 0.0;
+  bool state = true;
+  float distance = 0;
+  while(state){
+    motor1.run(-100);
+    motor2.run(100);
+    delay(25*100);
+    state = false;
+    // gyro.update();
+    // float currentVelocity = gyro.getAcclY()*0.005+v0;
+    // distance += v0*0.05 + (1/2)*gyro.getAcclY()*0.05;
+    // v0 = currentVelocity;
+    // if (distance*100>max(point[1],point[0])){
+    //   state = false;
+    //   motor1.stop();
+    //   motor2.stop();
+    // }
+  }
+}
+
+
+void turn90(){
+  bool state = true;
+  gyro.update();
+  float z_initial = gyro.getAngleZ();
+  float z_stop = z_initial - 90;
+  if(z_stop<-180){
+    float step = -180-z_stop;
+    z_stop = 180-step;
+  }
+  while(state){
+    gyro.update();
+    motor1.run(-100);
+    motor2.run(-100);
+    Serial.println(String(round(gyro.getAngleZ()))+" and "+String(round(z_stop)));
+    if (round(gyro.getAngleZ())==round(z_stop)){
+      state = false;
+      motor1.stop();
+      motor2.stop();
+    }
+  }
+}
\ No newline at end of file