Edwin Torres / Mbed 2 deprecated Xbee_Controller

Xbee controller for robot with IMU/joystick

Dependencies:   4DGL-uLCD-SE LSM9DS1_Library_cal mbed

Fork of LSM9DS1_Demo_wCal by jim hamblen

main.cpp@1:036b11214212, 2016-11-04 (annotated)

Committer:
etorres31
Date:
Fri Nov 04 16:28:04 2016 +0000
Revision:
1:036b11214212
Parent:
0:e693d5bf0a25 
Xbee controller code;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
4180_1 0:e693d5bf0a25 1 #include "mbed.h"
4180_1 0:e693d5bf0a25 2 #include "LSM9DS1.h"
4180_1 0:e693d5bf0a25 3 #define PI 3.14159
etorres31 1:036b11214212 4 #include "uLCD_4DGL.h"
etorres31 1:036b11214212 5 #include "mbed.h"
4180_1 0:e693d5bf0a25 6
etorres31 1:036b11214212 7 Serial xbee1(p13, p14);
etorres31 1:036b11214212 8 DigitalOut rst1(p11);
etorres31 1:036b11214212 9 Serial pc(USBTX, USBRX);
4180_1 0:e693d5bf0a25 10 DigitalOut myled(LED1);
etorres31 1:036b11214212 11 DigitalOut myled2(LED2);
etorres31 1:036b11214212 12 DigitalOut myled3(LED3);
etorres31 1:036b11214212 13 BusOut mbedleds(LED1,LED2,LED3,LED4);
etorres31 1:036b11214212 14 //BusOut/In is faster than multiple DigitalOut/Ins
etorres31 1:036b11214212 15
etorres31 1:036b11214212 16 class Nav_Switch
etorres31 1:036b11214212 17 {
etorres31 1:036b11214212 18 public:
etorres31 1:036b11214212 19 Nav_Switch(PinName up,PinName down,PinName left,PinName right,PinName fire);
etorres31 1:036b11214212 20 int read();
etorres31 1:036b11214212 21 //boolean functions to test each switch
etorres31 1:036b11214212 22 bool up();
etorres31 1:036b11214212 23 bool down();
etorres31 1:036b11214212 24 bool left();
etorres31 1:036b11214212 25 bool right();
etorres31 1:036b11214212 26 bool fire();
etorres31 1:036b11214212 27 //automatic read on RHS
etorres31 1:036b11214212 28 operator int ();
etorres31 1:036b11214212 29 //index to any switch array style
etorres31 1:036b11214212 30 bool operator[](int index) {
etorres31 1:036b11214212 31 return _pins[index];
etorres31 1:036b11214212 32 };
etorres31 1:036b11214212 33 private:
etorres31 1:036b11214212 34 BusIn _pins;
etorres31 1:036b11214212 35
etorres31 1:036b11214212 36 };
etorres31 1:036b11214212 37 Nav_Switch::Nav_Switch (PinName up,PinName down,PinName left,PinName right,PinName fire):
etorres31 1:036b11214212 38 _pins(up, down, left, right, fire)
4180_1 0:e693d5bf0a25 39 {
etorres31 1:036b11214212 40 _pins.mode(PullUp); //needed if pullups not on board or a bare nav switch is used - delete otherwise
etorres31 1:036b11214212 41 wait(0.001); //delays just a bit for pullups to pull inputs high
etorres31 1:036b11214212 42 }
etorres31 1:036b11214212 43 inline bool Nav_Switch::up()
etorres31 1:036b11214212 44 {
etorres31 1:036b11214212 45 return !(_pins[0]);
etorres31 1:036b11214212 46 }
etorres31 1:036b11214212 47 inline bool Nav_Switch::down()
etorres31 1:036b11214212 48 {
etorres31 1:036b11214212 49 return !(_pins[1]);
etorres31 1:036b11214212 50 }
etorres31 1:036b11214212 51 inline bool Nav_Switch::left()
etorres31 1:036b11214212 52 {
etorres31 1:036b11214212 53 return !(_pins[2]);
etorres31 1:036b11214212 54 }
etorres31 1:036b11214212 55 inline bool Nav_Switch::right()
etorres31 1:036b11214212 56 {
etorres31 1:036b11214212 57 return !(_pins[3]);
etorres31 1:036b11214212 58 }
etorres31 1:036b11214212 59 inline bool Nav_Switch::fire()
etorres31 1:036b11214212 60 {
etorres31 1:036b11214212 61 return !(_pins[4]);
etorres31 1:036b11214212 62 }
etorres31 1:036b11214212 63 inline int Nav_Switch::read()
etorres31 1:036b11214212 64 {
etorres31 1:036b11214212 65 return _pins.read();
etorres31 1:036b11214212 66 }
etorres31 1:036b11214212 67 inline Nav_Switch::operator int ()
etorres31 1:036b11214212 68 {
etorres31 1:036b11214212 69 return _pins.read();
etorres31 1:036b11214212 70 }
etorres31 1:036b11214212 71
etorres31 1:036b11214212 72 Nav_Switch myNav(p29,p26,p27,p25, p28); //pin order on Sparkfun breakout
4180_1 0:e693d5bf0a25 73
etorres31 1:036b11214212 74 float y=0;
etorres31 1:036b11214212 75 float z=0;
4180_1 0:e693d5bf0a25 76
4180_1 0:e693d5bf0a25 77 int main()
4180_1 0:e693d5bf0a25 78 {
etorres31 1:036b11214212 79 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
4180_1 0:e693d5bf0a25 80 IMU.begin();
4180_1 0:e693d5bf0a25 81 if (!IMU.begin()) {
4180_1 0:e693d5bf0a25 82 pc.printf("Failed to communicate with LSM9DS1.\n");
4180_1 0:e693d5bf0a25 83 }
4180_1 0:e693d5bf0a25 84 IMU.calibrate(1);
etorres31 1:036b11214212 85
etorres31 1:036b11214212 86 wait(3);
etorres31 1:036b11214212 87 rst1 = 0; //Set reset pin to 0
etorres31 1:036b11214212 88 myled = 1;
etorres31 1:036b11214212 89 //myled2= 0;
etorres31 1:036b11214212 90 wait_ms(1);
etorres31 1:036b11214212 91 rst1 = 1; //Set reset pin to 1
etorres31 1:036b11214212 92 wait_ms(1);
etorres31 1:036b11214212 93
4180_1 0:e693d5bf0a25 94 while(1) {
etorres31 1:036b11214212 95 int a;//getKeyNum();
etorres31 1:036b11214212 96 int b;
etorres31 1:036b11214212 97 while(!IMU.accelAvailable());
4180_1 0:e693d5bf0a25 98 IMU.readAccel();
etorres31 1:036b11214212 99
etorres31 1:036b11214212 100 //These conditional statements create a "dead-zone" for the wheel, to make it less
etorres31 1:036b11214212 101 //sensitive and more usable. Additionally, the IMU data will never exceed .9 to so
etorres31 1:036b11214212 102 //as to limit the maximum speed and steering for the robot.
etorres31 1:036b11214212 103 if(IMU.calcAccel(IMU.ay)<-.1) {
etorres31 1:036b11214212 104 b = 1;
etorres31 1:036b11214212 105 y=.9;
etorres31 1:036b11214212 106 a = 4;
etorres31 1:036b11214212 107 wait(.5);
etorres31 1:036b11214212 108 } else if (IMU.calcAccel(IMU.ay)>.1) {
etorres31 1:036b11214212 109 b = 1;
etorres31 1:036b11214212 110 y=-.9;
etorres31 1:036b11214212 111 a = 3;
etorres31 1:036b11214212 112 wait(.5);
etorres31 1:036b11214212 113 //} else if(IMU.calcAccel(IMU.ay)<=.1||IMU.calcAccel(IMU.ay)>=-.1) {
etorres31 1:036b11214212 114 // y=IMU.calcAccel(IMU.ay);
etorres31 1:036b11214212 115
etorres31 1:036b11214212 116 } else if(IMU.calcAccel(IMU.ax)>.1) {
etorres31 1:036b11214212 117 b = 1;
etorres31 1:036b11214212 118 z=.9;
etorres31 1:036b11214212 119 a = 2;
etorres31 1:036b11214212 120 wait(.5);
etorres31 1:036b11214212 121 } else if((IMU.calcAccel(IMU.ax)<-.1)) {
etorres31 1:036b11214212 122 b = 1;
etorres31 1:036b11214212 123 z=IMU.calcAccel(IMU.az);;
etorres31 1:036b11214212 124 a = 1;
etorres31 1:036b11214212 125 wait(.5);
etorres31 1:036b11214212 126 } else {
etorres31 1:036b11214212 127 a = 0;
etorres31 1:036b11214212 128 b = 0;
etorres31 1:036b11214212 129 }
etorres31 1:036b11214212 130
etorres31 1:036b11214212 131 if (b == 0) {
etorres31 1:036b11214212 132 //with pullups a button hit is a "0" - "~" inverts data to leds
etorres31 1:036b11214212 133 mbedleds = ~(myNav & 0x0F); //update leds with nav switch direction inputs
etorres31 1:036b11214212 134 if(myNav.fire()) mbedleds = 0x0F; //special all leds on case for fire (center button)
etorres31 1:036b11214212 135 //or use - if(myNav[4]==0) mbedleds = 0x0F; //can index a switch bit like this
etorres31 1:036b11214212 136 //wait(0.02);
etorres31 1:036b11214212 137
etorres31 1:036b11214212 138 if (myNav[0] == 0) {
etorres31 1:036b11214212 139 a = 1;
etorres31 1:036b11214212 140 } else if (myNav[1] == 0) {
etorres31 1:036b11214212 141 a = 2;
etorres31 1:036b11214212 142 } else if (myNav[2] == 0) {
etorres31 1:036b11214212 143 a = 3;
etorres31 1:036b11214212 144 } else if (myNav[3] == 0) {
etorres31 1:036b11214212 145 a = 4;
etorres31 1:036b11214212 146 } else if (myNav[4] == 0) {
etorres31 1:036b11214212 147 a = 5;
etorres31 1:036b11214212 148 } else {
etorres31 1:036b11214212 149 a = 0;
etorres31 1:036b11214212 150 }
etorres31 1:036b11214212 151 }
etorres31 1:036b11214212 152 if(a!=-1){
etorres31 1:036b11214212 153 //myled2 = 1;
etorres31 1:036b11214212 154 xbee1.putc(a); //XBee write
etorres31 1:036b11214212 155 //myled2 = 0;
etorres31 1:036b11214212 156 //myled = 1;
etorres31 1:036b11214212 157 wait(.2);
etorres31 1:036b11214212 158 }
4180_1 0:e693d5bf0a25 159 }
etorres31 1:036b11214212 160 }