Aaron Berk
A more advanced robot that uses PID control on the motor speed to maintain a more accurate heading.
Dependencies: mbed Motor QEI PID
Revision 0:be99ed42340d, committed 2010-09-10
- Comitter:
- aberk
- Date:
- Fri Sep 10 13:32:59 2010 +0000
- Commit message:
- Version 1.0
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.lib Fri Sep 10 13:32:59 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/Motor/#c75b234558af
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Fri Sep 10 13:32:59 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Fri Sep 10 13:32:59 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Sep 10 13:32:59 2010 +0000
@@ -0,0 +1,82 @@
+/**
+ * Drive a robot forwards or backwards by using a PID controller to vary
+ * the PWM signal to H-bridges connected to the motors to attempt to maintain
+ * a constant velocity.
+ */
+
+#include "Motor.h"
+#include "QEI.h"
+#include "PID.h"
+
+Motor leftMotor(p21, p20, p19); //pwm, inB, inA
+Motor rightMotor(p22, p17, p18); //pwm, inA, inB
+QEI leftQei(p29, p30, NC, 624); //chanA, chanB, index, ppr
+QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr
+//Tuning parameters calculated from step tests;
+//see http://mbed.org/cookbook/PID for examples.
+PID leftPid(0.4312, 0.1, 0.0, 0.01); //Kc, Ti, Td
+PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
+
+int main() {
+
+ leftMotor.period(0.00005); //Set motor PWM periods to 20KHz.
+ rightMotor.period(0.00005);
+
+ //Input and output limits have been determined
+ //empirically with the specific motors being used.
+ //Change appropriately for different motors.
+ //Input units: counts per second.
+ //Output units: PwmOut duty cycle as %.
+ //Default limits are for moving forward.
+ leftPid.setInputLimits(0, 3000);
+ leftPid.setOutputLimits(0.0, 0.9);
+ leftPid.setMode(AUTO_MODE);
+ rightPid.setInputLimits(0, 3200);
+ rightPid.setOutputLimits(0.0, 0.9);
+ rightPid.setMode(AUTO_MODE);
+
+
+ int leftPulses = 0; //How far the left wheel has travelled.
+ int leftPrevPulses = 0; //The previous reading of how far the left wheel
+ //has travelled.
+ float leftVelocity = 0.0; //The velocity of the left wheel in pulses per
+ //second.
+ int rightPulses = 0; //How far the right wheel has travelled.
+ int rightPrevPulses = 0; //The previous reading of how far the right wheel
+ //has travelled.
+ float rightVelocity = 0.0; //The velocity of the right wheel in pulses per
+ //second.
+ int distance = 6000; //Number of pulses to travel.
+
+ wait(5); //Wait a few seconds before we start moving.
+
+ //Velocity to mantain in pulses per second.
+ leftPid.setSetPoint(1000);
+ rightPid.setSetPoint(975);
+
+ while ((leftPulses < distance) || (rightPulses < distance)) {
+
+ //Get the current pulse count and subtract the previous one to
+ //calculate the current velocity in counts per second.
+ leftPulses = leftQei.getPulses();
+ leftVelocity = (leftPulses - leftPrevPulses) / 0.01;
+ leftPrevPulses = leftPulses;
+ //Use the absolute value of velocity as the PID controller works
+ //in the % (unsigned) domain and will get confused with -ve values.
+ leftPid.setProcessValue(fabs(leftVelocity));
+ leftMotor.speed(leftPid.compute());
+
+ rightPulses = rightQei.getPulses();
+ rightVelocity = (rightPulses - rightPrevPulses) / 0.01;
+ rightPrevPulses = rightPulses;
+ rightPid.setProcessValue(fabs(rightVelocity));
+ rightMotor.speed(rightPid.compute());
+
+ wait(0.01);
+
+ }
+
+ leftMotor.brake();
+ rightMotor.brake();
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Sep 10 13:32:59 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/e2ac27c8e93e