File content as of revision 0:67b964961693:

#include "mbed.h"
#include <motor.h>
#include "rtos.h"

#define TX        p9
#define RX        p10

#define encoder_baud        9600
#define encoder_resolution  (1 << 12)
#define resolution 12

float encoderPositionToAngle(uint16_t position);
bool checksum(uint16_t data);

Serial pc(USBTX, USBRX); // tx, rx
Serial encoder(TX, RX, encoder_baud);

Motor motor0(p5,p6,p7);
Motor motor1(p15,p16,p17);
Motor motor2(p18,p19,p20);  //pull,drive,enable
Motor motor3(p21,p22,p23);
Motor motor4(p24,p25,p26);
Motor motor5(p27,p28,p29);

const int HIGH = 1;
const int LOW  = 0;

//@@@@@@@@@@@@@@@__CONTROL__@@@@@@@@@@@@@@@@@@@
const int PPR = 400;    //Pulses Per Revolution
int ROTATION = 36000;   //3600 full rotation
int RPM = 10;            //Rotations Per Minute
int MOVEMENT = 0;       //Movement Type |0=F ,1=B ,2=R ,3=L|
int NUMSTEPS = 10;       //Number of Steps
//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

int pulseLength = 0;

//Converts desired RPM to required Pulse Length (In Microseconds)
void Configure_Pulse(float rpm){
    float holder = 0;           //Create holder variable (MBED is Stupid)

    holder = rpm;               //Set to RPM
    holder = holder/60;         //Brain.exe is no longer responding ;(
    holder = holder*PPR;        //Multiply by PPR
    holder = holder/2;          //Brain.exe is no longer responding ;(
    holder = 1/holder;          //Convert to Seconds
    holder = holder*1000000;    //Convert to Microseconds
    holder = holder/60;         //Dividing by Gear Ratio
    holder = holder/4;          //Math Magic
    
    pulseLength = holder;       
}

//Sets Drive Pins to Movement Configuration
void Configure_Movement(int input){
    switch(input){
        //Move Forward
        case 0: motor0.dir = HIGH;
                motor1.dir = HIGH;
                motor2.dir = HIGH;
                motor3.dir = HIGH;
                motor4.dir = HIGH;
                motor5.dir = HIGH;
            break;
        //Move Backward
        case 1: motor0.dir = LOW;
                motor1.dir = LOW;
                motor2.dir = LOW;
                motor3.dir = LOW;
                motor4.dir = LOW;
                motor5.dir = LOW;
            break;
        //Turn Right
        case 2: motor0.dir = HIGH;
                motor1.dir = HIGH;
                motor2.dir = HIGH;
                motor3.dir = LOW;
                motor4.dir = LOW;
                motor5.dir = LOW;
            break;
        //Turn Left
        case 3: motor0.dir = LOW;
                motor1.dir = LOW;
                motor2.dir = LOW;
                motor3.dir = HIGH;
                motor4.dir = HIGH;
                motor5.dir = HIGH;
            break;
    }
}

void motorFunc(){
    Configure_Movement(MOVEMENT);
    Configure_Pulse(RPM);
    bool altSet = false;    //
    int inv0 = 0, inv1 = 0; //Creates inverter variables for pulse controll
    
    //Loop for Number of Steps Made
    for(int i=0; i<NUMSTEPS; i++){
        if(altSet == false){
            //Pulses for total desired Rotation for set0
            for(int j=0; j<ROTATION; j++){
                inv0 =~ inv0;   //Inverts Value
                motor0.pul = inv0;
                motor2.pul = inv0;
                motor4.pul = inv0;
                
                //Pulses set1 every 3 pulses of set0
                if(j%3 == 0){
                    inv1 =~ inv1;   //Inverts Value
                    motor1.pul = inv1;
                    motor3.pul = inv1;
                    motor5.pul = inv1;
                }
                wait_us(pulseLength);
             }
             altSet = false;    //THIS IS WHAT I CHANGED
        }else if(altSet == true){
             //Pulses for total desired Rotation for set1
             for(int j=0; j<ROTATION; j++){
                inv1 =~ inv1;   //Inverts Value
                motor1.pul = inv1;
                motor3.pul = inv1;
                motor5.pul = inv1;
                
                //Pulses set0 every 3 pulses of set1
                if(j%3 == 0){
                    inv0 =~ inv0;   //Inverts Value
                    motor0.pul = inv0;
                    motor2.pul = inv0;
                    motor4.pul = inv0;
                }
                wait_us(pulseLength);
            }
             altSet = false;
        }
    }   
}

void encoderFunc(){
    pc.printf("Hello World!\n\r");
    char D[1] = {0x00};
    //char G[1] = {0xFF};
    uint16_t position = 0;
    pc.printf("Talking to Encoder\n\r");
    while(1) {
        encoder.putc(D[0]);
        pc.printf("MSG");
        if(encoder.readable()) {
            position = encoder.getc();  //Lower Byte
            position |= encoder.getc()<< 8;    //Upper Byte
            
            //if(!(checksum(position)==1)){
                position &= 0x3FFF;             //Removing Check Bits
                if (resolution == 12) position = position >> 2;
                float angle = encoderPositionToAngle(position);
                pc.printf("Position: %d\n\rAngle: %f\n\r",position, angle);
                pc.printf("\n\r\n\r\n\r");
                if(angle<180 && angle>160){
                    //motor1.ena=LOW;
                    pc.printf("STOP");
                }else{
                    //motor1.ena=HIGH;   
                    pc.printf("GO");
                }
            //}/*else{
                //pc.printf("FAILED CHECKSUM!");
                //pc.printf("\n\r\n\r\n\r");
          //  }*/
            
        }
        wait(0.5);
    }
    
    /*
    pc.printf("Hello World!\n\r");
    Encoder encoder(0xF0);
    while(1){
        pc.printf("Talking to Encoder\n\r");   
        encoder.updatePosition();
        pc.printf("Position: %d",encoder.getPosition(););
        pc.printf("\n\r\n\r\n\r");
    }
    */
}


Thread encoderThread;
Thread motorThread;

int main() {
    encoderThread.start(encoderFunc);
    motorThread.start(motorFunc);
    
}
//Assuming Odd Parity
bool checksum(uint16_t data){
    bool checksum = 0;
    uint8_t cbe = 0;    //Even Checker
    uint8_t cbo = 0;    //Odd Checker
    
    if(data & (1<<14)){ cbe++; }
    if(data & (1<<15)){ cbo++; }
    
    for(int i=0; i<8; i++){
        if(data & (1<<i*2)){ cbe++; }       //Checking Even Bits
        if(data & (1<<(i*2+1))){ cbo++; }   //Checking Odd Bits
    }
    
    if(!(cbe%2==1 && cbo%2==1)){ checksum = 1; }
    return checksum;
}

float encoderPositionToAngle(uint16_t position){
    return (float)position/11.3777;
}