From e73c0bc69c26bbbca0452b112263f99f2f3d914e Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Sun, 17 Mar 2019 18:57:32 +0100
Subject: [PATCH] implemented gy85 with yaw control

---
 controller.ino | 80 +++++++++++++++++++++++++++++++++++++++++++++-----
 1 file changed, 72 insertions(+), 8 deletions(-)

diff --git a/controller.ino b/controller.ino
index c8f9166..b6426d5 100644
--- a/controller.ino
+++ b/controller.ino
@@ -7,14 +7,25 @@
 //to flash set boot0 (the one further away from reset button) to 1 and press reset, flash, program executes immediately
 //set boot0 back to 0 to run program on powerup
 
-#define DEBUG
+//#define DEBUG
 
 #define PIN_LED PC13
 
+#define SENDPERIOD 20 //ms
 
-#define SENDPERIOD 20
+#define IMUUPDATEPERIOD 10 //ms
+long last_imuupdated=0;
 
 
+#include <IMUGY85.h>
+//https://github.com/fookingthg/GY85
+//ITG3200 and ADXL345 from https://github.com/jrowberg/i2cdevlib/tree/master/Arduino
+//https://github.com/mechasolution/Mecha_QMC5883L   //because QMC5883 on GY85 instead of HMC5883, source: https://circuitdigest.com/microcontroller-projects/digital-compass-with-arduino-and-hmc5883l-magnetometer
+//in qmc5883L library read values changed from uint16_t to int16_t 
+
+IMUGY85 imu;
+double ax, ay, az, gx, gy, gz, roll, pitch, yaw, mx,my,mz,ma;
+double setYaw=0;
 
 
 //from left to right. pins at bottom. chips on top
@@ -94,6 +105,9 @@ void setup() {
 
   radio.startListening();
 
+  Serial.println("Initializing IMU");
+  imu.init();
+
   Serial.println("Initialized");
 
 
@@ -101,6 +115,11 @@ void setup() {
 
 void loop() {
 
+  if (millis()-last_imuupdated>IMUUPDATEPERIOD){
+    updateIMU();
+    last_imuupdated=millis();
+  }
+  
   //NRF24
   nrf_delay=millis()-last_nrfreceive; //update nrf delay
   if ( radio.available() )
@@ -116,18 +135,15 @@ void loop() {
       motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0
       if (!motorenabled){ //disable motors?
         armed=false;
-        Serial.println("!motorenebled. armed=false");
       }
 
       if (lastnrfdata.speed==NRFDATA_CENTER && lastnrfdata.speed==NRFDATA_CENTER){ //arm only when centered        
         armed=true; //arm at first received packet
-        Serial.println("centered. armed=true");
       }      
 
       uint8_t calcchecksum=(uint8_t)((lastnrfdata.steer+3)*(lastnrfdata.speed+13));
       if (lastnrfdata.checksum!=calcchecksum){ //checksum not ok?
         armed=false; 
-        Serial.println("Checksum fail. armed=false");
       }else{ //checksum ok
         last_nrfreceive=millis();
       }
@@ -149,19 +165,48 @@ void loop() {
 
       //y positive = forward
       //x positive = right
+
+      setYaw+=((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*10/127;
+      while (setYaw<0){
+        setYaw+=360;
+      }
+      while (setYaw>=360){
+        setYaw-=360;
+      }
+
+      /*
+      Serial.print("setYaw=");
+      Serial.print(setYaw);
+      Serial.print(" Yaw=");
+      Serial.println(yaw);*/
       
     }
   }
 
   if (armed && nrf_delay>=MAX_NRFDELAY){ //too long since last sucessful nrf receive
     armed=false;
-    Serial.println("too long since last nrf data. armed=false");
   }
   if (armed){
     //out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
     //out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
-    out_speed=(int16_t)( ((int16_t)(lastnrfdata.speed)-NRFDATA_CENTER)*1000/127 );
-    out_steer=(int16_t)( ((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*1000/127 );
+    out_speed=(int16_t)( ((int16_t)(lastnrfdata.speed)-NRFDATA_CENTER)*1000/127 ); //-1000 to 1000
+    //out_steer=(int16_t)( ((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*1000/127 );
+    //Test Mag    
+    double yawdiff=(setYaw-180)-(yaw-180); //following angle difference works only for angles [-180,180]. yaw here is [0,360]
+    yawdiff += (yawdiff>180) ? -360 : (yawdiff<-180) ? 360 : 0;
+    //yawdiff/=2;
+    int yawdiffsign=1;
+    if (yawdiff<0){
+      yawdiffsign=-1;
+    }
+    yawdiff=yawdiff*yawdiff; //square
+    yawdiff=constrain(yawdiff/2,0,800);
+    yawdiff*=yawdiffsign; //redo sign
+    
+    out_steer=(int16_t)( yawdiff );
+    /*
+    Serial.print("Out steer=");
+    Serial.println(out_steer);*/
     
   }else{ //took too long since last nrf data
     out_steer=0;
@@ -199,3 +244,22 @@ void loop() {
   }
 
 }
+
+
+void updateIMU()
+{
+  //Update Imu and write to variables
+  imu.update();
+  imu.getAcceleration(&ax, &ay, &az);
+  imu.getGyro(&gx, &gy, &gz);
+  imu.getMag(&mx, &my, &mz,&ma); //calibration data such as bias is set in IMUGY85.h
+  roll = imu.getRoll();
+  pitch = imu.getPitch();
+  yaw = imu.getYaw();
+  /*Directions:
+   * Components on top.
+   * Roll: around Y axis (pointing to the right), left negative
+   * Pitch: around X axis (pointing forward), up positive
+   * Yaw: around Z axis, CCW positive,  0 to 360
+   */
+}