From 62e07ce32a957920d96ebe754a49232c1c51a044 Mon Sep 17 00:00:00 2001 From: Stedd Date: Sat, 21 Oct 2023 15:55:08 +0200 Subject: [PATCH] Balance controller is not working --- Balancebot.ino | 7 ++-- IMU.ino | 30 +++++++--------- motorControl.ino | 92 ++++++++++++++++++++++++++++++------------------ plot.ino | 55 ++++++++++++++++------------- 4 files changed, 104 insertions(+), 80 deletions(-) diff --git a/Balancebot.ino b/Balancebot.ino index b894d2c..19f254b 100644 --- a/Balancebot.ino +++ b/Balancebot.ino @@ -45,18 +45,19 @@ const char* _ps3Address = "18:5e:0f:92:00:6c"; void setup() { //Initialize serial - Serial.begin(57600); + Serial.begin(19200); delay(10); //Initialice I2C Wire.begin(IMU_I2C_SDA, IMU_I2C_SCL); - //delay(10); + delay(10); + //Initialize IMU Serial.println("Before IMU init"); IMU.init(); - //IMU.init(); Serial.println("After IMU init"); + delay(10); //Initialize encoder interrupts diff --git a/IMU.ino b/IMU.ino index 475b5e8..83ff38b 100644 --- a/IMU.ino +++ b/IMU.ino @@ -7,7 +7,7 @@ const int gyro_overflow_value = 4558; // 4096+512-50=4558 ? //IMU VARIABLES int ax, ay, az; int cx, cy, cz; -float gx, gy, gz; +int gx, gy, gz; float gt; float acc_pitch; float pitch_rate; @@ -16,30 +16,26 @@ float pitch_prev = 0; void readIMU() { - // Serial.println("ReadingIMU"); //Acceletometer - int* accelerometerReadings = IMU.readFromAccelerometer(); - ax = convertInt(IMU.accelerometer_x(accelerometerReadings), acc_overflow_value); - ay = convertInt(IMU.accelerometer_y(accelerometerReadings), acc_overflow_value); - az = convertInt(IMU.accelerometer_z(accelerometerReadings), acc_overflow_value); + ax = convertInt(IMU.accelerometer_x(IMU.readFromAccelerometer()), acc_overflow_value); + ay = convertInt(IMU.accelerometer_y(IMU.readFromAccelerometer()), acc_overflow_value); + az = convertInt(IMU.accelerometer_z(IMU.readFromAccelerometer()), acc_overflow_value); //Magnetometer - int* compassReadings = IMU.readFromCompass(); - cx = IMU.compass_x(compassReadings); - cy = IMU.compass_y(compassReadings); - cz = IMU.compass_z(compassReadings); + cx = IMU.compass_x(IMU.readFromCompass()); + cy = IMU.compass_y(IMU.readFromCompass()); + cz = IMU.compass_z(IMU.readFromCompass()); - // // Gyrocope - // float* gyroReadings = IMU.readGyro(); - // gx = convertInt(IMU.gyro_x(gyroReadings), gyro_overflow_value); // gx - Pitch rate - // gy = convertInt(IMU.gyro_y(gyroReadings), gyro_overflow_value); // gy - Roll rate - // gz = convertInt(IMU.gyro_z(gyroReadings), gyro_overflow_value); // gz - Yaw rate + // Gyrocope + gx = convertInt(IMU.gyro_x(IMU.readGyro()), gyro_overflow_value); // gx - Pitch rate + gy = convertInt(IMU.gyro_y(IMU.readGyro()), gyro_overflow_value); // gy - Roll rate + gz = convertInt(IMU.gyro_z(IMU.readGyro()), gyro_overflow_value); // gz - Yaw rate - // //Temperature sensor - // gt = IMU.temp(gyroReadings); + //Temperature sensor + gt = IMU.temp(IMU.readGyro()); // Pitch angle from accelerometer diff --git a/motorControl.ino b/motorControl.ino index 1869c87..75178a6 100644 --- a/motorControl.ino +++ b/motorControl.ino @@ -30,6 +30,7 @@ float OL_cont_out; float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup; float speedCmd1, speedCmd2; +bool balancingOn = false; //Matrices mtx_type motor_ang_vel[2][1]; @@ -47,61 +48,82 @@ void initMotors() { void motors() { + if (Ps3.data.button.cross) { + ResetIntegrators(); + balancingOn = true; + } - //Calculate wheel angular velocity - motor_ang_vel[0][0] = encoderReaderAngVel(m1Raw, m1RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); - motor_ang_vel[1][0] = encoderReaderAngVel(m2Raw, m2RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); + if (Ps3.data.button.circle) { + balancingOn = false; + } + + if (Ps3.data.button.triangle) { + ResetIntegrators(); + } + + if (balancingOn) { + + //Calculate wheel angular velocity + motor_ang_vel[0][0] = encoderReaderAngVel(m1Raw, m1RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); + motor_ang_vel[1][0] = encoderReaderAngVel(m2Raw, m2RawLast, motor_ang_vel[1][0], PULSES_PER_TURN, WHEEL_DIAMETER, dT_s, filter_gain); - //Calculate robot linear and angular velocity - Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix); + //Calculate robot linear and angular velocity + Matrix.Multiply((mtx_type*)inv_Kin, (mtx_type*)motor_ang_vel, 2, 2, 1, (mtx_type*)vel_Matrix); - //Get Control Commands - rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75); - rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35); + //Get Control Commands + rem_turn_speed_ref = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -3.75, 3.75); + rem_speed_ref = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -0.35, 0.35); - // Speed Controller - SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); + // Speed Controller + SC_cont_out = PController(rem_speed_ref, vel_Matrix[0][0], K_SC); - // Balance controller - // Outer loop - OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, K_OL); - // Inner loop - ref_IL = OL_cont_out; - act_IL = pitch_rate; - error_IL = ref_IL - act_IL; - iError_IL = iError_IL + (dT_s * (error_IL * I_IL) + (IL_anti_windup * ((1 / I_IL) + (1 / K_IL)))); - IL_cont_out = round((error_IL * K_IL) + iError_IL); + // Balance controller + // Outer loop + OL_cont_out = PController((BALANCE_POINT - SC_cont_out), pitch, K_OL); + // Inner loop + ref_IL = OL_cont_out; + act_IL = pitch_rate; + error_IL = ref_IL - act_IL; + iError_IL = iError_IL + (dT_s * (error_IL * I_IL) + (IL_anti_windup * ((1 / I_IL) + (1 / K_IL)))); + IL_cont_out = round((error_IL * K_IL) + iError_IL); - //Turn controller - TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[1][0], K_TC); + //Turn controller + TC_cont_out = PController(rem_turn_speed_ref, vel_Matrix[0][1], K_TC); + //Sum speed command for motors + M1_Speed_CMD = IL_cont_out - TC_cont_out; + M2_Speed_CMD = IL_cont_out + TC_cont_out; + //Motor control + IL_anti_windup = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); + IL_anti_windup = IL_anti_windup + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); + IL_anti_windup = IL_anti_windup / 2; - //Sum speed command for motors - M1_Speed_CMD = IL_cont_out - TC_cont_out; - M2_Speed_CMD = IL_cont_out + TC_cont_out; + } else { - //Sum speed command for motors - speedCmd1 = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -1.0, 1.0); - M1_Speed_CMD = MOTOR_SATURATION * speedCmd1; - motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); + //Sum speed command for motors + speedCmd1 = floatMap(Ps3.data.analog.stick.ry, -128.0, 127.0, -1.0, 1.0); + M1_Speed_CMD = MOTOR_SATURATION * speedCmd1; + motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); - speedCmd2 = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -1.0, 1.0); - M2_Speed_CMD = MOTOR_SATURATION * speedCmd2; - motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); + speedCmd2 = floatMap(Ps3.data.analog.stick.ly, -128.0, 127.0, -1.0, 1.0); + M2_Speed_CMD = MOTOR_SATURATION * speedCmd2; + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); + } - //Motor control - // IL_anti_windup = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); - // IL_anti_windup = IL_anti_windup + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG); - // IL_anti_windup = IL_anti_windup / 2; //Update variables for next scan cycle m1RawLast = m1Raw; m2RawLast = m2Raw; } +void ResetIntegrators() { + iError_IL = 0.0; + IL_anti_windup = 0.0; +} + float PController(float ref_, float act_, float k_) { return (ref_ - act_) * k_; } diff --git a/plot.ino b/plot.ino index 64664cf..b71dd6d 100644 --- a/plot.ino +++ b/plot.ino @@ -8,18 +8,20 @@ void plot() { // Serial.print(" "); // IMU - Serial.print ( "Pitch:" ); - Serial.println ( 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); + Serial.print("Pitch:"); + Serial.println(pitch); + + // Serial.print("Accelerometer_Pitch:"); + // Serial.println(acc_pitch); + + Serial.print("RollRate"); + Serial.println(gz); + + // Serial.print(","); + // Serial.println(gt); + + // Serial.print(" "); + // Serial.println(acc_pitch); // Remote control @@ -43,20 +45,23 @@ void plot() { // Serial.println(m2Raw); // Motors - // Serial.print("SpeedControllerOut:"); - // Serial.print(SC_cont_out); - // Serial.print(" "); - // Serial.print("BalanceOLControllerOut:"); - // Serial.print(OL_cont_out); - // Serial.print(" "); - // Serial.print("BalanceILControllerOut:"); - // Serial.print(IL_cont_out); - // Serial.print(" "); + Serial.print("SpeedControllerOut:"); + Serial.println(SC_cont_out); + + Serial.print("BalanceOLControllerOut:"); + Serial.println(OL_cont_out); + + Serial.print("BalanceILControllerOut:"); + Serial.println(IL_cont_out); + + Serial.print("AntiWindup:"); + Serial.println(IL_anti_windup); + // Serial.print("SpeedCmd1:"); // Serial.println(speedCmd1); // Serial.print(" "); // Serial.print("M1_CMD:"); - // Serial.print(M1_Speed_CMD); + // Serial.println(M1_Speed_CMD); // Serial.print(" "); // Serial.print("SpeedCmd2:"); // Serial.println(speedCmd2); @@ -65,13 +70,13 @@ void plot() { // Serial.println(M2_Speed_CMD); // Serial.print("M1_Ang_Vel:"); - // Serial.print(motor_ang_vel[0][0]); + // Serial.println(motor_ang_vel[0][0]); // Serial.print(" "); // Serial.print("M2_Ang_Vel:"); - // Serial.print(motor_ang_vel[0][1]); + // Serial.println(motor_ang_vel[0][1]); // Serial.print(" "); // Serial.print("botLinVel:"); - // Serial.print(vel_Matrix[0][0]); + // Serial.println(vel_Matrix[0][0]); // Serial.print(" "); // Serial.print("botAngVel:"); // Serial.println(vel_Matrix[1][0]);