Added IMU support

config/qt_pi_arduino_params.yaml

@@ -18,13 +18,13 @@ encoder_resolution: 240 # from Pololu for 131:1 motors
 motors_reversed: False
 gear_reduction: 1.0
 # === PID parameters
-lKp: 50
+lKp: 45
-lKd: 10
+lKd: 0
-lKi: 0
+lKi: 5
-lKo: 20
+lKo: 3
-rKp: 50
+rKp: 45
-rKd: 10
+rKd: 0
-rKi: 0
+rKi: 5
-rKo: 20
+rKo: 3
 
 #accel_limit: 0.1

src/libraries/Arduino/Arduino.ino

@@ -10,6 +10,19 @@
 #include "motor_driver.h"
 #include "encoder_driver.h"
 #include "diff_controller.h"
+#include <Wire.h>
+#include "I2Cdev.h"
+#include "MPU9150.h"
+#include "Kalman.h"
+
+Kalman filter;
+MPU9150 accelGyroMag;
+float batt_lvl;
+int16_t gx;
+float mx, my, mz;
+int dt;
+
+
 #define PID_RATE           30     // Hz
 const int PID_INTERVAL = 1000 / PID_RATE;
 unsigned long nextPID = PID_INTERVAL;
@@ -23,7 +36,7 @@ char argv1[16];
 char argv2[16];
 long arg1;
 long arg2;
-
+float heading = 0.0;
 void resetCommand() {
   cmd = NULL;
   memset(argv1, 0, sizeof(argv1));
@@ -109,8 +122,11 @@ int runCommand() {
     Serial.println("OK");
     break;
   case BATTERY_LVL:
-	Serial.println(analogRead(A0));
+	  Serial.println(batt_lvl);
-	break;
+	  break;
+  case IMU:
+    Serial.println(heading);
+    break;
   default:
     Serial.println("Invalid Command");
     break;
@@ -119,9 +135,21 @@ int runCommand() {
 
 /* Setup function--runs once at startup. */
 void setup() {
+  Wire.setClock(400000);
+  Wire.begin();  
   Serial.begin(BAUDRATE);
+  accelGyroMag.initialize();
+  delay(100);
+  accelGyroMag.getHadjusted(&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);
@@ -146,9 +174,30 @@ void setup() {
   
 }
 
-
+uint32_t timer;
 void loop() {
 
+  gx = accelGyroMag.getRotationX();
+  accelGyroMag.getHadjusted(&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();
+  
+  
+  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) {
     
@@ -189,7 +238,7 @@ void loop() {
       }
     }
   }
-
+  
   if (millis() > nextPID) {
     updatePID();
     nextPID += PID_INTERVAL;
@@ -199,6 +248,7 @@ void loop() {
   if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
     setMotorSpeeds(0, 0);
     moving = 0;
+    resetPID();
   }
   
 }

src/libraries/Arduino/commands.h

@@ -1,16 +1,16 @@
-#ifndef COMMANDS_H
+#ifndef  COMMANDS_H
-#define COMMANDS_H
+#define  COMMANDS_H
-
-#define GET_BAUDRATE   'b'
-#define READ_ENCODERS  'e'
-#define MOTOR_SPEEDS   'm'
-#define RESET_ENCODERS 'r'
-#define UPDATE_PID_L   'L'
-#define UPDATE_PID_R   'R'
-#define DISP_PID_P     'z'
-#define BATTERY_LVL    'B'
-#define LEFT            0
-#define RIGHT           1
 
+#define  GET_BAUDRATE   'b'
+#define  READ_ENCODERS  'e'
+#define  MOTOR_SPEEDS   'm'
+#define  RESET_ENCODERS 'r'
+#define  UPDATE_PID_L   'L'
+#define  UPDATE_PID_R   'R'
+#define  DISP_PID_P     'z'
+#define  BATTERY_LVL    'B'
+#define  LEFT            0
+#define  RIGHT           1
+#define  IMU            'i'
 #endif
 

src/libraries/Arduino/diff_controller.h

@@ -1,4 +1,3 @@
-
 typedef struct {
   double TargetTicksPerFrame;
   long Encoder;
@@ -7,14 +6,14 @@ typedef struct {
   int PrevInput;
   
   int ITerm;
-  int Kp = 500;
+  int Kp = 45;
-  int Kd = 100;
+  int Kd = 0;
-  int Ki = 0;
+  int Ki = 5;
-  int Ko = 50;
+  int Ko = 3;
-
+  int base_pwm = 70;
   long output;
-
 }
+
 SetPointInfo;
 
 SetPointInfo leftPID, rightPID;
@@ -45,20 +44,18 @@ void doPID(SetPointInfo * p) {
 
   input = p->Encoder - p->PrevEnc;
   Perror = p->TargetTicksPerFrame - input;
-
+  p->ITerm += (p->Ki) * Perror;
-  output = ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko);
+  output = p->base_pwm + ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko);
   p->PrevEnc = p->Encoder;
-
+  
-  output += p->output;
+  //output += p->output;
-
+  
   if (output >= MAX_PWM)
     output = MAX_PWM;
   else if (output <= -MAX_PWM)
     output = -MAX_PWM;
   else
 
-  p->ITerm += (p->Ki) * Perror;
-
   p->output = output;
   p->PrevInput = input;
 }
@@ -67,15 +64,14 @@ void updatePID() {
 
   leftPID.Encoder = readEncoder(LEFT);
   rightPID.Encoder = readEncoder(RIGHT);
-  
+
   if (!moving){
     if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
     return;
   }
 
-
+  if(leftPID.TargetTicksPerFrame != 0) doPID(&leftPID);
-  doPID(&leftPID);
+  if(rightPID.TargetTicksPerFrame != 0) doPID(&rightPID);
-  doPID(&rightPID);
+  
   setMotorSpeeds(leftPID.output, rightPID.output);
 }
-

src/libraries/Arduino/encoder_driver.h

@@ -1,5 +1,5 @@
-  #define LEFT_ENC_PIN_A PC4
+  #define LEFT_ENC_PIN_A PC2
-  #define LEFT_ENC_PIN_B PC5
+  #define LEFT_ENC_PIN_B PC3
   
   #define RIGHT_ENC_PIN_A PD2
   #define RIGHT_ENC_PIN_B PD3

src/libraries/Arduino/encoder_driver.ino

@@ -6,7 +6,7 @@
   	static uint8_t enc_last=0;
         
 	enc_last <<=2;
-  enc_last |= (PINC & (3 << 4)) >> 4;
+  enc_last |= (PINC & (3 << 2)) >> 2;
   
   	left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
   }