From d790520ddc65c772924627ec56a7338164b49ed9 Mon Sep 17 00:00:00 2001
From: yikestone <rishabhkundu13@gmail.com>
Date: Fri, 17 Aug 2018 23:08:54 +0530
Subject: [PATCH] minor changes

---
 src/libraries/Arduino/Arduino.ino | 61 +------------------------------
 src/libraries/Arduino/commands.h  |  2 +-
 2 files changed, 3 insertions(+), 60 deletions(-)

diff --git a/src/libraries/Arduino/Arduino.ino b/src/libraries/Arduino/Arduino.ino
index 933fa3d..cd2f14e 100644
--- a/src/libraries/Arduino/Arduino.ino
+++ b/src/libraries/Arduino/Arduino.ino
@@ -10,21 +10,8 @@
 #include "motor_driver.h"
 #include "encoder_driver.h"
 #include "diff_controller.h"
-#include <Wire.h>
-#include "I2Cdev.h"
-#include "AK8975.h"
-#include "MPU6050.h"
-#include "Kalman.h"
 
-
-Kalman filter;
-AK8975 mag(0x0C);
-MPU6050 accelgyro;
 float batt_lvl;
-int16_t gx;
-int16_t mx, my, mz;
-int dt;
-
 
 #define PID_RATE           30     // Hz
 const int PID_INTERVAL = 1000 / PID_RATE;
@@ -39,7 +26,6 @@ char argv1[16];
 char argv2[16];
 long arg1;
 long arg2;
-float heading = 0.0;
 
 void resetCommand() {
   cmd = NULL;
@@ -128,9 +114,6 @@ int runCommand() {
   case BATTERY_LVL:
 	  Serial.println((int)(batt_lvl * 100));
 	  break;
-  case IMU:
-    Serial.println((int)(heading * 100));
-    break;
   default:
     Serial.println("Invalid Command");
     break;
@@ -139,29 +122,12 @@ int runCommand() {
 
 /* Setup function--runs once at startup. */
 void setup() {
-  Wire.setClock(400000);
-  Wire.begin();
   Serial.begin(BAUDRATE);
   
-  accelgyro.initialize();
-  accelgyro.setI2CBypassEnabled(true);
-  mag.initialize();
-  mag.testConnection();
-  delay(100);
-  mag.getHeading(&my, &mx, &mz);
-  delay(10);
-  mag.getHeading(&my, &mx, &mz);
-  
-  heading = atan2(-mz, my) * RAD_TO_DEG;
-  
-  filter.setQangle(0.0001);
-  filter.setQbias(0.003);
-  filter.setRmeasure(0.05);
-  filter.setAngle(heading);
-
     pinMode(A0,INPUT);
     digitalWrite(A0,1);
     batt_lvl = analogRead(A0);
+    
     DDRC &= ~(1<<LEFT_ENC_PIN_A);
     DDRC &= ~(1<<LEFT_ENC_PIN_B);
     DDRD &= ~(1<<RIGHT_ENC_PIN_A);
@@ -185,33 +151,10 @@ void setup() {
   resetPID();
 
 }
-            
-uint32_t timer;
+
 void loop() {
 
-  gx = accelgyro.getRotationX();
-  
-  mag.getHeading(&my, &mx, &mz);
   batt_lvl = ( analogRead(A0) * 12.22 / 800.0 ) * 0.001 + batt_lvl * 0.999;
-  
-  double dt = (micros() - timer) / 1000000.0;
-  timer = micros();
-
-  Serial.println(heading);
-  
-  float tempY = atan2(-mz, my) * RAD_TO_DEG;
-
-  if ((tempY < -90 && heading > 90) || (tempY > 90 && heading < -90)) {
-    filter.setAngle(tempY);
-    heading = tempY;
-    }
-
-  else
-
-    heading = filter.getAngle(tempY, gx / 131.0, dt);
-
-  //Serial.println(heading);
-
 
   while (Serial.available() > 0) {
 
diff --git a/src/libraries/Arduino/commands.h b/src/libraries/Arduino/commands.h
index 4c35bda..3d7a393 100644
--- a/src/libraries/Arduino/commands.h
+++ b/src/libraries/Arduino/commands.h
@@ -11,6 +11,6 @@
 #define  BATTERY_LVL    'B'
 #define  LEFT            0
 #define  RIGHT           1
-#define  IMU            'i'
+
 #endif