Merge remote-tracking branch 'refs/remotes/origin/Linear-algebra-tests'
This commit is contained in:
Stedd
commit
4129c92d15
|
|
@ -54,7 +54,7 @@ void readIMU() {
|
|||
|
||||
|
||||
//Complementary filter
|
||||
dAngle = pitch_rate * dT * pow(10, -6);
|
||||
dAngle = pitch_rate * dT_s;
|
||||
pitch = acc_pitch * (1 - alpha) + (dAngle + pitch_prev * alpha);
|
||||
pitch_prev = pitch;
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
//Import
|
||||
#include <GY_85.h>
|
||||
#include <Wire.h>
|
||||
#include <MatrixMath.h>
|
||||
|
||||
|
||||
//Declare library objects
|
||||
|
|
@ -24,19 +25,25 @@ const byte IMU_I2C_SDA = 27;
|
|||
unsigned long tNow = micros();
|
||||
unsigned long tLast = micros() + 13000;
|
||||
int dT = 0;
|
||||
float dT_s = 0.0;
|
||||
|
||||
|
||||
//Motor variables
|
||||
const int PWM_CYCLE = 12000;
|
||||
const byte PWM_RESOLUTION = 12;
|
||||
|
||||
|
||||
//Encoders variables
|
||||
long int m1Raw, m1RawLast;
|
||||
long int m2Raw, m2RawLast;
|
||||
volatile bool M1_A_state;
|
||||
volatile bool M1_B_state;
|
||||
volatile bool M2_A_state;
|
||||
volatile bool M2_B_state;
|
||||
volatile bool M1_A_state, M1_B_state;
|
||||
volatile bool M2_A_state, 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
|
||||
|
|
@ -192,12 +199,22 @@ void setup() {
|
|||
ledcSetup(3, PWM_CYCLE, PWM_RESOLUTION);
|
||||
ledcSetup(4, PWM_CYCLE, PWM_RESOLUTION);
|
||||
|
||||
//Initialize differential drive inverse kinematics
|
||||
initMotors();
|
||||
|
||||
}
|
||||
|
||||
void loop() {
|
||||
//Update time variables
|
||||
tNow = micros();
|
||||
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
|
||||
|
|
|
|||
|
|
@ -1,37 +1,64 @@
|
|||
//Constants
|
||||
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 BALANCE_POINT = -0.05;
|
||||
const float BALANCE_POINT = 0.05;
|
||||
const float SPEED_REFERENCE = 0.0;
|
||||
const float DEADBAND_M1_POS = 90.0;
|
||||
const float DEADBAND_M1_NEG = 90.0;
|
||||
const float DEADBAND_M2_POS = 90.0;
|
||||
const float DEADBAND_M2_NEG = 90.0;
|
||||
|
||||
|
||||
//Tuning
|
||||
const float K_SC = 15.0;
|
||||
const float K_SC = 20.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 filter_gain = 15.0;
|
||||
|
||||
|
||||
//Help variables
|
||||
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;
|
||||
|
||||
float ref_SC, act_SC, error_SC, SC_cont_out;
|
||||
float ref_OL, act_OL, error_OL, OL_cont_out;
|
||||
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() {
|
||||
|
||||
|
||||
// Speed Controller
|
||||
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;
|
||||
SC_cont_out = (error_SC * K_SC);
|
||||
SC_cont_out = error_SC * K_SC;
|
||||
|
||||
|
||||
// Balance controller
|
||||
|
|
@ -39,23 +66,41 @@ void motors() {
|
|||
ref_OL = BALANCE_POINT - SC_cont_out;
|
||||
act_OL = pitch;
|
||||
error_OL = ref_OL - act_OL;
|
||||
OL_cont_out = (error_OL * K_OL);
|
||||
OL_cont_out = error_OL * K_OL;
|
||||
// Inner loop
|
||||
ref_IL = OL_cont_out;
|
||||
act_IL = pitch_rate;
|
||||
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);
|
||||
|
||||
M1_Speed_CMD = Speed_CMD;
|
||||
M2_Speed_CMD = Speed_CMD;
|
||||
|
||||
// M1_Speed_CMD = 0;
|
||||
// M2_Speed_CMD = 0;
|
||||
// M1_Speed_CMD = 500;
|
||||
// M2_Speed_CMD = 500;
|
||||
|
||||
//Calculate speed from encoders
|
||||
M1_Lin_Vel = encoderReader(m1Raw, m1RawLast, M1_Lin_Vel, PULSES_PER_TURN, WHEEL_DIAMETER, dT, filter_gain);
|
||||
M2_Lin_Vel = encoderReader(m2Raw, m2RawLast, M2_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 = 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
|
||||
|
|
@ -64,26 +109,53 @@ void motors() {
|
|||
|
||||
|
||||
// Serial plotter
|
||||
Serial.print("Balance_Point:");
|
||||
Serial.print(ref_OL);
|
||||
// Serial.print("Balance_Point:");
|
||||
// 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("Pitch_Angle:");
|
||||
Serial.print(act_OL);
|
||||
Serial.print("M2_Ang_Vel:");
|
||||
Serial.print(M2_Ang_Vel);
|
||||
Serial.print(" ");
|
||||
Serial.print("Speed_CMD:");
|
||||
Serial.println(Speed_CMD * (100.0 / 4096.0));
|
||||
Serial.print("botLinVel:");
|
||||
Serial.print(vel_Matrix[0][0]);
|
||||
Serial.print(" ");
|
||||
Serial.print("botAngVel:");
|
||||
Serial.println(vel_Matrix[1][0]);
|
||||
|
||||
|
||||
//Update variables for next scan cycle
|
||||
m1RawLast = m1Raw;
|
||||
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 dEnc_ = encRaw - encRawLast; //[Number of encoder pulses this cycle]
|
||||
float dTurn_ = dEnc_ / pulses_per_turn_; //[Amount wheel turned this cycle. 1 = full rotation]
|
||||
float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_ * 0.000001);
|
||||
return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * 0.000001 * 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 dTurn_ = dEnc_ / pulses_per_turn_; //[Amount wheel turned this cycle. 1 = full rotation]
|
||||
float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_);
|
||||
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_) {
|
||||
|
|
@ -92,32 +164,26 @@ void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, flo
|
|||
byte ch2 = motorID * 2;
|
||||
|
||||
|
||||
// Speed command saturation
|
||||
if (speedCMD_ > saturation) {
|
||||
speedCMD_ = saturation;
|
||||
}
|
||||
else if (speedCMD_ < -saturation) {
|
||||
speedCMD_ = -saturation;
|
||||
}
|
||||
|
||||
|
||||
//Deadband
|
||||
else if (speedCMD_ > 0 && speedCMD_ < dbPos_) {
|
||||
if (speedCMD_ > 0 && speedCMD_ < dbPos_) {
|
||||
speedCMD_ = dbPos_;
|
||||
}
|
||||
else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) {
|
||||
speedCMD_ = -dbNeg_;
|
||||
}
|
||||
|
||||
//Zero speed if input = 0
|
||||
else if (speedCMD_ == 0) {
|
||||
speedCMD_ = 0;
|
||||
// Speed command saturation
|
||||
else if (speedCMD_ > saturation) {
|
||||
speedCMD_ = saturation;
|
||||
}
|
||||
else if (speedCMD_ < -saturation) {
|
||||
speedCMD_ = -saturation;
|
||||
}
|
||||
|
||||
else {
|
||||
speedCMD_ = speedCMD_;
|
||||
}
|
||||
|
||||
|
||||
//Apply speed command to PWM output
|
||||
if (speedCMD_ > 0) {
|
||||
ledcWrite(ch1, 0);
|
||||
|
|
@ -127,4 +193,8 @@ void motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, flo
|
|||
ledcWrite(ch1, -1 * speedCMD_);
|
||||
ledcWrite(ch2, 0);
|
||||
}
|
||||
else if (speedCMD_ == 0) {
|
||||
ledcWrite(ch1, 0);
|
||||
ledcWrite(ch2, 0);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue