From 57dca60d0ceda05b5508dc5bb786c09c44d40b8d Mon Sep 17 00:00:00 2001 From: yikestone Date: Wed, 15 Aug 2018 01:18:36 +0530 Subject: [PATCH] Added IMU support --- config/qt_pi_arduino_params.yaml | 16 +++---- src/libraries/Arduino/Arduino.ino | 60 ++++++++++++++++++++++-- src/libraries/Arduino/commands.h | 26 +++++----- src/libraries/Arduino/diff_controller.h | 34 ++++++-------- src/libraries/Arduino/encoder_driver.h | 4 +- src/libraries/Arduino/encoder_driver.ino | 2 +- 6 files changed, 94 insertions(+), 48 deletions(-) diff --git a/config/qt_pi_arduino_params.yaml b/config/qt_pi_arduino_params.yaml index f5ec7fa..5c9d13a 100644 --- a/config/qt_pi_arduino_params.yaml +++ b/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 -lKd: 10 -lKi: 0 -lKo: 20 -rKp: 50 -rKd: 10 -rKi: 0 -rKo: 20 +lKp: 45 +lKd: 0 +lKi: 5 +lKo: 3 +rKp: 45 +rKd: 0 +rKi: 5 +rKo: 3 #accel_limit: 0.1 diff --git a/src/libraries/Arduino/Arduino.ino b/src/libraries/Arduino/Arduino.ino index d72a50c..43743cd 100644 --- a/src/libraries/Arduino/Arduino.ino +++ b/src/libraries/Arduino/Arduino.ino @@ -10,6 +10,19 @@ #include "motor_driver.h" #include "encoder_driver.h" #include "diff_controller.h" +#include +#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)); - break; + Serial.println(batt_lvl); + 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< 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(); } } diff --git a/src/libraries/Arduino/commands.h b/src/libraries/Arduino/commands.h index 4f3240b..4c35bda 100644 --- a/src/libraries/Arduino/commands.h +++ b/src/libraries/Arduino/commands.h @@ -1,16 +1,16 @@ -#ifndef 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 +#ifndef 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 IMU 'i' #endif diff --git a/src/libraries/Arduino/diff_controller.h b/src/libraries/Arduino/diff_controller.h index 564a490..25570b6 100644 --- a/src/libraries/Arduino/diff_controller.h +++ b/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 Kd = 100; - int Ki = 0; - int Ko = 50; - + int Kp = 45; + int Kd = 0; + int Ki = 5; + 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; - - output = ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko); + 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->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; } - - doPID(&leftPID); - doPID(&rightPID); + if(leftPID.TargetTicksPerFrame != 0) doPID(&leftPID); + if(rightPID.TargetTicksPerFrame != 0) doPID(&rightPID); + setMotorSpeeds(leftPID.output, rightPID.output); } - diff --git a/src/libraries/Arduino/encoder_driver.h b/src/libraries/Arduino/encoder_driver.h index 79f17cc..36d0c96 100644 --- a/src/libraries/Arduino/encoder_driver.h +++ b/src/libraries/Arduino/encoder_driver.h @@ -1,5 +1,5 @@ - #define LEFT_ENC_PIN_A PC4 - #define LEFT_ENC_PIN_B PC5 + #define LEFT_ENC_PIN_A PC2 + #define LEFT_ENC_PIN_B PC3 #define RIGHT_ENC_PIN_A PD2 #define RIGHT_ENC_PIN_B PD3 diff --git a/src/libraries/Arduino/encoder_driver.ino b/src/libraries/Arduino/encoder_driver.ino index 0acbf43..866dfaf 100644 --- a/src/libraries/Arduino/encoder_driver.ino +++ b/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)]; }