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_pub.publish(((self.batt_lvl) / 5) * 5 * 12.85 / 900)
+            self.batt_lvl = self.arduino.get_batt_lvl() / 100.0
+
+            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));
@@ -54,12 +58,12 @@ int runCommand() {
   int pid_args[4];
   arg1 = atoi(argv1);
   arg2 = atoi(argv2);
-  
+
   switch(cmd) {
   case GET_BAUDRATE:
     Serial.println(BAUDRATE);
     break;
-    
+
   case READ_ENCODERS:
     Serial.print(readEncoder(LEFT));
     Serial.print(" ");
@@ -85,7 +89,7 @@ int runCommand() {
     rightPID.TargetTicksPerFrame = arg2;
     Serial.println("OK");
     break;
-    
+
   case UPDATE_PID_L:
     while ((str = strtok_r(p, ":", &p)) != '\0') {
        pid_args[i] = atoi(str);
@@ -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");
@@ -136,17 +140,25 @@ int runCommand() {
 /* Setup function--runs once at startup. */
 void setup() {
   Wire.setClock(400000);
-  Wire.begin();  
+  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);
   filter.setAngle(heading);
-  
+
     pinMode(A0,INPUT);
     digitalWrite(A0,1);
     batt_lvl = analogRead(A0);
@@ -154,53 +166,55 @@ void setup() {
     DDRC &= ~(1<<LEFT_ENC_PIN_B);
     DDRD &= ~(1<<RIGHT_ENC_PIN_A);
     DDRD &= ~(1<<RIGHT_ENC_PIN_B);
-    
+
     //enable pull up resistors
     PORTC |= (1<<LEFT_ENC_PIN_A);
     PORTC |= (1<<LEFT_ENC_PIN_B);
     PORTD |= (1<<RIGHT_ENC_PIN_A);
     PORTD |= (1<<RIGHT_ENC_PIN_B);
-    
+
     // tell pin change mask to listen to left encoder pins
     PCMSK1 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
     // tell pin change mask to listen to right encoder pins
     PCMSK2 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
-    
+
     // enable PCINT1 and PCINT2 interrupt in the general interrupt mask
     PCICR |= (1 << PCIE1) | (1 << PCIE2);
-    
+
   initMotorController();
   resetPID();
-  
-}
 
+}
+            
 uint32_t timer;
 void loop() {
 
-  gx = accelGyroMag.getRotationX();
-  accelGyroMag.getHadjusted(&my, &mx, &mz);
+  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) {
-    
+
     // Read the next character
     chr = Serial.read();
 
@@ -238,18 +252,17 @@ void loop() {
       }
     }
   }
-  
+
   if (millis() > nextPID) {
     updatePID();
     nextPID += PID_INTERVAL;
   }
-  
+
   // Check to see if we have exceeded the auto-stop interval
   if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
     setMotorSpeeds(0, 0);
     moving = 0;
     resetPID();
   }
-  
-}
 
+}

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);
-  p->PrevEnc = p->Encoder;
+  output = ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko);
   
-  //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;