Diff: main.cpp

Revision:

2:64585b8d8404

Parent:

1:8638bdaf172b

Child:

3:ce743dbbd4a5

--- a/main.cpp	Tue Dec 06 00:48:26 2016 +0000
+++ b/main.cpp	Wed Dec 07 02:23:31 2016 +0000
@@ -1,5 +1,4 @@
 #include "mbed.h"
-#include "rtos.h"
 #include "Motor.h"
 #include "Servo.h"
 #include "LSM9DS1.h"
@@ -18,82 +17,52 @@
 //SERVO PINS
 Servo angle(p21);
 //IMU PINS
-LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
+LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
 
-int correction = 0;
+int correction = 0; //Used to adjust software delay for sonar measurement
 Timer sonar;
 
 Serial pc(USBTX, USBRX);
 Timer timestamp;
 // Set up Hall-effect control
-DigitalIn EncR(p19); // right motor
-DigitalIn EncL(p20); // left motor
-int oldEncR = 0; // Previous encoder values
-int oldEncL = 0;
+InterruptIn EncR(p25);
+InterruptIn EncL(p24);
 int ticksR = 0; // Encoder wheel state change counts
 int ticksL = 0;
-int E; // Error  between the 2 wheel speeds
 float speedL = 0; //PWM speed setting for left wheel (scaled between 0 and 1)
 float speedR = 0; //Same for right wheel
+int dirL = 1; //1 for fwd, -1 for reverse
+int dirR = 1; //1 for fwd, -1 for reverse
 Ticker Sampler; //Interrupt Routine to sample encoder ticks.
