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

Cleanup

This commit is contained in:
Stedd 2023-10-18 22:16:53 +02:00
parent 23c84d2e41
commit b5a53fa8c9
5 changed files with 199 additions and 214 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
Main/IMU.ino
View File

@ -17,25 +17,25 @@ float pitch_prev = 0;
void readIMU() { void readIMU() {
//Acceletometer //Acceletometer
ax = convertInt(IMU.accelerometer_x( IMU.readFromAccelerometer() ), acc_overflow_value); ax = convertInt(IMU.accelerometer_x(IMU.readFromAccelerometer()), acc_overflow_value);
ay = convertInt(IMU.accelerometer_y( 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); az = convertInt(IMU.accelerometer_z(IMU.readFromAccelerometer()), acc_overflow_value);
//Magnetometer //Magnetometer
cx = IMU.compass_x( IMU.readFromCompass() ); cx = IMU.compass_x(IMU.readFromCompass());
cy = IMU.compass_y( IMU.readFromCompass() ); cy = IMU.compass_y(IMU.readFromCompass());
cz = IMU.compass_z( IMU.readFromCompass() ); cz = IMU.compass_z(IMU.readFromCompass());
// Gyrocope // Gyrocope
gx = convertInt(IMU.gyro_x( IMU.readGyro() ), gyro_overflow_value); // gx - Pitch rate 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 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 gz = convertInt(IMU.gyro_z(IMU.readGyro()), gyro_overflow_value); // gz - Yaw rate
//Temperature sensor //Temperature sensor
gt = IMU.temp ( IMU.readGyro() ); gt = IMU.temp(IMU.readGyro());
// Pitch angle from accelerometer // Pitch angle from accelerometer
@ -49,8 +49,6 @@ void readIMU() {
//Complementary filter //Complementary filter
pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha); pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha);
pitch_prev = pitch; pitch_prev = pitch;
} }

11
Main/Main.ino
View File

@ -54,12 +54,10 @@ void setup() {
//Initialize IMU //Initialize IMU
IMU.init(); IMU.init();
//Might need some logic here to mke sure the gyro is calibrated correctly, or hardcode the values...
IMU.GyroCalibrate();
delay(10); delay(10);
//Initialize encoder interrupts //Initialize encoder interrupts
initInterrupt(); initEncoderInterrupt();
//Initialize encoders //Initialize encoders
m1Raw = 0; m1Raw = 0;
@ -83,14 +81,13 @@ void setup() {
//Initialize PS3 controller connection //Initialize PS3 controller connection
Ps3.begin(_ps3Address); Ps3.begin(_ps3Address);
} }
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] dT_s = dT * pow(10, -6); //[Cycle time in seconds]
//Get sensor data //Get sensor data
@ -102,7 +99,7 @@ void loop() {
// Plot // Plot
plot(); //plot();
//Save time for next cycle //Save time for next cycle
@ -111,4 +108,6 @@ void loop() {
//Delay //Delay
delay(5); delay(5);
//Test
} }

35
Main/interruptEncoders.ino
View File

