removed imu support, added battery level correction

config/qt_pi_arduino_params.yaml

@@ -1,5 +1,5 @@
 
-port: /dev/ttyUSB0
+port: /dev/rfcomm1
 baud: 57600
 timeout: 0.1
 

scripts/qt_pi/arduino_driver.py

@@ -237,7 +237,6 @@ class Arduino:
         else:
             if self.motors_reversed:
                 values[0], values[1] = -values[0], -values[1]
-            print values
             return values
 
     def reset_encoders(self):

scripts/qt_pi/base_controller.py

@@ -211,9 +211,9 @@ class BaseController:
                 self.arduino.drive(self.v_left, self.v_right)
                 
             self.t_next = now + self.t_delta
-            self.batt_lvl = self.arduino.get_batt_lvl()
+            self.batt_lvl = self.arduino.get_batt_lvl() / 100.0
 
-            self.batt_lvl_pub.publish(((self.batt_lvl) / 5) * 5 * 12.85 / 900)
+            self.batt_lvl_pub.publish(self.batt_lvl)
     def stop(self):
         self.stopped = True
         self.arduino.drive(0, 0)

src/libraries/Arduino/Arduino.ino

@@ -12,14 +12,17 @@
 #include "diff_controller.h"
 #include <Wire.h>
 #include "I2Cdev.h"
-#include "MPU9150.h"
+#include "AK8975.h"
+#include "MPU6050.h"
 #include "Kalman.h"
 
+
 Kalman filter;
-MPU9150 accelGyroMag;
+AK8975 mag(0x0C);
+MPU6050 accelgyro;
 float batt_lvl;
 int16_t gx;
-float mx, my, mz;
+int16_t mx, my, mz;
 int dt;
 
 
@@ -37,6 +40,7 @@ char argv2[16];
 long arg1;
 long arg2;
 float heading = 0.0;
+
 void resetCommand() {
   cmd = NULL;
   memset(argv1, 0, sizeof(argv1));
@@ -122,10 +126,10 @@ int runCommand() {
     Serial.println("OK");
     break;
   case BATTERY_LVL:
-	  Serial.println(batt_lvl);
+	  Serial.println((int)(batt_lvl * 100));
 	  break;
   case IMU:
-    Serial.println(heading);
+    Serial.println((int)(heading * 100));
     break;
   default:
     Serial.println("Invalid Command");
@@ -138,10 +142,18 @@ void setup() {
   Wire.setClock(400000);
   Wire.begin();
   Serial.begin(BAUDRATE);
-  accelGyroMag.initialize();
+  
+  accelgyro.initialize();
+  accelgyro.setI2CBypassEnabled(true);
+  mag.initialize();
+  mag.testConnection();
   delay(100);
-  accelGyroMag.getHadjusted(&my, &mx, &mz);
+  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);
@@ -177,13 +189,15 @@ void setup() {
 uint32_t timer;
 void loop() {
 
-  gx = accelGyroMag.getRotationX();
+  gx = accelgyro.getRotationX();
-  accelGyroMag.getHadjusted(&my, &mx, &mz);
+  
+  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;
 
@@ -252,4 +266,3 @@ void loop() {
   }
 
 }
-

src/libraries/Arduino/diff_controller.h

@@ -45,10 +45,12 @@ void doPID(SetPointInfo * p) {
   input = p->Encoder - p->PrevEnc;
   Perror = p->TargetTicksPerFrame - input;
   p->ITerm += (p->Ki) * Perror;
-  output = p->base_pwm + ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko);
+  output = ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko);
-  p->PrevEnc = p->Encoder;
   
-  //output += p->output;
+  if(p->TargetTicksPerFrame > 0) output += p->base_pwm;
+  else if(p->TargetTicksPerFrame < 0) output -= p->base_pwm;
+  
+  p->PrevEnc = p->Encoder;
   
   if (output >= MAX_PWM)
     output = MAX_PWM;