A simple robot with 2 motors and 2 encoders : IHM04A1. Motor speed is set to 40 and PID controllers are use to attain that speed
Dependencies: mbed HelloWorld_IHM04A1 PID X_NUCLEO_IHM04A1 PinDetect mRotaryEncoder IHM HBridgeMotor
main.cpp@1:10afd4e83e0e, 2019-10-13 (annotated)
- Committer:
- gabi22top
- Date:
- Sun Oct 13 12:56:18 2019 +0000
- Revision:
- 1:10afd4e83e0e
- Parent:
- 0:e019949c4c69
PID with encoder and IHM04A1;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
harryeakins | 0:e019949c4c69 | 1 | #include "mbed.h" |
harryeakins | 0:e019949c4c69 | 2 | |
harryeakins | 0:e019949c4c69 | 3 | #include "mRotaryEncoder.h" |
harryeakins | 0:e019949c4c69 | 4 | #include "HBridgeMotor.h" |
harryeakins | 0:e019949c4c69 | 5 | #include "PID.h" |
harryeakins | 0:e019949c4c69 | 6 | |
harryeakins | 0:e019949c4c69 | 7 | // PID constants |
harryeakins | 0:e019949c4c69 | 8 | #define RATE 0.2 |
harryeakins | 0:e019949c4c69 | 9 | #define Kc 1.5 // Proportional |
harryeakins | 0:e019949c4c69 | 10 | #define Ti 1.0 // Integral |
harryeakins | 0:e019949c4c69 | 11 | #define Td 0.0 // Derivative |
harryeakins | 0:e019949c4c69 | 12 | |
harryeakins | 0:e019949c4c69 | 13 | DigitalOut led1(LED1); |
harryeakins | 0:e019949c4c69 | 14 | |
harryeakins | 0:e019949c4c69 | 15 | // Two PID Controllers |
harryeakins | 0:e019949c4c69 | 16 | PID leftPID(Kc, Ti, Td, RATE); |
harryeakins | 0:e019949c4c69 | 17 | PID rightPID(Kc, Ti, Td, RATE); |
harryeakins | 0:e019949c4c69 | 18 | |
harryeakins | 0:e019949c4c69 | 19 | // Two de-bounced encoders |
harryeakins | 0:e019949c4c69 | 20 | mRotaryEncoder left_enc(p19, p20, p29); |
harryeakins | 0:e019949c4c69 | 21 | mRotaryEncoder right_enc(p26, p27, p28); |
harryeakins | 0:e019949c4c69 | 22 | |
harryeakins | 0:e019949c4c69 | 23 | // Two motor/h-bridge objects |
harryeakins | 0:e019949c4c69 | 24 | HBridgeMotor left_motor(p25, p24); |
harryeakins | 0:e019949c4c69 | 25 | HBridgeMotor right_motor(p21, p22); |
harryeakins | 0:e019949c4c69 | 26 | |
harryeakins | 0:e019949c4c69 | 27 | void initializePidControllers(void) { |
harryeakins | 0:e019949c4c69 | 28 | leftPID.setInputLimits(-300, 300); |
harryeakins | 0:e019949c4c69 | 29 | leftPID.setOutputLimits(-1.0, 1.0); |
harryeakins | 0:e019949c4c69 | 30 | leftPID.setMode(AUTO_MODE); |
harryeakins | 0:e019949c4c69 | 31 | |
harryeakins | 0:e019949c4c69 | 32 | rightPID.setInputLimits(-300, 300); |
harryeakins | 0:e019949c4c69 | 33 | rightPID.setOutputLimits(-1.0, 1.0); |
harryeakins | 0:e019949c4c69 | 34 | rightPID.setMode(AUTO_MODE); |
harryeakins | 0:e019949c4c69 | 35 | } |
harryeakins | 0:e019949c4c69 | 36 | |
harryeakins | 0:e019949c4c69 | 37 | int main() { |
harryeakins | 0:e019949c4c69 | 38 | int left_pulses = 0, right_pulses = 0; |
harryeakins | 0:e019949c4c69 | 39 | |
harryeakins | 0:e019949c4c69 | 40 | initializePidControllers(); |
harryeakins | 0:e019949c4c69 | 41 | |
harryeakins | 0:e019949c4c69 | 42 | // 40 encoder ticks per second target speed |
harryeakins | 0:e019949c4c69 | 43 | leftPID.setSetPoint(40.0); |
harryeakins | 0:e019949c4c69 | 44 | rightPID.setSetPoint(40.0); |
harryeakins | 0:e019949c4c69 | 45 | |
harryeakins | 0:e019949c4c69 | 46 | while (true) { |
harryeakins | 0:e019949c4c69 | 47 | // Get velocities |
harryeakins | 0:e019949c4c69 | 48 | float left_velocity = float(left_enc.Get() - left_pulses)/RATE; |
harryeakins | 0:e019949c4c69 | 49 | float right_velocity = float(right_enc.Get() - right_pulses)/RATE; |
harryeakins | 0:e019949c4c69 | 50 | |
harryeakins | 0:e019949c4c69 | 51 | // Pass the PID Controller the latest velocity value |
harryeakins | 0:e019949c4c69 | 52 | leftPID.setProcessValue(left_velocity); |
harryeakins | 0:e019949c4c69 | 53 | rightPID.setProcessValue(right_velocity); |
harryeakins | 0:e019949c4c69 | 54 | |
harryeakins | 0:e019949c4c69 | 55 | // Query the PID Controllers for the desired power outputs |
harryeakins | 0:e019949c4c69 | 56 | // and set the motors to those powers |
harryeakins | 0:e019949c4c69 | 57 | left_motor.set(leftPID.compute()); |
harryeakins | 0:e019949c4c69 | 58 | right_motor.set(rightPID.compute()); |
harryeakins | 0:e019949c4c69 | 59 | |
harryeakins | 0:e019949c4c69 | 60 | // Save the number of encoder ticks for velocity calculation in the next loop |
harryeakins | 0:e019949c4c69 | 61 | left_pulses = left_enc.Get(); |
harryeakins | 0:e019949c4c69 | 62 | right_pulses = right_enc.Get(); |
harryeakins | 0:e019949c4c69 | 63 | |
harryeakins | 0:e019949c4c69 | 64 | // Toggle the LED and wait |
harryeakins | 0:e019949c4c69 | 65 | led1 = 1 - led1; |
harryeakins | 0:e019949c4c69 | 66 | wait(RATE); |
harryeakins | 0:e019949c4c69 | 67 | } |
harryeakins | 0:e019949c4c69 | 68 | } |