Diff: main.cpp

Revision:

1:c70bc01ebfdd

Parent:

0:be99ed42340d

Child:

2:f4d6c9ba05d0

--- a/main.cpp	Fri Sep 10 13:32:59 2010 +0000
+++ b/main.cpp	Wed Mar 16 00:52:47 2016 +0000
@@ -3,25 +3,46 @@
  * the PWM signal to H-bridges connected to the motors to attempt to maintain
  * a constant velocity.
  */
+ /*Sources:     mbed Rover cookbook page: https://developer.mbed.org/cookbook/mbed-Rover
+                InterruptIn handbook page: https://developer.mbed.org/handbook/InterruptIn 
+ */
+ 
+#include "mbed.h"
+#include "motordriver.h"
+#include "PID.h"
+//one full revolution=193 counts
 
-#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
+class Counter {
+    public:
+    Counter(PinName pin) : _interrupt(pin) {        // create the InterruptIn on the pin specified to Counter
+        _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
+    }
+ 
+    void increment() {
+        _count++;
+    }
+ 
+    int read() {
+        return _count;
+    }
+ 
+private:
+    InterruptIn _interrupt;
+    volatile int _count;
+};
+
+Motor leftMotor(p22, p5, p6, 1); // pwm, fwd, rev, can brake 
+Motor rightMotor(p21, p8, p7, 1); // pwm, fwd, rev, can brake
+Counter leftPulses(p9), rightPulses (p10);
 //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 leftPid(0.4312, 0.1, 0.0, 0.01);  //Kc, Ti, Td old
+PID leftPid(0.4620, 0.1, 0.0, 0.01);  //Kc, Ti, Td
 PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
+DigitalOut led(LED1);
 
 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.
@@ -29,54 +50,55 @@
     //Output units: PwmOut duty cycle as %.
     //Default limits are for moving forward.
     leftPid.setInputLimits(0, 3000);
-    leftPid.setOutputLimits(0.0, 0.9);
+    leftPid.setOutputLimits(0.0, 0.8);
     leftPid.setMode(AUTO_MODE);
-    rightPid.setInputLimits(0, 3200);
-    rightPid.setOutputLimits(0.0, 0.9);
+    rightPid.setInputLimits(0, 3000);
+    rightPid.setOutputLimits(0.0, 0.8);
     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
+    
+    int leftPrevPulses  = 0, leftActPulses=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
+    int rightPrevPulses = 0, rightActPulses=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.
+    
+    int distance     = 1000; //Number of pulses to travel.
+    led=0;
 
-    wait(5); //Wait a few seconds before we start moving.
+    wait(0.5); //Wait a few seconds before we start moving.
 
     //Velocity to mantain in pulses per second.
     leftPid.setSetPoint(1000);
-    rightPid.setSetPoint(975);
+    rightPid.setSetPoint(1000);
 
-    while ((leftPulses < distance) || (rightPulses < distance)) {
+    while ((leftActPulses < distance) || (rightActPulses < 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;
+        leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
+        leftPrevPulses = leftActPulses;
         //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;
+        rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
+        rightPrevPulses = rightActPulses;
         rightPid.setProcessValue(fabs(rightVelocity));
         rightMotor.speed(rightPid.compute());
-
+        
         wait(0.01);
-
+        led=!led;
+        
+        leftActPulses=leftPulses.read();
+        rightActPulses=rightPulses.read();
     }
 
-    leftMotor.brake();
-    rightMotor.brake();
+    leftMotor.stop(0.5);
+    rightMotor.stop(0.5);
 
 }