Initial commit

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
+}

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);
+  }

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 );
+
+
+}