PID control for robot with incremental encoder
Dependencies: IncrementalEncoder Motor PID mbed
Fork of PIDRover by
main.cpp@1:246e8a504681, 2013-04-12 (annotated)
- Committer:
- rahulsharmak
- Date:
- Fri Apr 12 18:24:44 2013 +0000
- Revision:
- 1:246e8a504681
- Parent:
- 0:be99ed42340d
Robot with PID control using incremental encoder
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
aberk | 0:be99ed42340d | 1 | /** |
rahulsharmak | 1:246e8a504681 | 2 | PID Controller for maintaining constant velocity for robots using incremental encoder |
aberk | 0:be99ed42340d | 3 | */ |
aberk | 0:be99ed42340d | 4 | |
aberk | 0:be99ed42340d | 5 | #include "Motor.h" |
aberk | 0:be99ed42340d | 6 | #include "PID.h" |
rahulsharmak | 1:246e8a504681 | 7 | #include "IncrementalEncoder.h" |
aberk | 0:be99ed42340d | 8 | |
rahulsharmak | 1:246e8a504681 | 9 | IncrementalEncoder leftQei(p29); |
rahulsharmak | 1:246e8a504681 | 10 | IncrementalEncoder rightQei(p30); |
rahulsharmak | 1:246e8a504681 | 11 | Motor leftMotor(p23, p6, p5); // pwm, fwd, rev |
rahulsharmak | 1:246e8a504681 | 12 | Motor rightMotor(p22, p8, p7); // pwm, fwd, rev |
rahulsharmak | 1:246e8a504681 | 13 | |
aberk | 0:be99ed42340d | 14 | PID leftPid(0.4312, 0.1, 0.0, 0.01); //Kc, Ti, Td |
aberk | 0:be99ed42340d | 15 | PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td |
aberk | 0:be99ed42340d | 16 | |
aberk | 0:be99ed42340d | 17 | int main() { |
aberk | 0:be99ed42340d | 18 | |
aberk | 0:be99ed42340d | 19 | leftMotor.period(0.00005); //Set motor PWM periods to 20KHz. |
aberk | 0:be99ed42340d | 20 | rightMotor.period(0.00005); |
aberk | 0:be99ed42340d | 21 | |
aberk | 0:be99ed42340d | 22 | //Input and output limits have been determined |
aberk | 0:be99ed42340d | 23 | //empirically with the specific motors being used. |
aberk | 0:be99ed42340d | 24 | //Change appropriately for different motors. |
aberk | 0:be99ed42340d | 25 | //Input units: counts per second. |
aberk | 0:be99ed42340d | 26 | //Output units: PwmOut duty cycle as %. |
aberk | 0:be99ed42340d | 27 | //Default limits are for moving forward. |
aberk | 0:be99ed42340d | 28 | leftPid.setInputLimits(0, 3000); |
aberk | 0:be99ed42340d | 29 | leftPid.setOutputLimits(0.0, 0.9); |
aberk | 0:be99ed42340d | 30 | leftPid.setMode(AUTO_MODE); |
aberk | 0:be99ed42340d | 31 | rightPid.setInputLimits(0, 3200); |
aberk | 0:be99ed42340d | 32 | rightPid.setOutputLimits(0.0, 0.9); |
aberk | 0:be99ed42340d | 33 | rightPid.setMode(AUTO_MODE); |
aberk | 0:be99ed42340d | 34 | |
aberk | 0:be99ed42340d | 35 | |
aberk | 0:be99ed42340d | 36 | int leftPulses = 0; //How far the left wheel has travelled. |
aberk | 0:be99ed42340d | 37 | int leftPrevPulses = 0; //The previous reading of how far the left wheel |
aberk | 0:be99ed42340d | 38 | //has travelled. |
aberk | 0:be99ed42340d | 39 | float leftVelocity = 0.0; //The velocity of the left wheel in pulses per |
aberk | 0:be99ed42340d | 40 | //second. |
aberk | 0:be99ed42340d | 41 | int rightPulses = 0; //How far the right wheel has travelled. |
aberk | 0:be99ed42340d | 42 | int rightPrevPulses = 0; //The previous reading of how far the right wheel |
aberk | 0:be99ed42340d | 43 | //has travelled. |
aberk | 0:be99ed42340d | 44 | float rightVelocity = 0.0; //The velocity of the right wheel in pulses per |
aberk | 0:be99ed42340d | 45 | //second. |
rahulsharmak | 1:246e8a504681 | 46 | int distance = 600; //Number of pulses to travel. |
aberk | 0:be99ed42340d | 47 | |
aberk | 0:be99ed42340d | 48 | wait(5); //Wait a few seconds before we start moving. |
aberk | 0:be99ed42340d | 49 | |
aberk | 0:be99ed42340d | 50 | //Velocity to mantain in pulses per second. |
rahulsharmak | 1:246e8a504681 | 51 | leftPid.setSetPoint(20); |
rahulsharmak | 1:246e8a504681 | 52 | rightPid.setSetPoint(20); |
aberk | 0:be99ed42340d | 53 | |
aberk | 0:be99ed42340d | 54 | while ((leftPulses < distance) || (rightPulses < distance)) { |
aberk | 0:be99ed42340d | 55 | |
aberk | 0:be99ed42340d | 56 | //Get the current pulse count and subtract the previous one to |
aberk | 0:be99ed42340d | 57 | //calculate the current velocity in counts per second. |
rahulsharmak | 1:246e8a504681 | 58 | leftPulses = leftQei.readTotal(); |
aberk | 0:be99ed42340d | 59 | leftVelocity = (leftPulses - leftPrevPulses) / 0.01; |
aberk | 0:be99ed42340d | 60 | leftPrevPulses = leftPulses; |
aberk | 0:be99ed42340d | 61 | //Use the absolute value of velocity as the PID controller works |
aberk | 0:be99ed42340d | 62 | //in the % (unsigned) domain and will get confused with -ve values. |
aberk | 0:be99ed42340d | 63 | leftPid.setProcessValue(fabs(leftVelocity)); |
aberk | 0:be99ed42340d | 64 | leftMotor.speed(leftPid.compute()); |
aberk | 0:be99ed42340d | 65 | |
rahulsharmak | 1:246e8a504681 | 66 | rightPulses = rightQei.readTotal(); |
aberk | 0:be99ed42340d | 67 | rightVelocity = (rightPulses - rightPrevPulses) / 0.01; |
aberk | 0:be99ed42340d | 68 | rightPrevPulses = rightPulses; |
aberk | 0:be99ed42340d | 69 | rightPid.setProcessValue(fabs(rightVelocity)); |
aberk | 0:be99ed42340d | 70 | rightMotor.speed(rightPid.compute()); |
aberk | 0:be99ed42340d | 71 | |
aberk | 0:be99ed42340d | 72 | wait(0.01); |
aberk | 0:be99ed42340d | 73 | |
aberk | 0:be99ed42340d | 74 | } |
aberk | 0:be99ed42340d | 75 | |
aberk | 0:be99ed42340d | 76 | leftMotor.brake(); |
aberk | 0:be99ed42340d | 77 | rightMotor.brake(); |
aberk | 0:be99ed42340d | 78 | |
aberk | 0:be99ed42340d | 79 | } |