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

2
Main/IMU.ino
View File

@ -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;
} }

9
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,7 +81,6 @@ void setup() {
//Initialize PS3 controller connection //Initialize PS3 controller connection
Ps3.begin(_ps3Address); Ps3.begin(_ps3Address);
} }
void loop() { void loop() {
@ -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);
} }

19
Main/motorControl.ino
View File

@ -84,8 +84,8 @@ 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
@ -96,8 +96,6 @@ void motors() {
//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_) {
@ -132,8 +130,7 @@ 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_;
} }
@ -141,8 +138,7 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl
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;
} }