PID controll for the robot motors.
Dependencies: BioroboticsMotorControl MODSERIAL mbed
main.cpp@3:689f3f2e78e8, 2018-10-19 (annotated)
- Committer:
- brass_phoenix
- Date:
- Fri Oct 19 09:06:28 2018 +0000
- Revision:
- 3:689f3f2e78e8
- Parent:
- 2:3be8cd780b3d
- Child:
- 4:fce8c7458e97
+ Made the motor stall pwm configurable.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
brass_phoenix | 0:7c204101adb0 | 1 | #include "mbed.h" |
brass_phoenix | 0:7c204101adb0 | 2 | #include "FastPWM.h" |
brass_phoenix | 0:7c204101adb0 | 3 | #include "MODSERIAL.h" |
brass_phoenix | 0:7c204101adb0 | 4 | #include "QEI.h" |
brass_phoenix | 0:7c204101adb0 | 5 | |
brass_phoenix | 1:28377623e8c9 | 6 | #include "pid.h" |
brass_phoenix | 1:28377623e8c9 | 7 | |
brass_phoenix | 0:7c204101adb0 | 8 | const float PI = 3.14159265359; |
brass_phoenix | 0:7c204101adb0 | 9 | const int PULSES_PER_ROTATION = 6533; // Amount of motor encoder pulses per rotation. When using X4 encoding. |
brass_phoenix | 2:3be8cd780b3d | 10 | const float pid_period = 0.0001; // PID sample period in seconds. |
brass_phoenix | 0:7c204101adb0 | 11 | |
brass_phoenix | 3:689f3f2e78e8 | 12 | const double Kp = 10.0; |
brass_phoenix | 3:689f3f2e78e8 | 13 | const double Ki = 0.5; |
brass_phoenix | 3:689f3f2e78e8 | 14 | const double Kd = 0.5; |
brass_phoenix | 2:3be8cd780b3d | 15 | |
brass_phoenix | 2:3be8cd780b3d | 16 | const double motor_threshold_rps = 0.3; // Rad/s under which we send 0 to the motor, to prevent it from jittering around. |
brass_phoenix | 2:3be8cd780b3d | 17 | const double motor_stall_pwm = 0.5; // PWM fraction above which the motor starts to move. |
brass_phoenix | 2:3be8cd780b3d | 18 | |
brass_phoenix | 2:3be8cd780b3d | 19 | int printcount = 0; |
brass_phoenix | 1:28377623e8c9 | 20 | |
brass_phoenix | 1:28377623e8c9 | 21 | Ticker pidTicker; // Ticker function |
brass_phoenix | 0:7c204101adb0 | 22 | 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) |
brass_phoenix | 0:7c204101adb0 | 23 | 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) |
brass_phoenix | 0:7c204101adb0 | 24 | 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 |
brass_phoenix | 0:7c204101adb0 | 25 | DigitalOut directionpin2(D7); // 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 |
brass_phoenix | 0:7c204101adb0 | 26 | AnalogIn potmeter1(A1); // Analoge input van potmeter 1 -> Motor 1 |
brass_phoenix | 0:7c204101adb0 | 27 | AnalogIn potmeter2(A2); // Analoge input van potmeter 2 -> Motor 2 |
brass_phoenix | 0:7c204101adb0 | 28 | QEI encoder1(D11, D10, NC, PULSES_PER_ROTATION, QEI::X4_ENCODING); // Reads encoder, connect pins of encoder 1 to D12 and D13; NC: not connected pin (for X4); 6533 prm (counts per rotation) |
brass_phoenix | 0:7c204101adb0 | 29 | 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) |
brass_phoenix | 0:7c204101adb0 | 30 | Serial pc(USBTX, USBRX); |
brass_phoenix | 0:7c204101adb0 | 31 | |
brass_phoenix | 1:28377623e8c9 | 32 | PID pid(pid_period); |
brass_phoenix | 1:28377623e8c9 | 33 | |
brass_phoenix | 0:7c204101adb0 | 34 | // Updates a motor connected to the specified pins with the given speed. |
brass_phoenix | 1:28377623e8c9 | 35 | // The speed can be both positive and negative, in the range [-1, 1]. |
brass_phoenix | 1:28377623e8c9 | 36 | void update_motor(DigitalOut* dir, FastPWM* pwm, double speed) { |
brass_phoenix | 1:28377623e8c9 | 37 | if (speed < 1.0 && speed > 0) { |
brass_phoenix | 1:28377623e8c9 | 38 | // Speed is in the range [0, 1] but the motor only moves |
brass_phoenix | 1:28377623e8c9 | 39 | // in the range [0.5, 1]. Rescale for this. |
brass_phoenix | 2:3be8cd780b3d | 40 | speed = (speed * (1-motor_stall_pwm)) + motor_stall_pwm; |
brass_phoenix | 1:28377623e8c9 | 41 | } |
brass_phoenix | 1:28377623e8c9 | 42 | if (speed > -1.0 && speed < 0) { |
brass_phoenix | 1:28377623e8c9 | 43 | // Speed is in the range [-1, 0] but the motor only moves |
brass_phoenix | 1:28377623e8c9 | 44 | // in the range [-1, -0.5]. Rescale for this. |
brass_phoenix | 2:3be8cd780b3d | 45 | speed = (speed * (1-motor_stall_pwm)) - motor_stall_pwm; |
brass_phoenix | 1:28377623e8c9 | 46 | } |
brass_phoenix | 1:28377623e8c9 | 47 | |
brass_phoenix | 0:7c204101adb0 | 48 | // either true or false, determines direction (0 or 1) |
brass_phoenix | 1:28377623e8c9 | 49 | *dir = speed > 0; |
brass_phoenix | 0:7c204101adb0 | 50 | // pwm duty cycle can only be positive, floating point absolute value (if value is >0, the there still will be a positive value). |
brass_phoenix | 1:28377623e8c9 | 51 | *pwm = fabs(speed); |
brass_phoenix | 0:7c204101adb0 | 52 | } |
brass_phoenix | 0:7c204101adb0 | 53 | |
brass_phoenix | 1:28377623e8c9 | 54 | double encoder_pulses_to_radians(int pulses) { |
brass_phoenix | 1:28377623e8c9 | 55 | return (pulses/float(PULSES_PER_ROTATION)) * 2.0f*PI; |
brass_phoenix | 0:7c204101adb0 | 56 | } |
brass_phoenix | 0:7c204101adb0 | 57 | |
brass_phoenix | 0:7c204101adb0 | 58 | // Converts radians/s values into PWM values for motor controll. |
brass_phoenix | 0:7c204101adb0 | 59 | // Both positive and negative values. |
brass_phoenix | 1:28377623e8c9 | 60 | double radians_per_second_to_pwm(double rps) { |
brass_phoenix | 2:3be8cd780b3d | 61 | // If the rad/s is below the anti-jitter treshold, it is simply 0. |
brass_phoenix | 2:3be8cd780b3d | 62 | if (rps > 0 && rps < motor_threshold_rps) { |
brass_phoenix | 2:3be8cd780b3d | 63 | rps = 0; |
brass_phoenix | 2:3be8cd780b3d | 64 | } |
brass_phoenix | 3:689f3f2e78e8 | 65 | if (rps < 0 && rps > -motor_threshold_rps) { |
brass_phoenix | 2:3be8cd780b3d | 66 | rps = 0; |
brass_phoenix | 2:3be8cd780b3d | 67 | } |
brass_phoenix | 2:3be8cd780b3d | 68 | |
brass_phoenix | 2:3be8cd780b3d | 69 | |
brass_phoenix | 0:7c204101adb0 | 70 | // With our specific motor, full PWM is equal to 1 round per second. |
brass_phoenix | 0:7c204101adb0 | 71 | // Or 2PI radians per second. |
brass_phoenix | 1:28377623e8c9 | 72 | double pwm_speed = rps / (2*PI); |
brass_phoenix | 1:28377623e8c9 | 73 | |
brass_phoenix | 2:3be8cd780b3d | 74 | // PWM speeds can only go between [-1, 1] |
brass_phoenix | 1:28377623e8c9 | 75 | if (pwm_speed > 1) { pwm_speed = 1; } |
brass_phoenix | 1:28377623e8c9 | 76 | if (pwm_speed < -1) { pwm_speed = -1; } |
brass_phoenix | 0:7c204101adb0 | 77 | return pwm_speed; |
brass_phoenix | 0:7c204101adb0 | 78 | } |
brass_phoenix | 0:7c204101adb0 | 79 | |
brass_phoenix | 0:7c204101adb0 | 80 | // Normalizes a potmeter value from it's original range of [0, 1] to [-1, 1] |
brass_phoenix | 1:28377623e8c9 | 81 | double normalize_pot(double pot_value) { |
brass_phoenix | 0:7c204101adb0 | 82 | // scales value potmeter from 0-1 to -1 - 1. |
brass_phoenix | 0:7c204101adb0 | 83 | return pot_value * 2 - 1; |
brass_phoenix | 0:7c204101adb0 | 84 | }; |
brass_phoenix | 0:7c204101adb0 | 85 | |
brass_phoenix | 0:7c204101adb0 | 86 | |
brass_phoenix | 0:7c204101adb0 | 87 | void motorfunction() { |
brass_phoenix | 0:7c204101adb0 | 88 | // reads out value potmeter 1 between 0-1 |
brass_phoenix | 1:28377623e8c9 | 89 | double pot = potmeter2.read(); |
brass_phoenix | 1:28377623e8c9 | 90 | double desired_angle = normalize_pot(pot) * PI; // Scale the potmeter to [-PI, PI] |
brass_phoenix | 0:7c204101adb0 | 91 | |
brass_phoenix | 0:7c204101adb0 | 92 | int pulses = encoder2.getPulses(); |
brass_phoenix | 1:28377623e8c9 | 93 | double current_angle = encoder_pulses_to_radians(pulses); |
brass_phoenix | 0:7c204101adb0 | 94 | |
brass_phoenix | 1:28377623e8c9 | 95 | double error = current_angle - desired_angle; |
brass_phoenix | 1:28377623e8c9 | 96 | // PID controll. |
brass_phoenix | 1:28377623e8c9 | 97 | double speed_rps = pid.update(error); |
brass_phoenix | 0:7c204101adb0 | 98 | |
brass_phoenix | 1:28377623e8c9 | 99 | double speed_pwm = radians_per_second_to_pwm(speed_rps); |
brass_phoenix | 0:7c204101adb0 | 100 | |
brass_phoenix | 2:3be8cd780b3d | 101 | printcount++; |
brass_phoenix | 2:3be8cd780b3d | 102 | if (printcount >= 0.1L/pid_period) { |
brass_phoenix | 2:3be8cd780b3d | 103 | pc.printf("c_angle: %f, d_angle: %f, error: %f, rps: %f, speed: %f\n", current_angle, desired_angle, error, speed_rps, speed_pwm); |
brass_phoenix | 2:3be8cd780b3d | 104 | printcount = 0; |
brass_phoenix | 2:3be8cd780b3d | 105 | } |
brass_phoenix | 0:7c204101adb0 | 106 | |
brass_phoenix | 1:28377623e8c9 | 107 | update_motor(&directionpin2, &pwmpin2, speed_pwm); |
brass_phoenix | 0:7c204101adb0 | 108 | } |
brass_phoenix | 0:7c204101adb0 | 109 | |
brass_phoenix | 0:7c204101adb0 | 110 | |
brass_phoenix | 0:7c204101adb0 | 111 | int main() |
brass_phoenix | 0:7c204101adb0 | 112 | { |
brass_phoenix | 1:28377623e8c9 | 113 | pc.baud(115200); |
brass_phoenix | 0:7c204101adb0 | 114 | pc.printf("Starting."); |
brass_phoenix | 1:28377623e8c9 | 115 | pid.set_k_values(Kp, Ki, Kd); |
brass_phoenix | 1:28377623e8c9 | 116 | pidTicker.attach(motorfunction, pid_period); |
brass_phoenix | 0:7c204101adb0 | 117 | pwmpin1.period_us(60.0); // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to be done once) |
brass_phoenix | 0:7c204101adb0 | 118 | while(true){ |
brass_phoenix | 0:7c204101adb0 | 119 | } //Lege while loop zodat functie niet afloopt |
brass_phoenix | 0:7c204101adb0 | 120 | } |