@ -7,8 +7,7 @@ void IRAM_ATTR m1_A_changed() {
if (M1_A_state == HIGH) { if (M1_A_state == HIGH) {
if (M1_B_state == HIGH) { if (M1_B_state == HIGH) {
m1Raw = m1Raw - 1; m1Raw = m1Raw - 1;
} } else if (M1_B_state == LOW) {
else if (M1_B_state == LOW) {
m1Raw = m1Raw + 1; m1Raw = m1Raw + 1;
} }
} }
@ -17,8 +16,7 @@ void IRAM_ATTR m1_A_changed() {
else if (M1_A_state == LOW) { else if (M1_A_state == LOW) {
if (M1_B_state == HIGH) { if (M1_B_state == HIGH) {
m1Raw = m1Raw + 1; m1Raw = m1Raw + 1;
} } else if (M1_B_state == LOW) {
else if (M1_B_state == LOW) {
m1Raw = m1Raw - 1; m1Raw = m1Raw - 1;
} }
} }
@ -33,8 +31,7 @@ void IRAM_ATTR m1_B_changed() {
if (M1_B_state == HIGH) { if (M1_B_state == HIGH) {
if (M1_A_state == HIGH) { if (M1_A_state == HIGH) {
m1Raw = m1Raw + 1; m1Raw = m1Raw + 1;
} } else if (M1_A_state == LOW) {
else if (M1_A_state == LOW) {
m1Raw = m1Raw - 1; m1Raw = m1Raw - 1;
} }
} }
@ -43,8 +40,7 @@ void IRAM_ATTR m1_B_changed() {
else if (M1_B_state == LOW) { else if (M1_B_state == LOW) {
if (M1_A_state == HIGH) { if (M1_A_state == HIGH) {
m1Raw = m1Raw - 1; m1Raw = m1Raw - 1;
} } else if (M1_A_state == LOW) {
else if (M1_A_state == LOW) {
m1Raw = m1Raw + 1; m1Raw = m1Raw + 1;
} }
} }
@ -58,8 +54,7 @@ void IRAM_ATTR m2_A_changed() {
if (M2_A_state == HIGH) { if (M2_A_state == HIGH) {
if (M2_B_state == HIGH) { if (M2_B_state == HIGH) {
m2Raw = m2Raw + 1; m2Raw = m2Raw + 1;
} } else if (M2_B_state == LOW) {
else if (M2_B_state == LOW) {
m2Raw = m2Raw - 1; m2Raw = m2Raw - 1;
} }
} }
@ -68,8 +63,7 @@ void IRAM_ATTR m2_A_changed() {
else if (M2_A_state == LOW) { else if (M2_A_state == LOW) {
if (M2_B_state == HIGH) { if (M2_B_state == HIGH) {
m2Raw = m2Raw - 1; m2Raw = m2Raw - 1;
} } else if (M2_B_state == LOW) {
else if (M2_B_state == LOW) {
m2Raw = m2Raw + 1; m2Raw = m2Raw + 1;
} }
} }
@ -84,8 +78,7 @@ void IRAM_ATTR m2_B_changed() {
if (M2_B_state == HIGH) { if (M2_B_state == HIGH) {
if (M2_A_state == HIGH) { if (M2_A_state == HIGH) {
m2Raw = m2Raw - 1; m2Raw = m2Raw - 1;
} } else if (M2_A_state == LOW) {
else if (M2_A_state == LOW) {
m2Raw = m2Raw + 1; m2Raw = m2Raw + 1;
} }
} }
@ -94,21 +87,23 @@ void IRAM_ATTR m2_B_changed() {
else if (M2_B_state == LOW) { else if (M2_B_state == LOW) {
if (M2_A_state == HIGH) { if (M2_A_state == HIGH) {
m2Raw = m2Raw + 1; m2Raw = m2Raw + 1;
} } else if (M2_A_state == LOW) {
else if (M2_A_state == LOW) {
m2Raw = m2Raw - 1; m2Raw = m2Raw - 1;
} }
} }
} }
void initInterrupt(){ void initEncoderInterrupt() {
pinMode(M1_ENC_A, INPUT_PULLUP); pinMode(M1_ENC_A, INPUT_PULLUP);
pinMode(M1_ENC_B, INPUT_PULLUP);
pinMode(M2_ENC_A, INPUT_PULLUP);
pinMode(M2_ENC_B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(M1_ENC_A), m1_A_changed, CHANGE); attachInterrupt(digitalPinToInterrupt(M1_ENC_A), m1_A_changed, CHANGE);
pinMode(M1_ENC_B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(M1_ENC_B), m1_B_changed, CHANGE); attachInterrupt(digitalPinToInterrupt(M1_ENC_B), m1_B_changed, CHANGE);
pinMode(M2_ENC_A, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(M2_ENC_A), m2_A_changed, CHANGE); attachInterrupt(digitalPinToInterrupt(M2_ENC_A), m2_A_changed, CHANGE);
pinMode(M2_ENC_B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE); attachInterrupt(digitalPinToInterrupt(M2_ENC_B), m2_B_changed, CHANGE);
} }

43
Main/motorControl.ino
View File

@ -32,9 +32,9 @@ float ref_IL, act_IL, error_IL, IL_cont_out, iError_IL, IL_anti_windup;
//Matrices //Matrices
mtx_type motor_ang_vel [2][1]; mtx_type motor_ang_vel[2][1];
mtx_type vel_Matrix [2][1]; mtx_type vel_Matrix[2][1];
mtx_type inv_Kin [2][2]; mtx_type inv_Kin[2][2];
void initMotors() { void initMotors() {
@ -71,7 +71,7 @@ void motors() {
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 + (dT_s*(error_IL * I_IL) + (IL_anti_windup*((1/I_IL)+(1/K_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); IL_cont_out = round((error_IL * K_IL) + iError_IL);
@ -84,39 +84,37 @@ void motors() {
M2_Speed_CMD = IL_cont_out + TC_cont_out; M2_Speed_CMD = IL_cont_out + TC_cont_out;
//Sum speed command for motors //Sum speed command for motors
// M1_Speed_CMD = 0; M1_Speed_CMD = 0;
// M2_Speed_CMD = 0; M2_Speed_CMD = 0;
//Motor control //Motor control
IL_anti_windup = motorControl(1, M1_Speed_CMD, MOTOR_SATURATION, DEADBAND_M1_POS, DEADBAND_M1_NEG); 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 + motorControl(2, M2_Speed_CMD, MOTOR_SATURATION, DEADBAND_M2_POS, DEADBAND_M2_NEG);
IL_anti_windup = IL_anti_windup/2; IL_anti_windup = IL_anti_windup / 2;
//Update variables for next scan cycle //Update variables for next scan cycle
m1RawLast = m1Raw; m1RawLast = m1Raw;
m2RawLast = m2Raw; m2RawLast = m2Raw;
} }
float PController(float ref_, float act_, float k_){ float PController(float ref_, float act_, float k_) {
return (ref_-act_)*k_; return (ref_ - act_) * k_;
} }
float floatMap(int in, float inMin, float inMax, float outMin, float outMax){ float floatMap(int in, float inMin, float inMax, float outMin, float outMax) {
return (in - inMin) * (outMax - outMin) / (inMax - inMin) + outMin; return (in - inMin) * (outMax - outMin) / (inMax - inMin) + outMin;
} }
float encoderReaderLinVel(int encRaw, int encRawLast, float lin_vel_filtered_, float pulses_per_turn_, float wheel_diameter_, float 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_); float lin_vel_ = (dTurn_ * wheel_diameter_ * PI) / (dT_);
return lin_vel_filtered_ + ((lin_vel_ - lin_vel_filtered_) * dT_ * 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 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 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 ang_vel_ = (dTurn_ * 2 * PI) / (dT_); float ang_vel_ = (dTurn_ * 2 * PI) / (dT_);
@ -132,18 +130,16 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl
//Deadband //Deadband
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_;
} }
// Speed command saturation // Speed command saturation
else if (speedCMD_ > saturation) { else if (speedCMD_ > saturation) {
windup = saturation-speedCMD_; windup = saturation - speedCMD_;
speedCMD_ = saturation; speedCMD_ = saturation;
} } else if (speedCMD_ < -saturation) {
else if (speedCMD_ < -saturation) { windup = saturation - speedCMD_;
windup = saturation-speedCMD_;
speedCMD_ = -saturation; speedCMD_ = -saturation;
} }
@ -155,16 +151,13 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl
if (speedCMD_ > 0) { if (speedCMD_ > 0) {
ledcWrite(ch1, 0); ledcWrite(ch1, 0);
ledcWrite(ch2, speedCMD_); ledcWrite(ch2, speedCMD_);
} } else if (speedCMD_ < 0) {
else if (speedCMD_ < 0) {
ledcWrite(ch1, -1 * speedCMD_); ledcWrite(ch1, -1 * speedCMD_);
ledcWrite(ch2, 0); ledcWrite(ch2, 0);
} } else if (speedCMD_ == 0) {
else if (speedCMD_ == 0) {
ledcWrite(ch1, 0); ledcWrite(ch1, 0);
ledcWrite(ch2, 0); ledcWrite(ch2, 0);
} }
return windup; return windup;
} }

8
Main/plot.ino
View File

@ -1,5 +1,5 @@
void plot(){ void plot() {
// Time // Time
// Serial.print("dT:"); // Serial.print("dT:");
// Serial.println(dT); // Serial.println(dT);
// Serial.print(" "); // Serial.print(" ");
@ -7,7 +7,7 @@ void plot(){
// Serial.println(dT_s); // Serial.println(dT_s);
// Serial.print(" "); // Serial.print(" ");
// IMU // IMU
// Serial.print ( "Pitch:" ); // Serial.print ( "Pitch:" );
// Serial.print ( pitch ); // Serial.print ( pitch );
// Serial.print (" "); // Serial.print (" ");
@ -42,7 +42,7 @@ void plot(){
// Serial.print("m2Raw:"); // Serial.print("m2Raw:");
// Serial.println(m2Raw); // Serial.println(m2Raw);
// Motors // Motors
// Serial.print("SpeedControllerOut:"); // Serial.print("SpeedControllerOut:");
// Serial.print(SC_cont_out); // Serial.print(SC_cont_out);
// Serial.print(" "); // Serial.print(" ");