PID controll for the robot motors.
Dependencies: BioroboticsMotorControl MODSERIAL mbed
Diff: main.cpp
- Revision:
- 2:3be8cd780b3d
- Parent:
- 1:28377623e8c9
- Child:
- 3:689f3f2e78e8
--- a/main.cpp Fri Oct 19 07:58:26 2018 +0000 +++ b/main.cpp Fri Oct 19 08:43:25 2018 +0000 @@ -7,11 +7,16 @@ const float PI = 3.14159265359; const int PULSES_PER_ROTATION = 6533; // Amount of motor encoder pulses per rotation. When using X4 encoding. -const float pid_period = 0.1; // PID sample period in seconds. +const float pid_period = 0.0001; // PID sample period in seconds. -const double Kp = 10.0; -const double Ki = 0.5; -const double Kd = 2.0; +const double Kp = 5.0; +const double Ki = 0.1; +const double Kd = 0.1; + +const double motor_threshold_rps = 0.3; // Rad/s under which we send 0 to the motor, to prevent it from jittering around. +const double motor_stall_pwm = 0.5; // PWM fraction above which the motor starts to move. + +int printcount = 0; Ticker pidTicker; // Ticker function FastPWM pwmpin1(D5); // SPECIFIC PIN (hoeft niet aangesloten te worden) Tells you how fast the motor has to go (later: pwmpin.write will tell you the duty cycle, aka how much voltage the motor gets) @@ -32,12 +37,12 @@ if (speed < 1.0 && speed > 0) { // Speed is in the range [0, 1] but the motor only moves // in the range [0.5, 1]. Rescale for this. - speed = (speed * 0.5) + 0.5; + speed = (speed * (1-motor_stall_pwm)) + motor_stall_pwm; } if (speed > -1.0 && speed < 0) { // Speed is in the range [-1, 0] but the motor only moves // in the range [-1, -0.5]. Rescale for this. - speed = (speed * 0.5) - 0.5; + speed = (speed * (1-motor_stall_pwm)) - motor_stall_pwm; } // either true or false, determines direction (0 or 1) @@ -53,10 +58,20 @@ // Converts radians/s values into PWM values for motor controll. // Both positive and negative values. double radians_per_second_to_pwm(double rps) { + // If the rad/s is below the anti-jitter treshold, it is simply 0. + if (rps > 0 && rps < motor_threshold_rps) { + rps = 0; + } + if (rps < 0 && rps > motor_threshold_rps) { + rps = 0; + } + + // With our specific motor, full PWM is equal to 1 round per second. // Or 2PI radians per second. double pwm_speed = rps / (2*PI); + // PWM speeds can only go between [-1, 1] if (pwm_speed > 1) { pwm_speed = 1; } if (pwm_speed < -1) { pwm_speed = -1; } return pwm_speed; @@ -83,7 +98,11 @@ double speed_pwm = radians_per_second_to_pwm(speed_rps); - pc.printf("puls: %i, c_angle: %f, d_angle: %f, error: %f, rps: %f, speed: %f\n", pulses, current_angle, desired_angle, error, speed_rps, speed_pwm); + printcount++; + if (printcount >= 0.1L/pid_period) { + pc.printf("c_angle: %f, d_angle: %f, error: %f, rps: %f, speed: %f\n", current_angle, desired_angle, error, speed_rps, speed_pwm); + printcount = 0; + } update_motor(&directionpin2, &pwmpin2, speed_pwm); }