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

Merge remote-tracking branch 'refs/remotes/origin/Linear-algebra-tests'

This commit is contained in:
Stedd 2019-12-22 22:35:07 +01:00
parent c546c757c9 0836937e3f
commit 4129c92d15
3 changed files with 130 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Main/IMU.ino
View File

@ -54,7 +54,7 @@ void readIMU() {
//Complementary filter //Complementary filter
dAngle = pitch_rate * dT * pow(10, -6); dAngle = pitch_rate * dT_s;
pitch = acc_pitch * (1 - alpha) + (dAngle + pitch_prev * alpha); pitch = acc_pitch * (1 - alpha) + (dAngle + pitch_prev * alpha);
pitch_prev = pitch; pitch_prev = pitch;

25
Main/Main.ino
View File

@ -1,6 +1,7 @@
//Import //Import
#include <GY_85.h> #include <GY_85.h>
#include <Wire.h> #include <Wire.h>
#include <MatrixMath.h>
//Declare library objects //Declare library objects
@ -24,19 +25,25 @@ const byte IMU_I2C_SDA = 27;
unsigned long tNow = micros(); unsigned long tNow = micros();
unsigned long tLast = micros() + 13000; unsigned long tLast = micros() + 13000;
int dT = 0; int dT = 0;
float dT_s = 0.0;
//Motor variables //Motor variables
const int PWM_CYCLE = 12000; const int PWM_CYCLE = 12000;
const byte PWM_RESOLUTION = 12; const byte PWM_RESOLUTION = 12;
//Encoders variables //Encoders variables
long int m1Raw, m1RawLast; long int m1Raw, m1RawLast;
long int m2Raw, m2RawLast; long int m2Raw, m2RawLast;
volatile bool M1_A_state; volatile bool M1_A_state, M1_B_state;
volatile bool M1_B_state; volatile bool M2_A_state, M2_B_state;
volatile bool M2_A_state;
volatile bool M2_B_state;
//Matrices
mtx_type motor_ang_vel [2][1];
mtx_type vel_Matrix [2][1];
mtx_type inv_Kin [2][2];
//Interrupt routines //Interrupt routines
@ -192,12 +199,22 @@ void setup() {
ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION); ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION);
ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION); ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION);
//Initialize differential drive inverse kinematics
initMotors();
} }
void loop() { void loop() {
//Update time variables //Update time variables
tNow = micros(); tNow = micros();
dT = tNow - tLast; //[Cycle time in microseconds] dT = tNow - tLast; //[Cycle time in microseconds]
dT_s = dT * pow(10,-6); //[Cycle time in seconds]
// Serial.print("dT:");
// Serial.print(dT);
// Serial.print(" ");
// Serial.print("dT_s:");
// Serial.println(dT_s);
//Get sensor data //Get sensor data

142
Main/motorControl.ino
View File

@ -1,37 +1,64 @@
//Constants //Constants
const int MOTOR_SATURATION = round(pow(2, PWM_RESOLUTION)); const int MOTOR_SATURATION = round(pow(2, PWM_RESOLUTION));
const float WHEEL_DIAMETER = 0.067708; const float BASE_WIDTH = 0.1837;
const float WHEEL_DIAMETER = 0.0677;
const float PULSES_PER_TURN = 1320.0; const float PULSES_PER_TURN = 1320.0;
const float BALANCE_POINT = -0.05; const float BALANCE_POINT = 0.05;
const float SPEED_REFERENCE = 0.0; const float SPEED_REFERENCE = 0.0;
const float DEADBAND_M1_POS = 90.0; const float DEADBAND_M1_POS = 90.0;
const float DEADBAND_M1_NEG = 90.0; const float DEADBAND_M1_NEG = 90.0;
const float DEADBAND_M2_POS = 90.0; const float DEADBAND_M2_POS = 90.0;
const float DEADBAND_M2_NEG = 90.0; const float DEADBAND_M2_NEG = 90.0;
//Tuning //Tuning
const float K_SC = 15.0; const float K_SC = 20.0;
const float K_OL = 13.0; const float K_OL = 13.0;
const float K_IL = 80.0; const float K_IL = 90.0;
const float I_IL = 5.5; const float I_IL = 5.5;
const float filter_gain = 15.0; const float filter_gain = 15.0;
//Help variables //Help variables
float M1_Lin_Vel, M2_Lin_Vel; float M1_Lin_Vel, M2_Lin_Vel;
float M1_Ang_Vel, M2_Ang_Vel;
float botLinVel , botAngVel ;
int Speed_CMD, M1_Speed_CMD, M2_Speed_CMD; int Speed_CMD, M1_Speed_CMD, M2_Speed_CMD;
float ref_SC, act_SC, error_SC, SC_cont_out; float ref_SC, act_SC, error_SC, SC_cont_out;
float ref_OL, act_OL, error_OL, OL_cont_out; float ref_OL, act_OL, error_OL, OL_cont_out;
float ref_IL, act_IL, error_IL, iError_IL; float ref_IL, act_IL, error_IL, iError_IL;
void initMotors() {
// float temp[] = {WHEEL_DIAMETER / 4, WHEEL_DIAMETER / 4, (WHEEL_DIAMETER / 2) / BASE_WIDTH, -(WHEEL_DIAMETER / 2) / BASE_WIDTH};
// int k = 0;
// for (int i = 0; i < 2; i++)
// {
// for (int j = 0; j < 2; j++)
// {
// inv_Kin[i][j] = temp[k];
// k = k + 1;
// }
// }
inv_Kin[0][0] = WHEEL_DIAMETER / 4;
inv_Kin[1][0] = (WHEEL_DIAMETER / 2) / BASE_WIDTH;
inv_Kin[0][1] = WHEEL_DIAMETER / 4;
inv_Kin[1][1] = -(WHEEL_DIAMETER / 2) / BASE_WIDTH;
Matrix.Print((mtx_type*)inv_Kin, 2, 2, "Inverse kinematic matrix");
}
void motors() { void motors() {
// Speed Controller // Speed Controller
ref_SC = SPEED_REFERENCE; ref_SC = SPEED_REFERENCE;
act_SC = (M1_Lin_Vel + M2_Lin_Vel) / 2; act_SC = vel_Matrix[0][0];
error_SC = ref_SC - act_SC; error_SC = ref_SC - act_SC;
SC_cont_out = (error_SC * K_SC); SC_cont_out = error_SC * K_SC;
// Balance controller // Balance controller
@ -39,23 +66,41 @@ void motors() {
ref_OL = BALANCE_POINT - SC_cont_out; ref_OL = BALANCE_POINT - SC_cont_out;
act_OL = pitch; act_OL = pitch;
error_OL = ref_OL - act_OL; error_OL = ref_OL - act_OL;
OL_cont_out = (error_OL * K_OL); OL_cont_out = error_OL * K_OL;
// Inner loop // Inner loop
ref_IL = OL_cont_out; ref_IL = OL_cont_out;
act_IL = pitch_rate; act_IL = pitch_rate;
error_IL = ref_IL - act_IL; error_IL = ref_IL - act_IL;
iError_IL = iError_IL + (error_IL * dT * pow(10, -6) * I_IL); iError_IL = iError_IL + (error_IL * dT_s * I_IL);
Speed_CMD = round((error_IL * K_IL) + iError_IL); Speed_CMD = round((error_IL * K_IL) + iError_IL);
M1_Speed_CMD = Speed_CMD; M1_Speed_CMD = Speed_CMD;
M2_Speed_CMD = Speed_CMD; M2_Speed_CMD = Speed_CMD;
// M1_Speed_CMD = 0; // M1_Speed_CMD = 500;
// M2_Speed_CMD = 0; // M2_Speed_CMD = 500;
//Calculate speed from encoders //Calculate speed from encoders
M1_Lin_Vel = encoderReader(m1Raw, m1RawLast, M1_Lin_Vel, PULSES_PER_TURN, WHEEL_DIAMETER, dT, filter_gain); M1_Lin_Vel = encoderReaderLinVel(m1Raw, m1RawLast, M1_Lin_Vel, PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain);
M2_Lin_Vel = encoderReader(m2Raw, m2RawLast, M2_Lin_Vel, PULSES_PER_TURN, WHEEL_DIAMETER, dT, filter_gain); M2_Lin_Vel = encoderReaderLinVel(m2Raw, m2RawLast, M2_Lin_Vel, PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain);
M1_Ang_Vel = encoderReaderAngVel(m1Raw, m1RawLast, M1_Ang_Vel, PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain);
M2_Ang_Vel = encoderReaderAngVel(m2Raw, m2RawLast, M2_Ang_Vel, PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain);
motor_ang_vel[0][0] = M1_Ang_Vel;
motor_ang_vel[1][0] = M2_Ang_Vel;
//void MatrixMath::Multiply(mtx_type* A, mtx_type* B, int m, int p, int n, mtx_type* C)
//{
// A = input matrix (m x p)
// B = input matrix (p x n)
// m = number of rows in A
// p = number of columns in A = number of rows in B
// n = number of columns in B
// C = output matrix = A*B (m x n)
Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix);
//Motor control //Motor control
@ -64,26 +109,53 @@ void motors() {
// Serial plotter // Serial plotter
Serial.print("Balance_Point:"); // Serial.print("Balance_Point:");
Serial.print(ref_OL); // Serial.print(ref_OL);
// Serial.print(" ");
// Serial.print("Pitch_Angle:");
// Serial.print(act_OL);
// Serial.print(" ");
// Serial.print("Speed_CMD:");
// Serial.println(Speed_CMD * (100.0 / 4096.0));
Serial.print("M1_Ang_Vel:");
Serial.print(M1_Ang_Vel);
Serial.print(" "); Serial.print(" ");
Serial.print("Pitch_Angle:"); Serial.print("M2_Ang_Vel:");
Serial.print(act_OL); Serial.print(M2_Ang_Vel);
Serial.print(" "); Serial.print(" ");
Serial.print("Speed_CMD:"); Serial.print("botLinVel:");
Serial.println(Speed_CMD * (100.0 / 4096.0)); Serial.print(vel_Matrix[0][0]);
Serial.print(" ");
Serial.print("botAngVel:");
Serial.println(vel_Matrix[1][0]);
//Update variables for next scan cycle //Update variables for next scan cycle
m1RawLast = m1Raw; m1RawLast = m1Raw;
m2RawLast = m2Raw; m2RawLast = m2Raw;
// Serial.print("m1Raw:");
// Serial.print(m1Raw);
// Serial.print(" ");
// Serial.print("m2Raw:");
// Serial.println(m2Raw);
} }
float encoderReader(int encRaw, int encRawLast, float lin_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, int dT_, float filt_gain_ ) { float encoderReaderLinVel(int encRaw, int encRawLast, float lin_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, float dT_, float filt_gain_ ) {
float dEnc_ = encRaw - encRawLast; //[Number of encoder pulses this cycle] float dEnc_ = encRaw - encRawLast; //[Number of encoder pulses this cycle]
float dTurn_ = dEnc_ / pulses_per_turn_; //[Amount wheel turned this cycle. 1 = full rotation] float dTurn_ = dEnc_ / pulses_per_turn_; //[Amount wheel turned this cycle. 1 = full rotation]
float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_ * 0.000001); float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_);
return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * 0.000001 * filt_gain_); return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * filt_gain_);
}
float encoderReaderAngVel(int encRaw, int encRawLast, float ang_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, float dT_, float filt_gain_ ) {
float dEnc_ = encRaw - encRawLast; //[Number of encoder pulses this cycle]
float dTurn_ = dEnc_ / pulses_per_turn_; //[Amount wheel turned this cycle. 1 = full rotation]
float ang_vel_ = (dTurn_ * 2 * PI) / (dT_);
return ang_vel_filtered_ + ((ang_vel_ - ang_vel_filtered_) * dT_ * filt_gain_);
} }
void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, float dbNeg_) { void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, float dbNeg_) {
@ -92,32 +164,26 @@ void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, flo
byte ch2 = motorID * 2; byte ch2 = motorID * 2;
// Speed command saturation
if (speedCMD_ > saturation) {
speedCMD_ = saturation;
}
else if (speedCMD_ < -saturation) {
speedCMD_ = -saturation;
}
//Deadband //Deadband
else if (speedCMD_ > 0 && speedCMD_ < dbPos_) { if (speedCMD_ > 0 && speedCMD_ < dbPos_) {
speedCMD_ = dbPos_; speedCMD_ = dbPos_;
} }
else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) { else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) {
speedCMD_ = -dbNeg_; speedCMD_ = -dbNeg_;
} }
//Zero speed if input = 0 // Speed command saturation
else if (speedCMD_ == 0) { else if (speedCMD_ > saturation) {
speedCMD_ = 0; speedCMD_ = saturation;
} }
else if (speedCMD_ < -saturation) {
speedCMD_ = -saturation;
}
else { else {
speedCMD_ = speedCMD_; speedCMD_ = speedCMD_;
} }
//Apply speed command to PWM output //Apply speed command to PWM output
if (speedCMD_ > 0) { if (speedCMD_ > 0) {
ledcWrite(ch1, 0); ledcWrite(ch1, 0);
@ -127,4 +193,8 @@ void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, flo
ledcWrite(ch1, -1 * speedCMD_); ledcWrite(ch1, -1 * speedCMD_);
ledcWrite(ch2, 0); ledcWrite(ch2, 0);
} }
else if (speedCMD_ == 0) {
ledcWrite(ch1, 0);
ledcWrite(ch2, 0);
}
} }