Xbee controller for robot with IMU/joystick
Dependencies: 4DGL-uLCD-SE LSM9DS1_Library_cal mbed
Fork of LSM9DS1_Demo_wCal by
main.cpp
- Committer:
- etorres31
- Date:
- 2016-11-04
- Revision:
- 1:036b11214212
- Parent:
- 0:e693d5bf0a25
File content as of revision 1:036b11214212:
#include "mbed.h" #include "LSM9DS1.h" #define PI 3.14159 #include "uLCD_4DGL.h" #include "mbed.h" Serial xbee1(p13, p14); DigitalOut rst1(p11); Serial pc(USBTX, USBRX); DigitalOut myled(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); BusOut mbedleds(LED1,LED2,LED3,LED4); //BusOut/In is faster than multiple DigitalOut/Ins class Nav_Switch { public: Nav_Switch(PinName up,PinName down,PinName left,PinName right,PinName fire); int read(); //boolean functions to test each switch bool up(); bool down(); bool left(); bool right(); bool fire(); //automatic read on RHS operator int (); //index to any switch array style bool operator[](int index) { return _pins[index]; }; private: BusIn _pins; }; Nav_Switch::Nav_Switch (PinName up,PinName down,PinName left,PinName right,PinName fire): _pins(up, down, left, right, fire) { _pins.mode(PullUp); //needed if pullups not on board or a bare nav switch is used - delete otherwise wait(0.001); //delays just a bit for pullups to pull inputs high } inline bool Nav_Switch::up() { return !(_pins[0]); } inline bool Nav_Switch::down() { return !(_pins[1]); } inline bool Nav_Switch::left() { return !(_pins[2]); } inline bool Nav_Switch::right() { return !(_pins[3]); } inline bool Nav_Switch::fire() { return !(_pins[4]); } inline int Nav_Switch::read() { return _pins.read(); } inline Nav_Switch::operator int () { return _pins.read(); } Nav_Switch myNav(p29,p26,p27,p25, p28); //pin order on Sparkfun breakout float y=0; float z=0; int main() { LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); IMU.begin(); if (!IMU.begin()) { pc.printf("Failed to communicate with LSM9DS1.\n"); } IMU.calibrate(1); wait(3); rst1 = 0; //Set reset pin to 0 myled = 1; //myled2= 0; wait_ms(1); rst1 = 1; //Set reset pin to 1 wait_ms(1); while(1) { int a;//getKeyNum(); int b; while(!IMU.accelAvailable()); IMU.readAccel(); //These conditional statements create a "dead-zone" for the wheel, to make it less //sensitive and more usable. Additionally, the IMU data will never exceed .9 to so //as to limit the maximum speed and steering for the robot. if(IMU.calcAccel(IMU.ay)<-.1) { b = 1; y=.9; a = 4; wait(.5); } else if (IMU.calcAccel(IMU.ay)>.1) { b = 1; y=-.9; a = 3; wait(.5); //} else if(IMU.calcAccel(IMU.ay)<=.1||IMU.calcAccel(IMU.ay)>=-.1) { // y=IMU.calcAccel(IMU.ay); } else if(IMU.calcAccel(IMU.ax)>.1) { b = 1; z=.9; a = 2; wait(.5); } else if((IMU.calcAccel(IMU.ax)<-.1)) { b = 1; z=IMU.calcAccel(IMU.az);; a = 1; wait(.5); } else { a = 0; b = 0; } if (b == 0) { //with pullups a button hit is a "0" - "~" inverts data to leds mbedleds = ~(myNav & 0x0F); //update leds with nav switch direction inputs if(myNav.fire()) mbedleds = 0x0F; //special all leds on case for fire (center button) //or use - if(myNav[4]==0) mbedleds = 0x0F; //can index a switch bit like this //wait(0.02); if (myNav[0] == 0) { a = 1; } else if (myNav[1] == 0) { a = 2; } else if (myNav[2] == 0) { a = 3; } else if (myNav[3] == 0) { a = 4; } else if (myNav[4] == 0) { a = 5; } else { a = 0; } } if(a!=-1){ //myled2 = 1; xbee1.putc(a); //XBee write //myled2 = 0; //myled = 1; wait(.2); } } }