Diff: main.cpp

Revision:

0:10883d5ab141

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Oct 19 02:29:13 2014 +0000
@@ -0,0 +1,99 @@
+#include "mbed.h"
+#include "motordriver.h"
+
+Serial pc(USBTX, USBRX);
+Serial device(p9, p10); //Connect JY-MCU bluetooth module to 9,10
+
+Motor  right(p21, p22, p23, 1); // pwm, fwd, rev, has brake feature
+Motor left(p26, p25, p24, 1);
+
+DigitalIn Enc1(p19); //right motor
+DigitalIn Enc2(p20); //left motor
+int oldEnc1 = 0; //Was the encoder previously a 1 or zero?
+int oldEnc2 = 0;
+
+int ticksR = 0; //How many times has the Right wheel encoder changed state? 
+int ticksL = 0; //Same for left wheel
+int E; // the 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
+Ticker Sampler; //Interrupt Routine to sample encoder ticks.
+unsigned char Instr = 0;//What button was sent from app?
+unsigned char sentSpeed = 0; //What is the speed slider set to?
+
+void sampleEncoder(); //Function for sampling and adjusting speeds. 
+
+int main()
+{
+    Sampler.attach(&sampleEncoder, .02); //Sampler uses sampleEncoder function every 20ms
+    Enc1.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
+    Enc2.mode(PullUp);
+    device.baud(57600); //Baud rate of JY-MCU (not Default!) 
+
+    while(1) {
+        if(device.readable()) { //If the bluetooth has sent data, get the instruction
+            Instr = device.getc();
+            wait(.05);//Wait for the next bluetooth value to be recieved.
+            if(device.readable()) {//Now get the speed
+                sentSpeed = device.getc();
+                switch (Instr) {
+                    case 0://Stop
+                        left.speed(0);
+                        right.speed(0);
+                        break;
+                    case 1://Forward
+
+                        speedL = float(sentSpeed) / 100;;
+                        speedR = speedL;
+                        left.speed(speedL);
+                        right.speed(speedR);
+                        break;
+                    case 2://Reverse (not currently implemented in app
+                        speedL = -float(sentSpeed) / 100;;
+                        speedR = speedL;
+                        left.speed(speedL);
+                        right.speed(speedR);
+                        break;
+                    case 3://Clockwise Turn
+                        speedL = float(sentSpeed) / 100;
+                        left.speed(speedL);
+                        right.speed(-speedL);
+                        break;
+                    case 4://CCW Turn
+                        speedL = float(sentSpeed) / 100;
+                        left.speed(-speedL);
+                        right.speed(speedL);
+                        break;
+                }//End Switch
+
+            }//End Speed Read
+        }//End Instruction Read
+        if(Instr == 1 || Instr == 2) { // Only increment tick values if moving forward or reverse
+            if(Enc1 != oldEnc1) { // Increment ticks every time the state has switched. 
+                ticksR++;
+                oldEnc1 = Enc1;
+
+            }
+            if(Enc2 != oldEnc2) {
+                ticksL++;
+                oldEnc2 = Enc2;
+            }
+        }
+
+    }
+}
+
+//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. 
+
+        E = ticksL - ticksR; //Find error
+        speedR = speedR + float(E) / 255.0f; //Assign a scaled increment to the right wheel based on error
+        if(Instr == 1) speedR = speedR + float(E) / 255.0f;
+        //Reverse not implemented.  
+        right.speed(speedR);
+    }
+    ticksR = 0; //Restart the counters
+    ticksL = 0;
+}
\ No newline at end of file