Diff: main.cpp

Revision:

0:e36c23af3037

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Oct 18 18:06:15 2014 +0000
@@ -0,0 +1,213 @@
+#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);
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+// LED's blink every pole change degugging like feature
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+void align();
+
+// white wire is output, black is gound, Red is Vout or 3.3 volt
+DigitalIn Enc1(p19); //right motor
+DigitalIn Enc2(p20); //left motor
+float speed;
+//regulate speeds of motors
+float s1 = 0;
+float s2 = 0;
+// this increases speed of motors slightly
+int upspeed1 = 0;
+int upspeed2 = 0;
+// this is for correction that occurs for flag1 right motor speeding up
+// or flage 2 for left motor.
+int flag1 = 0;
+int flag2 = 0;
+
+int hexPos1 = 1;  //4 north and 4 south poles -number 1-8
+int hexPos2 = 1;  // calling them postions on wheel.
+// direction 1 = foward   &  dir = -1 is reverse
+float dir = 1;  //default set to forward
+int state = 0;
+
+
+int main()
+{
+    unsigned char Instr = 0;//What button was sent? 1 = forward, 2 = backward
+    unsigned char sentSpeed; //What is the slider set to?
+    device.baud(57600);
+    Enc1.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
+    Enc2.mode(PullUp);
+    // this tells us the starting N/S poles of the motors
+
+    myled1 = (!Enc1);
+    myled2 = (!Enc2);
+    //intialize motors to start on same pole if not move motor two to correct
+    align();
+
+    while(1) {
+
+
+        if(device.readable()) {
+            Instr = device.getc();
+            pc.printf("Instruction: %i\n\r", Instr);
+            wait(.05);//Wait for the next bluetooth value to be recieved.
+            if(device.readable()) {
+                sentSpeed = device.getc();
+                switch (Instr) {
+                    case 0://Stop
+                        left.speed(0);
+                        right.speed(0);
+                        align();
+                        break;
+                    case 1://Forward
+                        pc.printf("Speed: %i\n\r", sentSpeed);
+                        speed = float(sentSpeed) / 100;
+                        left.speed(speed);
+                        right.speed(speed);
+                        s1 = speed;
+                        s2 = speed;
+                        dir = 1;
+                        break;
+                    case 2://Reverse
+                        pc.printf("Reverse speed: %i\n\r", sentSpeed);
+                        hexPos1 = 0;
+                        hexPos2 = 0;
+                        speed = -float(sentSpeed) / 100;
+                        left.speed(speed);
+                        right.speed(speed);
+                        s1 = speed;
+                        s2 = speed;
+                        dir = -1;
+                        break;
+                    case 3://Clockwise Turn
+                        speed = float(sentSpeed) / 100;
+                        left.speed(speed);
+                        right.speed(-speed);
+                        break;
+                    case 4://CCW Turn
+                        speed = float(sentSpeed) / 100;
+                        left.speed(-speed);
+                        right.speed(speed);
+                        break;
+                }//End Switch
+
+            }//End Speed Read
+        }//End Instruction Read
+
+
+        // forward and reverse Instructions from app are 2 and 1
+        if((Instr == 2) || (Instr == 1)) {
+            // Right motor faster then left
+            if(hexPos1 >= hexPos2) {
+                // this takes out negative values as being < 2
+                if(((hexPos1 - hexPos2) < 2) ) {
+
+                    if(flag2) {
+                        // now that left side is caught up reduce speed increased by half since
+                        // it was slower to bein with.
+                        left.speed(s2 -= (upspeed2*0.01*dir));
+                        flag2 = 0;
+                    }
+
+
+                }
+                if(((hexPos1 - hexPos2) > 2) ) {
+                    // requires corrective action
+                    flag2 = 1;
+                    if(dir*s2<1) {
+                        left.speed(s2 +=(dir*0.02));
+                        upspeed2 += 1;
+                    }
+
+                }
+            }
+
+
+            // Left motor faster then right
+            if(hexPos2 > hexPos1) {
+                // this takes out negative values as being < 2
+                if(((hexPos2 - hexPos1) < 2)) {
+
+                    if(flag1) {
+                        // now that left side is caught up reduce speed increased by half since
+                        // it was slower to bein with.
+                        right.speed(s1 -= (upspeed1*0.01*dir));
+                        flag1 = 0;
+                    }
+
+                }
+                if(((hexPos2 - hexPos1) > 2) ) {
+                    // requires corrective action
+                    flag1 = 1;
+                    if(dir*s1<1) {
+                        right.speed(s1 +=(0.02*dir));
+                        upspeed1 += 1;
+                    }
+
+                }
+            }
+            // keeps count low.
+            if(hexPos1 > 2000 || hexPos2 > 2000) {
+                hexPos1 = hexPos1-1000;
+                hexPos2 = hexPos2-1000;
+            }
+
+
+            // this is for keeping track of the poles and updating LED's so you can see
+            // if the motors are getting out of wack.
+            if(!Enc1) { // I'm assuming this is a postive pole
+                if(!myled1) { // it was a south pole last time and is now north
+                    hexPos1++;
+                    myled3 = !myled3;
+                    myled1 = 1;
+                }
+            } else {
+                if(myled1) { // it was a North pole last time and is now South
+                    hexPos1++;
+                    myled3 = !myled3;
+                    myled1 = 0;
+                }
+            }
+
+            if(!Enc2) { // I'm assuming this is a postive pole
+                if(!myled2) { // it was a south pole last time and is now north
+                    hexPos2++;
+                    myled4= !myled4;
+                    myled2 = 1;
+                }
+            } else {
+                if(myled2) { // it was a North pole last time and is now South
+                    hexPos2++;
+                    myled4 = !myled4;
+                    myled2 = 0;
+                }
+            }
+        }
+
+    }
+}
+
+
+//intialize motors to start on same pole if not move motor two to correct
+void align()
+{
+    if(Enc1 != Enc2) {
+        while(1) {
+            left.speed(.5);
+            if(Enc1 == Enc2) {
+                left.speed(0);
+                myled1 = (!Enc1);
+                myled2 = (!Enc2);
+                hexPos1 = 0;
+                hexPos2 = 0;
+                break;
+            }
+        }
+    }
+}
\ No newline at end of file