File content as of revision 0:e36c23af3037:

#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;
            }
        }
    }
}