PID controll for the robot motors.
Dependencies: BioroboticsMotorControl MODSERIAL mbed
Diff: main.cpp
- Revision:
- 1:28377623e8c9
- Parent:
- 0:7c204101adb0
- Child:
- 2:3be8cd780b3d
diff -r 7c204101adb0 -r 28377623e8c9 main.cpp --- a/main.cpp Thu Oct 18 12:37:34 2018 +0000 +++ b/main.cpp Fri Oct 19 07:58:26 2018 +0000 @@ -3,10 +3,17 @@ #include "MODSERIAL.h" #include "QEI.h" +#include "pid.h" + 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. -Ticker motorTicker; // Ticker function +const double Kp = 10.0; +const double Ki = 0.5; +const double Kd = 2.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) FastPWM pwmpin2(D6); // 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) DigitalOut directionpin1(D4); // SPECIFIC PIN (hoeft niet aangesloten te worden) Direction value (0-1) that the mbed will give the motor: in which direction the motor must rotate @@ -17,71 +24,78 @@ QEI encoder2(D13, D12, NC, PULSES_PER_ROTATION, QEI::X4_ENCODING); // Reads encoder, connect pins of encoder 2 to D12 and D13; NC: not connected pin (for X4); 6533 prm (counts per rotation) Serial pc(USBTX, USBRX); +PID pid(pid_period); + // Updates a motor connected to the specified pins with the given speed. -// The speed can be both positive and negative. -void update_motor(DigitalOut dir, FastPWM pwm, int speed) { +// The speed can be both positive and negative, in the range [-1, 1]. +void update_motor(DigitalOut* dir, FastPWM* pwm, double speed) { + 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; + } + 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; + } + // either true or false, determines direction (0 or 1) - dir = speed > 0; + *dir = speed > 0; // pwm duty cycle can only be positive, floating point absolute value (if value is >0, the there still will be a positive value). - pwm = abs(speed); + *pwm = fabs(speed); } -float encoder_pulses_to_radians(int pulses) { - return (pulses/(float)PULSES_PER_ROTATION) * 2.0f*PI; +double encoder_pulses_to_radians(int pulses) { + return (pulses/float(PULSES_PER_ROTATION)) * 2.0f*PI; } // Converts radians/s values into PWM values for motor controll. // Both positive and negative values. -int radians_per_second_to_pwm(float rps) { +double radians_per_second_to_pwm(double rps) { // With our specific motor, full PWM is equal to 1 round per second. // Or 2PI radians per second. - int pwm_speed = (rps / (2*PI)) * 255; - if (pwm_speed > 255) { pwm_speed = 255; } - if (pwm_speed < -255) { pwm_speed = -255; } + double pwm_speed = rps / (2*PI); + + if (pwm_speed > 1) { pwm_speed = 1; } + if (pwm_speed < -1) { pwm_speed = -1; } return pwm_speed; } // Normalizes a potmeter value from it's original range of [0, 1] to [-1, 1] -float normalize_pot(float pot_value) { +double normalize_pot(double pot_value) { // scales value potmeter from 0-1 to -1 - 1. return pot_value * 2 - 1; }; -double P_controller(double error) { - double Kp = 10; - - // Proportional part: - double u_k = Kp * error; - - return u_k; -} - void motorfunction() { // reads out value potmeter 1 between 0-1 - float pot = potmeter2.read(); - float desired_angle = normalize_pot(pot) * PI; // Scale the potmeter to [-PI, PI] + double pot = potmeter2.read(); + double desired_angle = normalize_pot(pot) * PI; // Scale the potmeter to [-PI, PI] int pulses = encoder2.getPulses(); - float current_angle = encoder_pulses_to_radians(pulses); + double current_angle = encoder_pulses_to_radians(pulses); - float error = current_angle - desired_angle; - float speed_rps = P_controller(error); + double error = current_angle - desired_angle; + // PID controll. + double speed_rps = pid.update(error); - int speed_pwm = radians_per_second_to_pwm(speed_rps); + double speed_pwm = radians_per_second_to_pwm(speed_rps); - pc.printf("cur_angle: %f, des_angle: %f, rps: %f, pwm: %i\n", current_angle, desired_angle, speed_rps, speed_pwm); + 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); - update_motor(directionpin2, pwmpin2, speed_pwm); + update_motor(&directionpin2, &pwmpin2, speed_pwm); } int main() { + pc.baud(115200); pc.printf("Starting."); + pid.set_k_values(Kp, Ki, Kd); + pidTicker.attach(motorfunction, pid_period); pwmpin1.period_us(60.0); // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to be done once) while(true){ - motorfunction(); - wait(0.1); } //Lege while loop zodat functie niet afloopt } \ No newline at end of file