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

Moved variables to main file to prevent scope errors

This commit is contained in:
Stedd 2023-10-22 14:15:20 +02:00
parent 42d8d8b70a
commit 88676bb66e
4 changed files with 61 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
Balancebot.ino
View File

@ -29,12 +29,7 @@ int dT = 0;
float dT_s = 0.0;
//Motor variables
const int PWM_CYCLE = 12000;
const byte PWM_RES = 12;
//Encoders variables
//Encoder variables
long int m1Raw, m1RawLast;
long int m2Raw, m2RawLast;
volatile bool M1_A_state, M1_B_state;
@ -45,6 +40,66 @@ volatile bool M2_A_state, M2_B_state;
const char* _ps3Address = "18:5e:0f:92:00:6c";
//MotorControl Constants
const int PWM_CYCLE = 12000;
const byte PWM_RES = 12;
const int MOTOR_SATURATION = round(pow(2, PWM_RES));
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 SPEED_REF = 0.00;
const float TURN_SPEED_REF = 0.00;
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;
//MotorControl Tuning
const float gainScale = 1;
const float K_SC = 18.5 * gainScale; //Speed controller gain
const float K_TC = 90.0 * gainScale; //Turn controller gain
const float K_OL = 13.0 * gainScale; //Outer loop balance controller gain
const float K_IL = 72.0 * gainScale; //Inner loop balance controller gain
const float I_IL = 80.0 * gainScale; //Inner loop balance controller Igain
const float filter_gain = 16.0; //Motor speed LPF gain
//MotorControl Help variables
int M1_Speed_CMD, M2_Speed_CMD;
float rem_speed_ref, rem_turn_speed_ref;
float SC_cont_out;
float TC_cont_out;
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;
//MotorControl Matrices
mtx_type motor_ang_vel[2][1];
mtx_type vel_Matrix[2][1];
mtx_type inv_Kin[2][2];
//IMU 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 cx, cy, cz;
int gx, gy, gz;
float gt;
float acc_pitch;
float pitch_rate;
float pitch = 0;
float pitch_prev = 0;
//UDP variables
uint8_t data[30 * 4];

17
IMU.ino
View File

@ -1,20 +1,3 @@
//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 cx, cy, cz;
int gx, gy, gz;
float gt;
float acc_pitch;
float pitch_rate;
float pitch = 0;
float pitch_prev = 0;
void readIMU() {
//Acceletometer
ax = convertInt(IMU.accelerometer_x(IMU.readFromAccelerometer()), acc_overflow_value);

1
UDP.ino
View File

@ -30,7 +30,6 @@ void ConnectToWiFi() {
}
void PackUdpData() {
int i = 0;
data[i] = watchdog++;
data[i += 1] = balancingOn << 1;

40
motorControl.ino
View File

@ -1,43 +1,3 @@
//Constants
const int MOTOR_SATURATION = round(pow(2, PWM_RES));
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 SPEED_REF = 0.00;
const float TURN_SPEED_REF = 0.00;
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 gainScale = 1;
const float K_SC = 18.5 * gainScale; //Speed controller gain
const float K_TC = 90.0 * gainScale; //Turn controller gain
const float K_OL = 13.0 * gainScale; //Outer loop balance controller gain
const float K_IL = 72.0 * gainScale; //Inner loop balance controller gain
const float I_IL = 80.0 * gainScale; //Inner loop balance controller Igain
const float filter_gain = 16.0; //Motor speed LPF gain
//Help variables
int M1_Speed_CMD, M2_Speed_CMD;
float rem_speed_ref, rem_turn_speed_ref;
float SC_cont_out;
float TC_cont_out;
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];
mtx_type vel_Matrix[2][1];
mtx_type inv_Kin[2][2];
void initMotors() {
// Inverse Kinematic matrix of differential drive robot
inv_Kin[0][0] = WHEEL_DIAMETER / 4;