File content as of revision 4:39e949908fc5:

#include "mbed.h"
#include "C12832.h"
#include "LM75B.h"
#include "MMA7660.h"
#include "FXOS8700Q.h"
#include "comms.h"
#include "CalibrateMagneto.h"

//OUTPUTS

DigitalOut lr(PTB22), lg(PTE26), lb(PTB21), xr(D5), xg(D9), xb(PTC12);
C12832 lcd(D11, D13, D12, D7, D10);

//INPUTS

LM75B temp(D14, D15);
MMA7660 sAcc(D14, D15);

FXOS8700Q_acc bAcc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // Proper Ports and I2C Address for K64F Freedom board
FXOS8700Q_mag bMag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // Proper Ports and I2C Address for K64F Freedom board
CalibrateMagneto magCal;

InterruptIn sw2_int(SW2), sw3_int(SW3), up_int(A2), down_int(A3), left_int(A4), right_int(A5), fire_int(D4);
DigitalIn   sw2(SW2),     sw3(SW3),     up(A2),     down(A3),     left(A4),     right(A5),     fire(D4);

AnalogIn pot1(A0), pot2(A1);

//INTERRUPT FLAGS

uint16_t flags;
#define SW2_DOWN   0x0001
#define SW2_UP     0x0002
#define SW3_UP     0x0004
#define SW3_DOWN   0x0008
#define UP_UP      0x0010
#define UP_DOWN    0x0020
#define DOWN_UP    0x0040
#define DOWN_DOWN  0x0080
#define LEFT_UP    0x0100
#define LEFT_DOWN  0x0200
#define RIGHT_UP   0x0400
#define RIGHT_DOWN 0x0800
#define FIRE_UP    0x1000
#define FIRE_DOWN  0x2000

//COMMS

DataSerial pc(USBTX, USBRX);

//INTERRUPT HANDLERS

void sw2Down(){ flags |= SW2_DOWN; }
void sw2Up()  { flags |= SW2_UP; }

void sw3Down(){ flags |= SW3_DOWN; }
void sw3Up()  { flags |= SW3_UP; }

void upDown(){ flags |= UP_DOWN; }
void upUp()  { flags |= UP_UP; }

void downDown(){ flags |= DOWN_DOWN; }
void downUp()  { flags |= DOWN_UP; }

void leftDown(){ flags |= LEFT_DOWN; }
void leftUp()  { flags |= LEFT_UP; }

void rightDown(){ flags |= RIGHT_DOWN; }
void rightUp()  { flags |= RIGHT_UP; }

void fireDown(){ flags |= FIRE_DOWN; }
void fireUp()  { flags |= FIRE_UP; }

//PRETTY WAITING LEDS (Modified from Fred Barnes)

Ticker waitTicker;
int ctr;

void waitTick() {
    switch(ctr) {
        case 0:
            xr=xg=0; xb=1; break;
        case 1:
            xr=0; xg=xb=1; break;
        case 2:
            xr=xb=0; xg=1; break;
        case 3:
            xb=0; xr=xg=1; break;
        case 4:
            xg=xb=0; xr=1; break;
        case 5:
            xg=0; xr=xb=1; ctr=0; return;
    }
    ctr++;
}

void cleanup() {
    lr = lg = lb = 1;
    xr = xg = xb = 1.0;
    temp.read();
    lcd.cls();
    
    bAcc.enable();
    bMag.enable();
    
    pc.sendChar('+');
}

/*
Return the state of a requested pin
*/
void readDigitalIn() {
    bool val;
    switch(pc.getc()) {
        case '2': val=sw2; break;
        case '3': val=sw3; break;
        case 'u': val=up; break;
        case 'd': val=down; break;
        case 'l': val=left; break;
        case 'r': val=right; break;
        case 'f': val=fire; break;
        default: return;
    }
    char c = val?'t':'f';
    pc.sendChar(c);
}

/*
Return the state of a requested potentiometer
*/
void readPot() {
    float val;
    switch(pc.getc()) {
        case '1': val=pot1; break;
        case '2': val=pot2; break;
        default: return;
    }
    
    pc.sendFloat(val);
}

float getHeading() {
    MotionSensorDataUnits mag;
    bMag.getAxis(mag);
    float cal[3];
    cal[0]=mag.x;
    cal[1]=mag.y;
    cal[2]=mag.z;
    magCal.run(cal,cal);
    return atan2(-cal[1],cal[0])-0.4333f;
}

