File content as of revision 1:44b71d096f7a:

#include "mbed.h"
#include "LSM9DS1.h"
#include "rtos.h"
#include "PinDetect.h"


Serial xbee1(p9, p10);
DigitalOut rst1(p11);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalOut haptic(p7);
Serial pc(USBTX, USBRX);
PinDetect shoot(p8);
PinDetect reload(p21);
LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
Mutex mutex;

//Global Variables
int init_x;
int init_y;
int x = 64;
int y = 64;
int old_x;
int old_y;
int dx;
int dy;
int shots_fired = 0;
bool reload_checksum = 0;
bool send_shoot = 0;
bool send_reload = 0;

//Function Declarations
void computeCursorCoordinates();
void sendCursorSwitchAndPB();
void receiveLEDsAndHaptic();
void setShoot();
void setReload();

int main() {
    
    // Initialize IMU
    imu.begin();
    if (!imu.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    imu.calibrate();
    imu.readAccel();
    init_x = imu.ax;
    init_y = imu.ay;
    
    
    //Initialize Xbee
    rst1 = 0;//Set reset pin to 0
    wait_ms(1);
    rst1 = 1;//Set reset pin to 1
    wait_ms(1);
    
    //Initialize LEDs and Haptic
    haptic = 0;
    led1 = 1;
    led2 = 1;
    led3 = 1;
    led4 = 1;
    
    //Initialize Shoot PB
    shoot.mode(PullDown);
    shoot.attach_asserted( &setShoot );
    shoot.setSampleFrequency(2000); // Defaults to 20ms.
    
    //Initialize Reload Switch
    reload.attach_asserted( &setReload );
    reload.setSampleFrequency(2000); // Defaults to 20ms.

    //Initialize Threads
    Thread thread1(receiveLEDsAndHaptic);
    Thread thread2(sendCursorSwitchAndPB);
    while(1){}
}

/****************************Helper Functions**********************************/


void computeCursorCoordinates() {
    old_x = x;
    old_y = y;
    imu.readAccel();
    imu.readMag();
    
    dx = -imu.ay/2000;
    x = old_x + dx;
    dy = -imu.ax/2000;
    y = old_y + dy;
    
    // Set boundaries for Cursor
    if(x<10)   { x=10;  }
    if(y<23)   { y=23;  }
    if(x>117)  { x=117; }
    if(y>117)  { y=117; }
}

void sendCursorSwitchAndPB() {
    while(1) { 
        computeCursorCoordinates();
        int x0 = (x/100);
        int x1 = ((x%100)/10);
        int x2 = ((x%100)%10);
        int y0 = (y/100);
        int y1 = ((y%100)/10);
        int y2 = ((y%100)%10);
        mutex.lock();
        if(xbee1.writeable()) {
            xbee1.putc('@');
            xbee1.putc(x0+48);
            xbee1.putc(x1+48);
            xbee1.putc(x2+48);
            xbee1.putc(y0+48);
            xbee1.putc(y1+48);
            xbee1.putc(y2+48);
            
            //Send Reload Switch - Only send true if rising edge
            xbee1.putc(send_reload+48);
            reload_checksum = send_reload;
            if(send_reload) {
                pc.printf("RELOAD\n");
            }
            
            //Send Shoot PB
            xbee1.putc(send_shoot+48);
            if(send_shoot==1) {
                shots_fired++;
                pc.printf("Shots Fired: %d\n",shots_fired);
            }
            
            //Calculate and Send Checksum
            int sum = x+y+send_shoot+reload_checksum;
            int sum0 = (sum/100);
            int sum1 = ((sum%100)/10);
            int sum2 = ((sum%100)%10);
            xbee1.putc(sum0+48);
            xbee1.putc(sum1+48);
            xbee1.putc(sum2+48);
            
            //Only send shoot and reload flags once
            send_shoot = 0;
            send_reload = 0;
        }
        mutex.unlock();
        Thread::wait(5);
    }
}

void receiveLEDsAndHaptic() {
    while(1) {
        if(xbee1.readable()){
            char X;
            mutex.lock();
            X = xbee1.getc();
            mutex.unlock();
            if(X == '#') { 
                mutex.lock();
                X = xbee1.getc()-48;
                mutex.unlock();
                led1 = X & (1<<4);
                led2 = X & (1<<3);
                led3 = X & (1<<2);
                led4 = X & (1<<1);
                haptic = X & (1<<0);
            }
        }
        Thread::wait(500);
    }
}

void setShoot() {
    mutex.lock();
    send_shoot = 1;
    mutex.unlock();
}

void setReload() {
    mutex.lock();
    send_reload = 1;
    mutex.unlock();
}