File content as of revision 0:eae9712515a6:

#include "mbed.h"
#include "motordriver.h"
#include "Servo.h"
#include "HMC6352.h"

HMC6352 compass(p9, p10);
//Smartbot

DigitalOut mbedled1(LED1); //signals draw mode
DigitalOut mbedled2(LED2); //singals relocate mode
DigitalOut mbedled3(LED2); //signals left wheel movement
DigitalOut mbedled4(LED2); //signals right wheel movement

Serial pc(USBTX, USBRX); // tx, rx
Serial controller(p13, p14); //change to whatever rcv method we use

//See http://mbed.org/cookbook/Motor
//http://mbed.org/users/4180_1/notebook/sparkfun-magician-robot-base-kit/
//Connections to dual H-brdige driver for the two drive motors
Motor left(p21, p22, p20, 1); // pwm, fwd, rev, has brake feature
Motor right(p24, p25, p27, 1);

Servo penservo(p23);

int main()
{
    //init command vars
    char command = 0; //bit0 = 1 if shake; bit7 = 0 if y, 1 if x; bit6 = 0 if pos, 1 if neg
    char XorY = 128; // X or Y move command bitmask
    char NorP = 64; // Down or Up move command bitmask
    char mag = 62; // bitmask for magnitude of move command
    char magnitude = 0; //magnitude of move

    //Direction and Movement Variables, start on page 0 facing +y with dx, dy =0
    float nphi, myphi, dphi = 0.0; //angle math, (36)0 = +y, 90 = +x, 180 = -y, 270 = -x
    float dy, dx = 0.0; // where on page?
    float ny, nx = 0.0; // next move command coords
    //int pagesize = 1050; //square page side draw length + border
    float moveweight = 0; //length of current move order, effects speed
    int comcount = 4; //number of move commands to add into 1 move order
   // int flip = 0; //backwards to save time?
    int i;
    
    //Init Servo
    penservo = .5;

    //Initialize LEDs
    mbedled1 = 1;
    mbedled2 = 0;

    //Init IR Receiver
    controller.baud(2400);
    wait(1);
    
    //Init Compass to continuous mode, periodic set/reset, 20Hz measurement rate.
    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    wait(3);
    
    //Init Compass reading to 0
    for (i = 0; i < 5; i++) {  //Take Averaged Compass reading
        myphi += compass.sample()/50.0;
        wait(.05);
    }
    dphi = nphi - myphi;               // calc angle diff
    while (abs(dphi) > 15) {           // if diff sig, turn to 0
        myphi = 0.0;
        left.speed(-.5);
        for (i = 0; i < 3; i++) {  //Take Averaged Compass reading
        myphi += compass.sample()/30.0;
        wait(.01);
        }
        dphi = nphi - myphi;  
        wait(.01);             
    }         
    right.stop(1);                   //stop wheel(s) after turn 
    left.stop(1);              
    wait(.5);
    
    //pc.printf("myphi: %f", myphi); 
    //pc.printf("starting \n");

    while(1) {

        //if readable, get command
        if(controller.readable()) {
            mbedled2 = 1;
            command = controller.getc(); //Serial read
            comcount--;
            //pc.printf("%x \n", (command));
            mbedled2 = 0;
        }   else if (comcount < 4) { //decrement if command countdown started even if no command read
            command = 0;
            comcount--;
        }   else {
            command = 0;
        }

        //check for shake
        if (command==1) { //lift pen up and reposition
            mbedled1 = 0;
            penservo = 1;
            wait(3);
            dx = 0; //reset vars after reposition
            dy = 0;
            comcount = 5;
            mbedled1 = 1;
            penservo = .5;
        }

        //check for move command
        if ((abs(command) > 1) && (comcount > 0)) {
            magnitude = mag & command;
            if ( (command & XorY) == 128 ) {        // if bit7 then X axis
                if ( (command & NorP) == 64 ) {         // if bit6 then -x
                   // nx -= (.3+magnitude/16.0)/5.0;
                   nx -= magnitude / 4.0;
                } else {                                //else if !bit6 then +x
                   // nx += (.3+magnitude/16.0)/5.0;
                   nx += magnitude / 4.0;
                }
            } else {                                //else if !bit7 then Y axis
                if ( (command & NorP) == 64 ) {         // if bit6 then -y
                   // ny -= (.3+magnitude/16.0)/5.0;
                   ny -= magnitude / 4.0;
                } else {                                //else if !bit6 then +y
                   // ny += (.3+magnitude/16.0)/5.0;
                   ny += magnitude / 4.0;
                }
            }
            wait(.05);
        } else if (comcount == 0) { //move math
            if ((nx != 0) && (ny != 0)) {    // if both x&y change, calculate vector angle and length in polar coords
                dx += nx;
                dy += ny;
                moveweight = pow(ny, 2) + pow(nx, 2);   //moveweight = r
                moveweight = sqrt(moveweight);
                nphi = atan2(nx, ny);                   //new phi = principal arctan from -180 to 180. x and y flipped to get angle from y to x
                //pc.printf("POLAR myphi: %f, newphi: %f, movelength: %f \n", myphi, nphi, moveweight); 
            } else {
                dphi = 0;
                moveweight = 0;
            }
            
            for (i = 0; i < 3; i++) {  //Take Averaged Compass reading
                myphi += compass.sample()/30.0;
                wait(.01);
            }
            
            if (abs(nx) < 2) {                   //if only signif. y axis move
                moveweight = abs(ny);                        // set speed
                if (ny < -2) {nphi = 180;}         // set angles to axis 
                else if (ny > 2) {nphi = 0;}
                else {nphi = myphi;
                    moveweight = 0;}
            }
    
            if (abs(ny) < 2) {                   //if only signif. x axis move
                moveweight = abs(nx);
                if (nx < -2) {nphi = 270;}
                else if (nx > 2) {nphi = 90;}
                else {nphi = myphi;
                    moveweight = 0;}
            }
             
            dphi = nphi - myphi;               // calc angle diff
            //if (abs(dphi) > 340) { dphi = 0;}        // fix pos y axis
            /*if ((160 < abs(dphi)) && (abs(dphi)< 200)) {
                moveweight = -moveweight;
                dphi = 0;}       //flip speed instead of flip around
            if (dphi > 180) {                 // switch angle diff with mag above 180 and flip
                nphi -= 180;
                 moveweight = -moveweight;   //flip speed
            }
            if (dphi < -180) {
                nphi += 180;
                 moveweight = -moveweight;      //flip speed
            }
            */
            
            
            if (abs(dphi) > 15) { //turn left
                right.stop(1);                   //stop wheels before turn 
                left.stop(1);
                wait(.1);
                while (abs(dphi) > 15) {           // if diff sig, turn to 0
                    myphi = 0.0;
                    left.speed(-.5);
                    for (i = 0; i < 2; i++) { //Take quick Averaged Compass reading
                    myphi += compass.sample()/20.0;
                    wait(.01);}
                    dphi = nphi - myphi;               
                }         
                right.stop(1);                   //stop wheels after turn 
                left.stop(1);              
                wait(.1);
            }                         
            
            while (moveweight >= 1) {   //go fwd for r
                left.speed(.7);  
                right.speed(.7);
                wait(.1);
                moveweight--;
                //moveweight = moveweight/2;
            }
            
            while (moveweight <= -1) {   //go back for r
                left.speed(-.7);  
                right.speed(-.7);
                wait(.1);
                moveweight++;
                //moveweight = moveweight/2;
            }
    
            comcount = 4;            //reset vars
            nx = 0;
            ny = 0;
            moveweight = 0;
            dphi = 0;

        } else { //else stop
            left.stop(1);
            right.stop(1);
            wait(.1);
        }
    }
}