int main() {  
    //SETUP
    pc.baud(9600);
    
    lr = lg = lb = 1;
    xr = xg = xb = 1.0;
    
    sw2_int.mode (PullUp);
    sw2_int.fall(&sw2Down);
    sw2_int.rise(&sw2Up);
    
    sw3_int.mode (PullUp);
    sw3_int.fall(&sw3Down);
    sw3_int.rise(&sw3Up);
    
    up_int.mode (PullUp);
    up_int.rise(&upDown);
    up_int.fall(&upUp);
    
    down_int.mode (PullUp);
    down_int.rise(&downDown);
    down_int.fall(&downUp);
    
    left_int.mode (PullUp);
    left_int.rise(&leftDown);
    left_int.fall(&leftUp);
    
    right_int.mode (PullUp);
    right_int.rise(&rightDown);
    right_int.fall(&rightUp);
    
    fire_int.mode (PullUp);
    fire_int.rise(&fireDown);
    fire_int.fall(&fireUp);
    
    //Pretty wait
    ctr=0;
    lcd.locate(0,0);
    lcd.printf("Awaiting connection...");
    waitTicker.attach(&waitTick,0.5);
    while((!pc.readable()) && (pc.getc()!='-')) sleep();
    waitTicker.detach();
    cleanup();
    
    for(;;) {
        //Check if any interrupts fired
        if(flags&SW2_UP) {
            flags&=!SW2_UP;
            pc.sendSpecialCommand('2','u');
        }
        if(flags&SW2_DOWN) {
            flags&=!SW2_DOWN;
            pc.sendSpecialCommand('2','d');
        }
        if(flags&SW3_UP) {
            flags&=!SW3_UP;
            pc.sendSpecialCommand('3','u');
        }
        if(flags&SW3_DOWN) {
            flags&=!SW3_DOWN;
            pc.sendSpecialCommand('3','d');
        }
        if(flags&UP_UP) {
            flags&=!UP_UP;
            pc.sendSpecialCommand('u','u');
        }
        if(flags&UP_DOWN) {
            flags&=!UP_DOWN;
            pc.sendSpecialCommand('u','d');
        }
        if(flags&DOWN_UP) {
            flags&=!DOWN_UP;
            pc.sendSpecialCommand('d','u');
        }
        if(flags&DOWN_DOWN) {
            flags&=!DOWN_DOWN;
            pc.sendSpecialCommand('d','d');
        }
        if(flags&LEFT_UP) {
            flags&=!LEFT_UP;
            pc.sendSpecialCommand('l','u');
        }
        if(flags&LEFT_DOWN) {
            flags&=!LEFT_DOWN;
            pc.sendSpecialCommand('l','d');
        }
        if(flags&RIGHT_UP) {
            flags&=!RIGHT_UP;
            pc.sendSpecialCommand('r','u');
        }
        if(flags&RIGHT_DOWN) {
            flags&=!RIGHT_DOWN;
            pc.sendSpecialCommand('r','d');
        }
        if(flags&FIRE_UP) {
            flags&=!FIRE_UP;
            pc.sendSpecialCommand('f','u');
        }
        if(flags&FIRE_DOWN) {
            flags&=!FIRE_DOWN;
            pc.sendSpecialCommand('f','d');
        }
        
        //Check for host requests
        if(pc.readable()) {
            char cmd = pc.getc();
            switch(cmd) {
                case 'R': { //read digital input
                    readDigitalIn();
                    break;
                }
                case 'p': {
                    readPot();
                    break;
                }
                case 't': { //read temp
                    float t = temp.read();
                    pc.sendFloat(t);
                    break;
                }
                case 'a': {//read shield accel
                    float x, y, z;
                    x = sAcc.x();
                    y = sAcc.y();
                    z = sAcc.z();
                    pc.sendFloat(x);
                    pc.sendFloat(y);
                    pc.sendFloat(z);
                    break;
                }
                case 'l': { //shield led
                    char raw = pc.getc();
                    char r = ((raw>>2)&1)^1;
                    char g = ((raw>>1)&1)^1;
                    char b = (raw&1)^1;
                    xr = r; xg = g; xb = b;
                    break;
                }
                case 'o': { //read shield accel orientation
                    MMA7660::Orientation o = sAcc.getOrientation();
                    pc.sendChar((char)o);
                    break;
                }
                case 's': { //read shield accel orientation
                    MMA7660::Orientation o = sAcc.getSide();
                    pc.sendChar((char)o);
                    break;
                }
                case 'h': {
                    float f = getHeading();
                    pc.sendFloat(f);
                    break;
                }
                case 'L': { // board led
                    char raw = pc.getc();
                    char r = ((raw>>2)&1)^1;
                    char g = ((raw>>1)&1)^1;
                    char b = (raw&1)^1;
                    lr = r; lg = g; lb = b;
                    break;
                }
                case 'M': { //read board magno
//                    float f;
//                    bMag.getX(&f); pc.sendFloat(f);
//                    bMag.getY(&f); pc.sendFloat(f);
//                    bMag.getZ(&f); pc.sendFloat(f);
                    MotionSensorDataUnits mag;
                    bMag.getAxis(mag);
                    float cal[3];
                    cal[0]=mag.x;
                    cal[1]=mag.y;
                    cal[2]=mag.z;
                    magCal.run(cal,cal);
                    pc.sendFloat(cal[0]);
                    pc.sendFloat(cal[1]);
                    pc.sendFloat(cal[2]);
                    break;
                }
                case 'A': { //read board accel
                    float f;
                    bAcc.getX(&f); pc.sendFloat(f);
                    bAcc.getY(&f); pc.sendFloat(f);
                    bAcc.getZ(&f); pc.sendFloat(f);
                    break;
                }
                case 'P': { //set lcd position
                    unsigned char x, y;
                    x = pc.getc();
                    y = pc.getc();
                    lcd.locate(x,y);
                    break;
                }
                case 'S': { //print lcd text
                    char buff[256];
                    unsigned char len = pc.getc();
                    char * j= buff;
                    while(len>0) {
                        *j++=pc.getc();
                        len--;
                    }
                    *j=0;
                    lcd.printf(buff);
                    lcd.copy_to_lcd();
                    break;
                }
                case 'D': { //set lcd pixel
                    char x = pc.getc();
                    char y = pc.getc();
                    char c = pc.getc();
                    lcd.pixel(x,y,c);
                    lcd.copy_to_lcd();
                    break;
                }
                case 'C': { //clear lcd
                    lcd.cls();
                    break;
                }
                case '-': { //reconnection
                    cleanup(); break;
                }
                default: break;
            }
        }
    }
}