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
pitch = acc_pitch * (1 - alpha) + (((pitch_rate * dT_s) + pitch_prev) * alpha);
pitch_prev = pitch;
}

9
Main/Main.ino
View File

@ -54,12 +54,10 @@ void setup() {
//Initialize IMU
IMU.init();
//Might need some logic here to mke sure the gyro is calibrated correctly, or hardcode the values...
IMU.GyroCalibrate();
delay(10);
//Initialize encoder interrupts
initInterrupt();
initEncoderInterrupt();
//Initialize encoders
m1Raw = 0;
@ -83,7 +81,6 @@ void setup() {
//Initialize PS3 controller connection
Ps3.begin(_ps3Address);
}
void loop() {
@ -102,7 +99,7 @@ void loop() {
// Plot
plot();
//plot();
//Save time for next cycle
@ -111,4 +108,6 @@ void loop() {
//Delay
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_B_state == HIGH) {
m1Raw = m1Raw - 1;
}
else if (M1_B_state == LOW) {
} else if (M1_B_state == LOW) {
m1Raw = m1Raw + 1;
}
}
@ -17,8 +16,7 @@ void IRAM_ATTR m1_A_changed() {
else if (M1_A_state == LOW) {
if (M1_B_state == HIGH) {
m1Raw = m1Raw + 1;
}
else if (M1_B_state == LOW) {
} else if (M1_B_state == LOW) {
m1Raw = m1Raw - 1;
}
}
@ -33,8 +31,7 @@ void IRAM_ATTR m1_B_changed() {
if (M1_B_state == HIGH) {
if (M1_A_state == HIGH) {
m1Raw = m1Raw + 1;
}
else if (M1_A_state == LOW) {
} else if (M1_A_state == LOW) {
m1Raw = m1Raw - 1;
}
}
@ -43,8 +40,7 @@ void IRAM_ATTR m1_B_changed() {
else if (M1_B_state == LOW) {
if (M1_A_state == HIGH) {
m1Raw = m1Raw - 1;
}
else if (M1_A_state == LOW) {
} else if (M1_A_state == LOW) {
m1Raw = m1Raw + 1;
}
}
@ -58,8 +54,7 @@ void IRAM_ATTR m2_A_changed() {
if (M2_A_state == HIGH) {
if (M2_B_state == HIGH) {
m2Raw = m2Raw + 1;
}
else if (M2_B_state == LOW) {
} else if (M2_B_state == LOW) {
m2Raw = m2Raw - 1;
}
}
@ -68,8 +63,7 @@ void IRAM_ATTR m2_A_changed() {
else if (M2_A_state == LOW) {
if (M2_B_state == HIGH) {
m2Raw = m2Raw - 1;
}
else if (M2_B_state == LOW) {
} else if (M2_B_state == LOW) {
m2Raw = m2Raw + 1;
}
}
@ -84,8 +78,7 @@ void IRAM_ATTR m2_B_changed() {
if (M2_B_state == HIGH) {
if (M2_A_state == HIGH) {
m2Raw = m2Raw - 1;
}
else if (M2_A_state == LOW) {
} else if (M2_A_state == LOW) {
m2Raw = m2Raw + 1;
}
}
@ -94,21 +87,23 @@ void IRAM_ATTR m2_B_changed() {
else if (M2_B_state == LOW) {
if (M2_A_state == HIGH) {
m2Raw = m2Raw + 1;
}
else if (M2_A_state == LOW) {
} else if (M2_A_state == LOW) {
m2Raw = m2Raw - 1;
}
}
}
void initInterrupt(){
void initEncoderInterrupt() {
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);
pinMode(M1_ENC_B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(M1_ENC_B), m1_B_changed, CHANGE);
pinMode(M2_ENC_A, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(M2_ENC_A), m2_A_changed, CHANGE);
pinMode(M2_ENC_B, INPUT_PULLUP);
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;
//Sum speed command for motors
// M1_Speed_CMD = 0;
// M2_Speed_CMD = 0;
M1_Speed_CMD = 0;
M2_Speed_CMD = 0;
//Motor control
@ -96,8 +96,6 @@ void motors() {
//Update variables for next scan cycle
m1RawLast = m1Raw;
m2RawLast = m2Raw;
}
float PController(float ref_, float act_, float k_) {
@ -132,8 +130,7 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl
//Deadband
if (speedCMD_ > 0 && speedCMD_ < dbPos_) {
speedCMD_ = dbPos_;
}
else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) {
} else if (speedCMD_ < 0 && speedCMD_ > -dbNeg_) {
speedCMD_ = -dbNeg_;
}
@ -141,8 +138,7 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl
else if (speedCMD_ > saturation) {
windup = saturation - speedCMD_;
speedCMD_ = saturation;
}
else if (speedCMD_ < -saturation) {
} else if (speedCMD_ < -saturation) {
windup = saturation - speedCMD_;
speedCMD_ = -saturation;
}
@ -155,16 +151,13 @@ float motorControl(byte motorID, int speedCMD_, int saturation, float dbPos_, fl
if (speedCMD_ > 0) {
ledcWrite(ch1, 0);
ledcWrite(ch2, speedCMD_);
}
else if (speedCMD_ < 0) {
} else if (speedCMD_ < 0) {
ledcWrite(ch1, -1 * speedCMD_);
ledcWrite(ch2, 0);
}
else if (speedCMD_ == 0) {
} else if (speedCMD_ == 0) {
ledcWrite(ch1, 0);
ledcWrite(ch2, 0);
}
return windup;
}