From eb11db6033df0d66c249c4dded0a93991c6c571b Mon Sep 17 00:00:00 2001
From: Stedd <tor.andre.paulsen@gmail.com>
Date: Thu, 19 Dec 2019 18:19:16 +0100
Subject: [PATCH] Initial commit

---
 Main/Main.ino          | 45 ++++++++++++++++++++++++
 Main/motor_control.ino | 35 +++++++++++++++++++
 Main/read_IMU.ino      | 78 ++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 158 insertions(+)
 create mode 100644 Main/Main.ino
 create mode 100644 Main/motor_control.ino
 create mode 100644 Main/read_IMU.ino

diff --git a/Main/Main.ino b/Main/Main.ino
new file mode 100644
index 0000000..6207a5e
--- /dev/null
+++ b/Main/Main.ino
@@ -0,0 +1,45 @@
+//Import
+#include "GY_85.h"
+#include <Wire.h>
+
+//Define L298N pin mappings
+const int IN1 = 3;
+const int IN2 = 2;
+const int IN3 = 5;
+const int IN4 = 4;
+#define LOW 0
+
+GY_85 GY85;     //create the object
+
+int   ax_, ay_, az_;
+int   ax, ay, az;
+int   cx, cy, cz;
+float gx, gy, gz, gt;
+
+
+
+void setup() {
+
+  //  Wire.begin(SCL,SDA);
+  Wire.begin(26, 27);
+  delay(10);
+  Serial.begin(57600);
+  delay(10);
+  GY85.init();
+  delay(10);
+
+  //Assign H bridge Pins
+  pinMode(IN1, OUTPUT);
+  pinMode(IN2, OUTPUT);
+  pinMode(IN3, OUTPUT);
+  pinMode(IN4, OUTPUT);
+
+}
+
+void loop() {
+  readIMU();
+  motorControl();
+
+  //Delay
+  delay(5);             // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz
+}
diff --git a/Main/motor_control.ino b/Main/motor_control.ino
new file mode 100644
index 0000000..e2b9040
--- /dev/null
+++ b/Main/motor_control.ino
@@ -0,0 +1,35 @@
+void motorControl()
+{
+  //Backward
+  if (mapValue[2] < 0)
+  {
+    if (mapValue[3] > 0)
+    {
+      analogWrite(IN3, -mapValue[2] * ((255 - mapValue[3]) / 255.0));
+      analogWrite(IN1, -mapValue[2]);
+    }
+    if (mapValue[3] < 0)
+    {
+      analogWrite(IN3, -mapValue[2]);
+      analogWrite(IN1, -mapValue[2] * ((255 + mapValue[3]) / 255.0));
+    }
+    analogWrite(IN2, LOW);
+    analogWrite(IN4, LOW);
+  }
+
+  //Forward
+  if (mapValue[2] > 0)
+  {
+    if (mapValue[3] > 0)
+    {
+      analogWrite(IN4, mapValue[2] * ((255 - mapValue[3]) / 255.0));
+      analogWrite(IN2, mapValue[2]);
+    }
+    if (mapValue[3] < 0)
+    {
+      analogWrite(IN4, mapValue[2]);
+      analogWrite(IN2, mapValue[2] * ((255 + mapValue[3]) / 255.0));
+    }
+    analogWrite(IN1, LOW);
+    analogWrite(IN3, LOW);
+  }
diff --git a/Main/read_IMU.ino b/Main/read_IMU.ino
new file mode 100644
index 0000000..71818b4
--- /dev/null
+++ b/Main/read_IMU.ino
@@ -0,0 +1,78 @@
+void readIMU()
+{
+  ax_ = GY85.accelerometer_x( GY85.readFromAccelerometer() );
+  ay_ = GY85.accelerometer_y( GY85.readFromAccelerometer() );
+  az_ = GY85.accelerometer_z( GY85.readFromAccelerometer() );
+
+  cx = GY85.compass_x( GY85.readFromCompass() );
+  cy = GY85.compass_y( GY85.readFromCompass() );
+  cz = GY85.compass_z( GY85.readFromCompass() );
+
+  gx = GY85.gyro_x( GY85.readGyro() );
+  gy = GY85.gyro_y( GY85.readGyro() );
+  gz = GY85.gyro_z( GY85.readGyro() );
+  gt = GY85.temp  ( GY85.readGyro() );
+
+  //    Serial.print  ( "accelerometer" );
+  //    Serial.print  ( " x:" );
+  //    Serial.print  ( ax );
+  //    Serial.print  ( " y:" );
+  //    Serial.print  ( ay );
+  //    Serial.print  ( " z:" );
+  //    Serial.print  ( az );
+
+  //For some reason the ints in the example behaves as unsigned int.. Maybe look at the .cpp code, might be something there, if not. This works OK.
+  if (ax_ > 60000) {
+    ax = (ax_ - 65535);
+  }
+  else  {
+    ax = ax_;
+  }
+
+  if (ay_ > 60000) {
+    ay = (ay_ - 65535);
+  }
+  else  {
+    ay = ay_;
+  }
+
+  if (az_ > 60000) {
+    az = (az_ - 65535);
+  }
+  else  {
+    az = az_;
+  }
+
+
+  //Serial plotter
+  //    Serial.print  ( " x:" );
+  Serial.print  ( ax );
+  Serial.print  ( "," );
+  Serial.print  ( ay );
+  Serial.print  ( "," );
+  Serial.println  ( az );
+
+  //        Serial.print(deg_R);
+  //    Serial.print(",");
+  //    Serial.println(deg_L);
+
+  //    Serial.print  ( "  compass" );
+  //    Serial.print  ( " x:" );
+  //    Serial.print  ( cx );
+  //    Serial.print  ( " y:" );
+  //    Serial.print  ( cy );
+  //    Serial.print  (" z:");
+  //    Serial.print  ( cz );
+
+  //    Serial.print  ( "  gyro" );
+  //    Serial.print  ( " x:" );
+  //    Serial.print  ( gx );
+  //    Serial.print  ( " y:" );
+  //    Serial.print  ( gy );
+  //    Serial.print  ( " z:" );
+  //    Serial.print  ( gz );
+  //    Serial.print  ( " gyro temp:" );
+  //    Serial.println( gt );
+
+
+}