From f8195c3f37c577e4c052c343f20ff7aa5715e1f8 Mon Sep 17 00:00:00 2001
From: Julian <julian.leclerc@ecam.fr>
Date: Fri, 1 Apr 2022 12:11:19 +0200
Subject: [PATCH] testPoints

---
 control/control.ino | 94 +++++++++++++++++++++++++++++++++++++++++++++
 gyro/gyro.ino       | 44 ---------------------
 2 files changed, 94 insertions(+), 44 deletions(-)
 create mode 100644 control/control.ino
 delete mode 100644 gyro/gyro.ino

diff --git a/control/control.ino b/control/control.ino
new file mode 100644
index 0000000..7d9e5c4
--- /dev/null
+++ b/control/control.ino
@@ -0,0 +1,94 @@
+//#include "MeOrion.h"
+#include <MeMCore.h>
+#include <Wire.h>
+
+MeGyro gyro;
+MeDCMotor motor3(M1);
+MeDCMotor motor4(M2);
+
+uint8_t motorSpeed = 200;
+MeUltrasonicSensor ultraSensor(PORT_3);
+
+double rWheel = 6.5;
+double dWheel = 11.5;
+
+double Pos[4][1];
+
+double angularError = 10;
+double posError = 10;
+
+double angle1;
+double angle2;
+double dist;
+
+void setup()
+{
+  Serial.begin(115200);
+  gyro.begin();
+
+  Pos[0][0] = 0;
+  Pos[0][1] = 0;
+  Pos[1][0] = 25;
+  Pos[1][1] = 0;
+  Pos[2][0] = 25;
+  Pos[2][1] = 25;
+  Pos[3][0] = 0;
+  Pos[3][1] = 25;
+}
+
+void loop()
+{
+  Serial.println("Start Loop");
+  int i;
+  int j;
+  Serial.read();
+  for(i=0;i<4;i++){
+  Serial.print("Beggin angular adjustment for i: ");
+  Serial.println(i);
+  //angle adjustment
+    gyro.update();
+    
+    angle1=gyro.getAngleZ();
+    angle2=atan( (Pos[i+1][1]-Pos[i][1])/ (Pos[i+1][0]-Pos[i][0]) );
+    motorSpeed = 70;
+    Serial.print("angle1: ");
+    Serial.print(angle1);
+    Serial.print(" angle2: ");
+    Serial.println(angle2);
+    
+    while(angle1-angle2+360 > angularError) {
+      gyro.update();
+      angle1=gyro.getAngleZ();
+      angle2=atan( (Pos[i+1][0]-Pos[i][0])/ (Pos[i+1][1]-Pos[i][1]) );
+      Serial.println("i: ");
+      Serial.println(i);
+      Serial.print(" angle1: ");
+      Serial.print(angle1);
+      Serial.print(" angle2: ");
+      Serial.println(angle2);
+      if(angle2+180<angle1+180){
+        motor3.run(-motorSpeed);
+        motor4.run(-motorSpeed);
+      } else {
+        motor3.run(motorSpeed);
+        motor4.run(motorSpeed);
+      }
+      delay(200);
+    }
+    motor3.stop();
+    motor4.stop();
+    //translation
+    dist=sqrt( pow(Pos[i+1][0]-Pos[i][0],2)+pow(Pos[i+1][1]-Pos[i][1],2) );
+    motorSpeed = 0.2/rWheel;
+    for(j = 0;j<5;j++) {
+      motor3.run(motorSpeed);
+      motor4.run(-motorSpeed);
+      delay(1000);
+    }
+
+    motor3.stop();
+    motor4.stop();
+    delay(1000);
+  }
+  delay(100);
+}
diff --git a/gyro/gyro.ino b/gyro/gyro.ino
deleted file mode 100644
index 7e7dd8e..0000000
--- a/gyro/gyro.ino
+++ /dev/null
@@ -1,44 +0,0 @@
-/**
- * \par Copyright (C), 2012-2016, MakeBlock
- * @file    GyroRotation.ino
- * @author  MakeBlock
- * @version V1.0.0
- * @date    2015/09/09
- * @brief   Description: this file is sample code for MeGyro device.
- *
- * Function List:
- * 1. void MeGyro::begin(void)
- * 2. void MeGyro::update(void) 
- * 3. double MeGyro::angleX(void)
- * 4. double MeGyro::angleY(void)
- * 5. double MeGyro::angleZ(void)
- *
- * \par History:
- * <pre>
- * <Author>     <Time>        <Version>      <Descr>
- * Mark Yan     2015/09/09    1.0.0          rebuild the old lib
- * </pre>
- *
- */
-#include "MeOrion.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);
-}