Diff: main.cpp

Revision:

0:be99ed42340d

Child:

1:246e8a504681

--- /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();
+
+}