Diff: main.cpp

Revision:

1:246e8a504681

Parent:

0:be99ed42340d

--- a/main.cpp	Fri Sep 10 13:32:59 2010 +0000
+++ b/main.cpp	Fri Apr 12 18:24:44 2013 +0000
@@ -1,19 +1,16 @@
 /**
- * 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.
+ PID Controller for maintaining constant velocity for robots using incremental encoder
  */
 
 #include "Motor.h"
-#include "QEI.h"
 #include "PID.h"
+#include "IncrementalEncoder.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.
+IncrementalEncoder leftQei(p29);
+IncrementalEncoder rightQei(p30);
+Motor leftMotor(p23, p6, p5); // pwm, fwd, rev
+Motor rightMotor(p22, p8, p7); // pwm, fwd, rev
+
 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
 
@@ -46,19 +43,19 @@
     //has travelled.
     float rightVelocity = 0.0; //The velocity of the right wheel in pulses per
     //second.
-    int distance     = 6000; //Number of pulses to travel.
+    int distance     = 600; //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);
+    leftPid.setSetPoint(20);
+    rightPid.setSetPoint(20);
 
     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();
+        leftPulses = leftQei.readTotal();
         leftVelocity = (leftPulses - leftPrevPulses) / 0.01;
         leftPrevPulses = leftPulses;
         //Use the absolute value of velocity as the PID controller works
@@ -66,7 +63,7 @@
         leftPid.setProcessValue(fabs(leftVelocity));
         leftMotor.speed(leftPid.compute());
 
-        rightPulses = rightQei.getPulses();
+        rightPulses = rightQei.readTotal();
         rightVelocity = (rightPulses - rightPrevPulses) / 0.01;
         rightPrevPulses = rightPulses;
         rightPid.setProcessValue(fabs(rightVelocity));