Stedd
/
BalanceBot
1
0
Fork 0
Code Issues 6 Pull Requests Packages Projects 1 Releases Wiki Activity

Added gyroscope + complementary filter for pitch angle + lowpass filter for motors

This commit is contained in:
Stedd 2019-12-20 18:31:12 +01:00
parent 5f57c6cb5c
commit 36254d96e6
3 changed files with 130 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
Main/IMU.ino
View File

@ -1,9 +1,22 @@
//IMU variables
int ax_, ay_, az_;
int ax, ay, az;
int cx, cy, cz;
float gx, gy, gz, gt;
float acc_pitch;
//CONSTANTS
const float alpha = 0.95;
const int acc_overflow_value = 65535;
const int gyro_overflow_value = 4558; // 4096+512-50=4558 ?
//IMU VARIABLES
int ax_, ay_, az_;
int ax, ay, az;
int cx_, cy_, cz_;
int gx_, gy_, gz_;
int gx, gy, gz;
//float gt;
float acc_pitch;
float pitch_rate;
float dAngle, estAngle;
float pitch = 0;
float pitch_prev = 0;
void readIMU() {
ax_ = IMU.accelerometer_x( IMU.readFromAccelerometer() );
@ -14,51 +27,92 @@ void readIMU() {
// cy = IMU.compass_y( IMU.readFromCompass() );
// cz = IMU.compass_z( IMU.readFromCompass() );
gx = IMU.gyro_x( IMU.readGyro() );
gy = IMU.gyro_y( IMU.readGyro() );
gz = IMU.gyro_z( IMU.readGyro() );
gx_ = IMU.gyro_x( IMU.readGyro() );
gy_ = IMU.gyro_y( IMU.readGyro() );
gz_ = IMU.gyro_z( IMU.readGyro() );
// gt = IMU.temp ( IMU.readGyro() );
//For some reason the ints in the example behaves as unsigned int.. Maybe look at the .cpp code, might be something there, if not. This works OK.
if (ax_ > 60000) {
ax = (ax_ - 65535);
//Convert accelerometer
if (ax_ > 32768) {
ax = (ax_ - acc_overflow_value);
}
else {
ax = ax_;
}
if (ay_ > 60000) {
ay = (ay_ - 65535);
if (ay_ > 32768) {
ay = (ay_ - acc_overflow_value);
}
else {
ay = ay_;
}
if (az_ > 60000) {
az = (az_ - 65535);
if (az_ > 32768) {
az = (az_ - acc_overflow_value);
}
else {
az = az_;
}
acc_pitch = -1*atan2(ax,sqrt((pow(ay,2)+pow(az,2))))*180/3.14;
//Convert gyro
// Gyroscope coordinate system
// gx - Pitch rate
// gy - Roll rate
// gz - Yaw rate
// Gyro is calibrated for +-2000deg/s
// Conversion is happening in GY_85.h line 48-50
if (gx_ > 2279) {
gx = (gx_ - gyro_overflow_value);
}
else {
gx = gx_;
}
if (gy_ > 2279) {
gy = (gy_ - gyro_overflow_value);
}
else {
gy = gy_;
}
if (gz_ > 2279) {
gz = (gz_ - gyro_overflow_value);
}
else {
gz = gz_;
}
//Serial plotter
// map(ax,-140,140,-1,1)
// map(ay,-140,140,-1,1)
// map(az,-140,140,-1,1)
// Serial.print ( " x:" );
// Serial.print ( ax );
// Serial.print ( "," );
// Serial.print ( ay );
// Serial.print ( "," );
// Serial.println ( az );
// Serial.print ( "," );
// Serial.println ( acc_pitch);
// Pitch angle from accelerometer
acc_pitch = -1 * atan2(ax, sqrt((pow(ay, 2) + pow(az, 2)))) * 180.0 / PI;
//Pitch rate from gyroscope
pitch_rate = -gx;
//Complementary filter
dAngle = pitch_rate * dT * pow(10, -6);
pitch = acc_pitch * (1 - alpha) + (dAngle + pitch_prev * alpha);
pitch_prev = pitch;
//Serial plotter
// Serial.print ( "Pitch:" );
// Serial.print ( pitch );
// Serial.print (" ");
// Serial.print ( "Accelerometer_Pitch:" );
// Serial.print ( acc_pitch );
// Serial.print (" ");
// Serial.print ( "," );
// Serial.println ( gz );
// Serial.print ( "," );
// Serial.println ( gt );
// Serial.print ( "," );
// Serial.println ( acc_pitch);
}

13
Main/Main.ino
View File

@ -1,3 +1,5 @@
#include <GY_85.h>
//Import
#include "GY_85.h"
#include <Wire.h>
@ -22,6 +24,8 @@ const byte IMU_I2C_SDA = 27;
//System variables
unsigned long tNow = micros();
unsigned long tLast = micros()+13000;
int dT = 0;
//Motor variables
@ -156,6 +160,8 @@ void setup() {
//Initialize IMU
IMU.init();
//Might need some logic here to mke sure the gyro is calibrated correctly, or hardcode the values...
IMU.GyroCalibrate();
delay(10);
//Initialize encoder interrupts
@ -188,13 +194,14 @@ void setup() {
ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION);
ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION);
Serial.println("Reference,Actual,SpeedCommand");
// Serial.println("Reference,Actual,SpeedCommand");
}
void loop() {
//Update system variables
tNow = micros();
tNow = micros();
dT = tNow - tLast; //[Cycle time in microseconds]
//// //Only print encoder value if value changed since last print
// if (m1Raw != m1RawLast) {
@ -212,7 +219,7 @@ void loop() {
//Act
motorControl();
tLast = tNow;
//Delay
delay(5); // only read every 0,5 seconds, 10ms for 100Hz, 20ms for 50Hz
}

59
Main/motorControl.ino
View File

@ -1,16 +1,17 @@
//Constants
const float wheel_diameter = 0.067708;
const float pulses_per_turn = 1320.0;
const float filt_gain = 15;
//Tuning
const float K = 2.0;
const float I = 10.0;
const float K = 3.5;
const float I = 7.5;
//Help variables
int dEnc, dT;
float dTurn, ang_vel, lin_vel, ang_vel_filtered, lin_vel_filtered;
unsigned long tLast;
int dEnc;
float dTurn, ang_vel, lin_vel, lin_vel_prev, ang_vel_filtered, lin_vel_filtered;
float dVel, diff;
float speed_setp, referenceSpeed, actualSpeed;
float speed_setp, referenceSpeed, actualSpeed, actualSpeedFiltered;
float error, iError;
int speedCMD;
@ -18,9 +19,10 @@ int speedCMD;
void motorControl() {
//Speed Controller
referenceSpeed = acc_pitch * ((4096.0) / (90.0));
referenceSpeed = pitch * ((4096.0) / (90.0));
actualSpeed = lin_vel * 4096.0;
error = referenceSpeed - actualSpeed;
actualSpeedFiltered = lin_vel_filtered * 4096.0;
error = referenceSpeed - actualSpeedFiltered;
iError = iError + (error * dT * pow(10, -6) * I);
speedCMD = round((error * K) + iError);
@ -46,13 +48,7 @@ void motorControl() {
ledcWrite(1, -1 * speedCMD);
ledcWrite(2, 0);
}
// Serial.print (acc_pitch);
// Serial.print ( "," );
// Serial.println(speed_setp);
//
// ledcWrite(1, 0);
// ledcWrite(2, 0);
//Motor 2
@ -60,26 +56,45 @@ void motorControl() {
// ledcWrite(4, 255);
//No speed command
// ledcWrite(1, 0);
// ledcWrite(2, 0);
ledcWrite(3, 0);
ledcWrite(4, 0);
//Calculate speed from encoders
dEnc = m1Raw - m1RawLast; //[Number of encoder pulses this cycle]
dT = tNow - tLast; //[Cycle time in microseconds]
dTurn = dEnc / pulses_per_turn; //[Amount wheel turned this cycle. 1 = full rotation]
ang_vel = (dTurn * 2 * PI) / (dT * 0.000001);
lin_vel = (dTurn * wheel_diameter * PI) / (dT * 0.000001);
Serial.print(referenceSpeed);
Serial.print ( "," );
Serial.print(actualSpeed);
Serial.print ( "," );
Serial.println(speedCMD);
// Serial.print ( "," );
// Lowpass filter
// lin_vel_filtered = lin_vel_filtered + ((lin_vel - lin_vel_filtered)*(1-abs(diff))* dT * pow(10, -6)*filt_gain);
lin_vel_filtered = lin_vel_filtered + ((lin_vel - lin_vel_filtered) * dT * pow(10, -6) * filt_gain);
// Serial plotter
Serial.print("referenceSpeed:");
Serial.print(referenceSpeed * (100.0 / 4096.0));
Serial.print(" ");
// Serial.print("actualSpeed:");
// Serial.print(actualSpeed * (100.0 / 4096.0));
// Serial.print(" ");
Serial.print("actualSpeedFiltered:");
Serial.print(actualSpeedFiltered * (100.0 / 4096.0));
Serial.print(" ");
Serial.print("speedCMD:");
Serial.println(speedCMD * (100.0 / 4096.0));
// Serial.print ( "," );
// Serial.println(lin_vel);
// Serial.print ( "," );
//Update variables for next scan cycle
m1RawLast = m1Raw;
tLast = tNow;
}