Code for Wireless IMU Glove Controller used for SharpShooter II
Dependencies: LSM9DS1_Library PinDetect mbed-rtos2 mbed
main.cpp
- Committer:
- jboettcher
- Date:
- 2016-12-07
- Revision:
- 1:44b71d096f7a
- Parent:
- 0:f3043bee193a
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(); }