Aaron Berk / Mbed 2 deprecated PIDRover

A more advanced robot that uses PID control on the motor speed to maintain a more accurate heading.

Dependencies:   mbed Motor QEI PID

main.cpp@0:be99ed42340d, 2010-09-10 (annotated)

Committer:
aberk
Date:
Fri Sep 10 13:32:59 2010 +0000
Revision:
0:be99ed42340d
Version 1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aberk 0:be99ed42340d 1 /**
aberk 0:be99ed42340d 2 * Drive a robot forwards or backwards by using a PID controller to vary
aberk 0:be99ed42340d 3 * the PWM signal to H-bridges connected to the motors to attempt to maintain
aberk 0:be99ed42340d 4 * a constant velocity.
aberk 0:be99ed42340d 5 */
aberk 0:be99ed42340d 6
aberk 0:be99ed42340d 7 #include "Motor.h"
aberk 0:be99ed42340d 8 #include "QEI.h"
aberk 0:be99ed42340d 9 #include "PID.h"
aberk 0:be99ed42340d 10
aberk 0:be99ed42340d 11 Motor leftMotor(p21, p20, p19); //pwm, inB, inA
aberk 0:be99ed42340d 12 Motor rightMotor(p22, p17, p18); //pwm, inA, inB
aberk 0:be99ed42340d 13 QEI leftQei(p29, p30, NC, 624); //chanA, chanB, index, ppr
aberk 0:be99ed42340d 14 QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr
aberk 0:be99ed42340d 15 //Tuning parameters calculated from step tests;
aberk 0:be99ed42340d 16 //see http://mbed.org/cookbook/PID for examples.
aberk 0:be99ed42340d 17 PID leftPid(0.4312, 0.1, 0.0, 0.01); //Kc, Ti, Td
aberk 0:be99ed42340d 18 PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
aberk 0:be99ed42340d 19
aberk 0:be99ed42340d 20 int main() {
aberk 0:be99ed42340d 21
aberk 0:be99ed42340d 22 leftMotor.period(0.00005); //Set motor PWM periods to 20KHz.
aberk 0:be99ed42340d 23 rightMotor.period(0.00005);
aberk 0:be99ed42340d 24
aberk 0:be99ed42340d 25 //Input and output limits have been determined
aberk 0:be99ed42340d 26 //empirically with the specific motors being used.
aberk 0:be99ed42340d 27 //Change appropriately for different motors.
aberk 0:be99ed42340d 28 //Input units: counts per second.
aberk 0:be99ed42340d 29 //Output units: PwmOut duty cycle as %.
aberk 0:be99ed42340d 30 //Default limits are for moving forward.
aberk 0:be99ed42340d 31 leftPid.setInputLimits(0, 3000);
aberk 0:be99ed42340d 32 leftPid.setOutputLimits(0.0, 0.9);
aberk 0:be99ed42340d 33 leftPid.setMode(AUTO_MODE);
aberk 0:be99ed42340d 34 rightPid.setInputLimits(0, 3200);
aberk 0:be99ed42340d 35 rightPid.setOutputLimits(0.0, 0.9);
aberk 0:be99ed42340d 36 rightPid.setMode(AUTO_MODE);
aberk 0:be99ed42340d 37
aberk 0:be99ed42340d 38
aberk 0:be99ed42340d 39 int leftPulses = 0; //How far the left wheel has travelled.
aberk 0:be99ed42340d 40 int leftPrevPulses = 0; //The previous reading of how far the left wheel
aberk 0:be99ed42340d 41 //has travelled.
aberk 0:be99ed42340d 42 float leftVelocity = 0.0; //The velocity of the left wheel in pulses per
aberk 0:be99ed42340d 43 //second.
aberk 0:be99ed42340d 44 int rightPulses = 0; //How far the right wheel has travelled.
aberk 0:be99ed42340d 45 int rightPrevPulses = 0; //The previous reading of how far the right wheel
aberk 0:be99ed42340d 46 //has travelled.
aberk 0:be99ed42340d 47 float rightVelocity = 0.0; //The velocity of the right wheel in pulses per
aberk 0:be99ed42340d 48 //second.
aberk 0:be99ed42340d 49 int distance = 6000; //Number of pulses to travel.
aberk 0:be99ed42340d 50
aberk 0:be99ed42340d 51 wait(5); //Wait a few seconds before we start moving.
aberk 0:be99ed42340d 52
aberk 0:be99ed42340d 53 //Velocity to mantain in pulses per second.
aberk 0:be99ed42340d 54 leftPid.setSetPoint(1000);
aberk 0:be99ed42340d 55 rightPid.setSetPoint(975);
aberk 0:be99ed42340d 56
aberk 0:be99ed42340d 57 while ((leftPulses < distance) || (rightPulses < distance)) {
aberk 0:be99ed42340d 58
aberk 0:be99ed42340d 59 //Get the current pulse count and subtract the previous one to
aberk 0:be99ed42340d 60 //calculate the current velocity in counts per second.
aberk 0:be99ed42340d 61 leftPulses = leftQei.getPulses();
aberk 0:be99ed42340d 62 leftVelocity = (leftPulses - leftPrevPulses) / 0.01;
aberk 0:be99ed42340d 63 leftPrevPulses = leftPulses;
aberk 0:be99ed42340d 64 //Use the absolute value of velocity as the PID controller works
aberk 0:be99ed42340d 65 //in the % (unsigned) domain and will get confused with -ve values.
aberk 0:be99ed42340d 66 leftPid.setProcessValue(fabs(leftVelocity));
aberk 0:be99ed42340d 67 leftMotor.speed(leftPid.compute());
aberk 0:be99ed42340d 68
aberk 0:be99ed42340d 69 rightPulses = rightQei.getPulses();
aberk 0:be99ed42340d 70 rightVelocity = (rightPulses - rightPrevPulses) / 0.01;
aberk 0:be99ed42340d 71 rightPrevPulses = rightPulses;
aberk 0:be99ed42340d 72 rightPid.setProcessValue(fabs(rightVelocity));
aberk 0:be99ed42340d 73 rightMotor.speed(rightPid.compute());
aberk 0:be99ed42340d 74
aberk 0:be99ed42340d 75 wait(0.01);
aberk 0:be99ed42340d 76
aberk 0:be99ed42340d 77 }
aberk 0:be99ed42340d 78
aberk 0:be99ed42340d 79 leftMotor.brake();
aberk 0:be99ed42340d 80 rightMotor.brake();
aberk 0:be99ed42340d 81
aberk 0:be99ed42340d 82 }