-int Instr = 0;
-
+int tracking = 0; //Flag used to indicate right wheel correction
 
 //Sample encoders and find error on right wheel. Assume Left wheel is always correct speed.
 void sampleEncoder()
 {
-    if((Instr == 1 || Instr == 2) && ticksL != 0) { //Make sure the robot is moving forward or reversed and that left wheel hasn't stopped. 
+    float E; // Error  between the 2 wheel speeds
+    if(tracking) { //Right wheel "tracks" left wheel if enabled. 
 
         E = ticksL - ticksR; //Find error
-        speedR = speedR + float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
+        //E = (ticksL+1)/(ticksR+1);
+        if (dirL == 1)
+        {
+            speedR = speedR + float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
+            //speedR = E*speedR;
+            if (speedR < 0) speedR = 0;
+        }
+        else if (dirL == -1)
+        {
+            speedR = speedR - float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
+            //speedR = E*speedR;
+            if (speedR > 0) speedR = 0;
+        }
         rwheel.speed(speedR);
     }
+    //pc.printf("tickL: %i   tickR: %i  Error: %i\n\r ",ticksL, ticksR, ticksL - ticksR);
     ticksR = 0; //Restart the counters
     ticksL = 0;
 }
 
-/*  move(float, int)
-    Usage: Moves the robot forward in a straight line at a constant speed.
-    s is a float from -1 to 1 representing the speed to move. 
-    A negative speed is used for backwards movement.
-    t is the time in milliseconds to move.
-*/
-void move(float s, int t)
-{
-    Timer timer;
-    //Start the timer
-    timer.start();
-    //Set the robot in motion at speed s
-    rwheel.speed(s);
-    lwheel.speed(s);
-    //Give other threads priority until the elapsed time
-    while(timer.read_ms() < t); //Thread::yield();
-    //Stop the robot
-    rwheel.speed(0);
-    lwheel.speed(0);
-}
-
-/*  turn(float,int)
-    Usage: Turns the robot about its center at constant angular speed.
-    s is a float from -1 to 1 representing the speed to turn.
-    A positive s indicates a clockwise rotation.
-    A negative s indicates a counter-clockwise rotation. 
-*/
-void turn(float s, int t)
-{
-    Timer timer;
-    //Start the timer
-    timer.start();
-    //Set the robot in motion turning at speed s
-    rwheel.speed(-1*s);
-    lwheel.speed(s);
-    //Give other threads priority until the elapsed time
-    while(timer.read_ms() < t);// Thread::yield();
-    //Stop the robot
-    rwheel.speed(0);
-    lwheel.speed(0);
-}
-
 int ping(int i)
 {
     int distance = 0;
@@ -145,57 +114,75 @@
     return distance;
 }
 
+void incTicksR()
+{
+    ticksR++;
+}
+
+void incTicksL()
+{
+    ticksL++;
+}
+
 int main() 
 {
+    char cmd;
     timestamp.start(); 
+    angle = 0.0f;
     // Initialize hall-effect control
-    Sampler.attach(&sampleEncoder, 0.02); //Sampler uses sampleEncoder function every 20ms
+    Sampler.attach(&sampleEncoder, 0.2); //Sampler uses sampleEncoder function every 200ms
     EncL.mode(PullUp); // Use internal pullups
     EncR.mode(PullUp);
-    while(1) {
-                Instr = 1;
-                switch (Instr) {
-                    case 0://Stop
-                        lwheel.speed(0);
-                        rwheel.speed(0);
-                        break;
-                    case 1://Forward
-
-                        speedL = 1;
-                        //speedR = speedL;
-                        lwheel.speed(speedL);
-                        rwheel.speed(speedR);
-                        break;
-                    case 2://Reverse
-                        speedL = -0.5;
-                        speedR = speedL;
-                        lwheel.speed(speedL);
-                        rwheel.speed(speedR);
-                        break;
-                    case 3://Clockwise Turn
-                        speedL = 0.3;
-                        lwheel.speed(speedL);
-                        rwheel.speed(-speedL);
-                        break;
-                    case 4://CCW Turn
-                        speedL = -0.3;
-                        lwheel.speed(-speedL);
-                        rwheel.speed(speedL);
-                        break;
-                }//End Switch
-
-        if(Instr == 1 || Instr == 2) { // Only increment tick values if moving forward or reverse
-            if(EncR != oldEncR) { // Increment ticks every time the state has switched. 
-                ticksR++;
-                oldEncR = EncR;
-
+    EncR.rise(&incTicksR);
+    EncL.rise(&incTicksL);
+    while(1) 
+    {
+        //Check if char is ready to be read and put into command buffer;
+        if(bt.getc() =='!')
+        {
+            cmd = bt.getc();
+            switch (cmd)
+            {
+                case 'B':
+                pc.printf("Got Command B!\n\r");
+                break;
+                
+                case 'S': //Stop moving
+                pc.printf("Got Command STOP!\n\r");
+                tracking = 0; //Turn off wheel feedback updates
+                speedL = 0;
+                speedR = 0;
+                lwheel.speed(0);
+                rwheel.speed(0);
+                break;
+                
+                case 'F': //Forward
+                pc.printf("Got Command FORWARD!\n\r");
+                dirL = 1;
+                dirR = 1;
+                speedL = 0.9f;
+                speedR = 0.9f;
+                rwheel.speed(speedR);
+                lwheel.speed(speedL);
+                tracking = 1; //Turn on wheel feedback updates
+                break;
+                
+                case 'R': //Reverse
+                pc.printf("Got Command REVERSE!\n\r");
+                dirL = -1;
+                dirR = -1;
+                speedL = -0.9f;
+                speedR = -0.9f;
+                rwheel.speed(speedR);
+                lwheel.speed(speedL);
+                tracking = 1;
+                break;
+                
+                default:
+                pc.printf("Unknown Command!\n\r");
+                break;
+                
             }
-            if(EncL != oldEncR) {
-                ticksL++;
-                oldEncL = EncL;
-            }
-            pc.printf("tickR: %i \n\r",ticksR);
-            pc.printf("tickL: %i \n\r",ticksL);
         }
     }
 }