rahul sharma / Mbed 2 deprecated incrementalencoder-pid-robot

PID control for robot with incremental encoder

Dependencies:   IncrementalEncoder Motor PID mbed

Fork of PIDRover by Aaron Berk

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?

UserRevisionLine numberNew 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 }