diff --git a/Main/IMU.ino b/Main/IMU.ino index 28627c5..a7da9f2 100644 --- a/Main/IMU.ino +++ b/Main/IMU.ino @@ -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; diff --git a/Main/Main.ino b/Main/Main.ino index d37df56..b63a292 100644 --- a/Main/Main.ino +++ b/Main/Main.ino @@ -1,6 +1,7 @@ //Import #include #include +#include //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 diff --git a/Main/motorControl.ino b/Main/motorControl.ino index c3bd071..fe3dcbe 100644 --- a/Main/motorControl.ino +++ b/Main/motorControl.ino @@ -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); + } }