From e03bde591da62e832830d0d067725b5095a61612 Mon Sep 17 00:00:00 2001
From: hhaupt <hans.haupt@dezibot.de>
Date: Sun, 12 May 2024 18:55:54 +0200
Subject: [PATCH] first working example of IMU Based Motioncorrection

---
 .../motion_correction/.theia/launch.json      |   8 ++
 .../motion_correction/motion_correction.ino   | 112 ++++++++++++++++++
 src/Dezibot.cpp                               |   6 +-
 src/Dezibot.h                                 |   7 +-
 4 files changed, 131 insertions(+), 2 deletions(-)
 create mode 100644 example/IMU/Motion_Correction/motion_correction/.theia/launch.json
 create mode 100644 example/IMU/Motion_Correction/motion_correction/motion_correction.ino

diff --git a/example/IMU/Motion_Correction/motion_correction/.theia/launch.json b/example/IMU/Motion_Correction/motion_correction/.theia/launch.json
new file mode 100644
index 0000000..9a49ac9
--- /dev/null
+++ b/example/IMU/Motion_Correction/motion_correction/.theia/launch.json
@@ -0,0 +1,8 @@
+{
+  // Use IntelliSense to learn about possible attributes.
+  // Hover to view descriptions of existing attributes.
+  "version": "0.2.0",
+  "configurations": [
+    
+  ]
+}
diff --git a/example/IMU/Motion_Correction/motion_correction/motion_correction.ino b/example/IMU/Motion_Correction/motion_correction/motion_correction.ino
new file mode 100644
index 0000000..58416df
--- /dev/null
+++ b/example/IMU/Motion_Correction/motion_correction/motion_correction.ino
@@ -0,0 +1,112 @@
+#include "Dezibot.h"
+#define interval 30
+Dezibot dezibot = Dezibot();
+#define maxDuty 160
+#define minDuty 90
+#define baseDuty 110
+void setup() {
+  // put your setup code here, to run once:
+  dezibot.begin();
+  Serial.begin(115200);
+  //dezibot.motion.move();
+  //analogWrite(11, 2000);
+  //analogWrite(12, 2000);
+}
+uint8_t leftDuty= baseDuty;
+uint8_t rightDuty = baseDuty;
+unsigned long previous = millis();
+unsigned long current = millis();
+long averageZRotation = 0;
+long averageXAccel = 0;
+int left=0;
+int right=0;
+int resultcounter = 0;
+void loop() {
+  // put your main code here, to run repeatedly:
+  current = millis();
+  IMUResult result = dezibot.motionDetection.getRotation();
+  averageXAccel += dezibot.motionDetection.getAcceleration().x;
+  averageZRotation += result.z;
+  //Serial.println(result.z);
+  if (result.z > 100){
+    right++;
+  } else if(result.z < -100) {
+    left++;
+  }
+  resultcounter++;
+  if ((current - previous) > interval){
+    Serial.println("=============================================");
+    previous = current;
+    averageZRotation = averageZRotation / resultcounter;
+    averageXAccel = averageXAccel / resultcounter;
+    Serial.print("Z:");
+    Serial.println(averageZRotation);
+    dezibot.display.clearDisplay();
+    dezibot.display.setCursor(0, 0);
+    dezibot.display.setTextColor(WHITE);
+    dezibot.display.setTextSize(2); // Draw 2X-scale text
+    dezibot.display.println(averageZRotation);
+    dezibot.display.print(left);
+    dezibot.display.print("   ");
+    dezibot.display.println(right);
+    Serial.println("=============================================");
+    /*if(averageZRotation < -20){
+      //turns right
+      if((rightDuty < maxDuty || leftDuty <= minDuty) && rightDuty<255){
+        rightDuty++;
+      } else{
+        leftDuty--;
+      }
+    } else if(averageZRotation > 20){
+      //turns left
+      if(leftDuty < maxDuty || rightDuty <= minDuty && leftDuty<255){
+        leftDuty++;
+      } else {
+        rightDuty--;
+      }
+    }*/
+    if(left>right){ //rotates anticlock
+      leftDuty++;
+      rightDuty--;
+      /*if(abs(leftDuty-baseDuty)<abs(rightDuty-baseDuty)){
+        rightDuty--;
+      } else{
+        leftDuty++;
+      }
+        
+      if (leftDuty == maxDuty){
+        rightDuty -= 5;
+        leftDuty -= 1;
+       }else {
+      leftDuty++;
+      }*/
+    } else if((left-right)<0){
+      leftDuty--;
+      rightDuty++;
+      /*if(abs(leftDuty-baseDuty)>abs(rightDuty-baseDuty)){
+        rightDuty++;
+      } else{
+        leftDuty--;
+      }
+      if( leftDuty == minDuty){
+        rightDuty += 5;
+        leftDuty += 1;
+      } else{
+        leftDuty--;
+      }*/
+    } 
+    analogWrite(12,leftDuty);
+    analogWrite(11, rightDuty);
+
+    dezibot.display.println(leftDuty);
+    dezibot.display.println(rightDuty);
+    dezibot.display.display();
+
+
+    averageZRotation = 0;
+    averageXAccel = 0;
+    resultcounter = 0;
+    left = 0;
+    right = 0;
+  }
+}
diff --git a/src/Dezibot.cpp b/src/Dezibot.cpp
index c412e89..f708ccd 100644
--- a/src/Dezibot.cpp
+++ b/src/Dezibot.cpp
@@ -12,11 +12,15 @@
 
 #include "Dezibot.h"
 
-Dezibot::Dezibot():multiColorLight(),motionDetection(){
+Dezibot::Dezibot():multiColorLight(),motionDetection(),display(128, 64, &Wire, -1){
 
 };
 void Dezibot::begin(void) {
     motion.begin();
     multiColorLight.begin();
     motionDetection.begin();
+    Wire.begin(1,2);
+    display.begin(SSD1306_SWITCHCAPVCC,0x3C);
+    display.clearDisplay();
+    display.display();
 };
diff --git a/src/Dezibot.h b/src/Dezibot.h
index 50fcca6..001744d 100644
--- a/src/Dezibot.h
+++ b/src/Dezibot.h
@@ -16,6 +16,9 @@
 #include "colorDetection/ColorDetection.h"
 #include "multiColorLight/MultiColorLight.h"
 #include "motionDetection/MotionDetection.h"
+#include "Arduino.h"
+#include "Wire.h"
+#include "Adafruit_SSD1306.h"
 
 class Dezibot {
 protected:
@@ -27,7 +30,9 @@ public:
     ColorDetection colorDetection;
     MultiColorLight multiColorLight;
     MotionDetection motionDetection;
-
+    //temporary, display component is not implemented yet
+    Adafruit_SSD1306 display;
+    
     void begin(void);
 /*
 Display display