EmaMaker
/
Two-Wheel-Self-Balancing-Robot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

[UNSURE] open-loop wheel sync

main
EmaMaker 2024-01-12 13:43:05 +01:00
parent b5798d2eb3
commit 7b7c09714d
1 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
firmware-arduino/selfbalance-madgwick/selfbalance-madgwick.ino
View File

@ -30,6 +30,12 @@ double input = 0;
double roll{ 0 }, pitch{ 0 }, yaw{ 0 };
constexpr double max_torque_sx = 0.392;
constexpr double max_torque_dx = 0.36;
constexpr double max_torque = max_torque_dx+max_torque_sx;
constexpr double torque_sx_coeff = max_torque_sx/max_torque;
constexpr double torque_dx_coeff = max_torque_dx/max_torque;
void setup(void) {
Serial.begin(9600);
delay(500);
@ -47,7 +53,7 @@ void setup(void) {
}
myController.begin(&input, &output, &setpoint, KP, KI, KD, P_ON_E, FORWARD);
myController.setOutputLimits(-0.72, 0.72); // double of max torque motors can exhert
myController.setOutputLimits(-max_torque, max_torque); // double of max torque motors can exhert
// myController.setWindUpLimits(-0.2 , 0.02);
myController.setSampleTime(1);
myController.start();
@ -71,11 +77,11 @@ void compute(){
myController.compute();
double torque = abs(output) * 0.5;
double torque = abs(output);
double s = sign(output);
double pwm_dx = torque <= 0.005 ? 30*s : torque_to_pwm_dx(torque)*s;
double pwm_sx = torque <= 0.005 ? 30*s : torque_to_pwm_sx(torque)*s;
double pwm_dx = torque <= 0.005 ? 30*s : torque_to_pwm_dx(torque*torque_dx_coeff)*s;
double pwm_sx = torque <= 0.005 ? 30*s : torque_to_pwm_sx(torque*torque_sx_coeff)*s;
move_pwm(MOT_SX, pwm_sx);
move_pwm(MOT_DX, pwm_dx);