Diff: main.cpp

Revision:

2:f4d6c9ba05d0

Parent:

1:c70bc01ebfdd

Child:

3:905643e72bcd

--- a/main.cpp	Wed Mar 16 00:52:47 2016 +0000
+++ b/main.cpp	Wed Mar 16 04:46:09 2016 +0000
@@ -10,6 +10,7 @@
 #include "mbed.h"
 #include "motordriver.h"
 #include "PID.h"
+#include "uLCD_4DGL.h"
 //one full revolution=193 counts
 
 
@@ -32,17 +33,35 @@
     volatile int _count;
 };
 
+int distTransform(float input) {
+    if (input>3) return 6;
+    else if (input>2.5) return 8;
+    else if (input>2) return 10;
+    else if (input>1.5) return 14;
+    else if (input>1.1) return 22;
+    else if (input>0.9) return 27;
+    else if (input>0.75) return 35;
+    else if (input>0.6) return 45;
+    else if (input>0.5) return 60;
+    else if (input>0.4) return 75;
+    else return 99;    
+}
+
 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 old
-PID leftPid(0.4620, 0.1, 0.0, 0.01);  //Kc, Ti, Td
+PID leftPid(0.4310, 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);
+AnalogIn ain(p15);
+uLCD_4DGL uLCD(p28,p27,p29); // serial tx, serial rx, reset pin;
 
 int main() {
+    uLCD.printf("\nHello World\n"); //Default Green on black text
+    
     //Input and output limits have been determined
     //empirically with the specific motors being used.
     //Change appropriately for different motors.
@@ -52,16 +71,16 @@
     leftPid.setInputLimits(0, 3000);
     leftPid.setOutputLimits(0.0, 0.8);
     leftPid.setMode(AUTO_MODE);
-    rightPid.setInputLimits(0, 3000);
+    rightPid.setInputLimits(0, 3200);
     rightPid.setOutputLimits(0.0, 0.8);
     rightPid.setMode(AUTO_MODE);
-
+    Serial pc(USBTX, USBRX);
     
-    int leftPrevPulses  = 0, leftActPulses=0; //The previous reading of how far the left wheel
+    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 rightPrevPulses = 0, rightActPulses=0; //The previous reading of how far the right wheel
+    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.
@@ -70,32 +89,32 @@
     led=0;
 
     wait(0.5); //Wait a few seconds before we start moving.
+    uLCD.cls();
 
     //Velocity to mantain in pulses per second.
     leftPid.setSetPoint(1000);
-    rightPid.setSetPoint(1000);
+    rightPid.setSetPoint(900);
 
-    while ((leftActPulses < distance) || (rightActPulses < distance)) {
+    while ((leftPulses.read() < distance) || (rightPulses.read() < distance)) {
+        
 
-        //Get the current pulse count and subtract the previous one to
-        //calculate the current velocity in counts per second.
-        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.
+        leftVelocity = (leftPulses.read() - leftPrevPulses) / 0.01;
+        leftPrevPulses = leftPulses.read();
+        rightVelocity = (rightPulses.read() - rightPrevPulses) / 0.01;
+        rightPrevPulses = rightPulses.read();
+
         leftPid.setProcessValue(fabs(leftVelocity));
         leftMotor.speed(leftPid.compute());
-
-        rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
-        rightPrevPulses = rightActPulses;
         rightPid.setProcessValue(fabs(rightVelocity));
         rightMotor.speed(rightPid.compute());
         
-        wait(0.01);
+        pc.printf("\n%i", distTransform(ain));
+        
+        uLCD.locate(1,2);
+        uLCD.printf("%i", distTransform(ain));
+        wait(0.1);
         led=!led;
         
-        leftActPulses=leftPulses.read();
-        rightActPulses=rightPulses.read();
     }
 
     leftMotor.stop(0.5);