controller
Dependencies: FXAS21002 FXOS8700 Hexi_KW40Z Hexi_OLED_SSD1351
Fork of Hexi_BLE_Example by
main.cpp@4:f412749d800d, 2017-05-01 (annotated)
- Committer:
- mikebenq
- Date:
- Mon May 01 11:18:03 2017 +0000
- Revision:
- 4:f412749d800d
- Parent:
- 3:c2ab3a0de448
ece4180
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
khuang | 0:c80666325948 | 1 | #include "mbed.h" |
khuang | 0:c80666325948 | 2 | #include "Hexi_KW40Z.h" |
khuang | 0:c80666325948 | 3 | #include "Hexi_OLED_SSD1351.h" |
khuang | 0:c80666325948 | 4 | #include "OLED_types.h" |
khuang | 0:c80666325948 | 5 | #include "OpenSans_Font.h" |
khuang | 0:c80666325948 | 6 | #include "string.h" |
mikebenq | 4:f412749d800d | 7 | #include "FXAS21002.h" |
mikebenq | 4:f412749d800d | 8 | #include "FXOS8700.h" |
khuang | 0:c80666325948 | 9 | |
khuang | 0:c80666325948 | 10 | #define LED_ON 0 |
khuang | 0:c80666325948 | 11 | #define LED_OFF 1 |
mikebenq | 4:f412749d800d | 12 | #define DECLINATION -4.94 |
mikebenq | 4:f412749d800d | 13 | #define PI 3.14159 |
cotigac | 3:c2ab3a0de448 | 14 | |
mikebenq | 4:f412749d800d | 15 | |
khuang | 0:c80666325948 | 16 | void StartHaptic(void); |
khuang | 0:c80666325948 | 17 | void StopHaptic(void const *n); |
khuang | 1:a0d9eeedb771 | 18 | void txTask(void); |
khuang | 0:c80666325948 | 19 | |
khuang | 0:c80666325948 | 20 | DigitalOut redLed(LED1,1); |
khuang | 0:c80666325948 | 21 | DigitalOut greenLed(LED2,1); |
khuang | 0:c80666325948 | 22 | DigitalOut blueLed(LED3,1); |
khuang | 0:c80666325948 | 23 | DigitalOut haptic(PTB9); |
khuang | 0:c80666325948 | 24 | |
mikebenq | 4:f412749d800d | 25 | |
mikebenq | 4:f412749d800d | 26 | Serial pc(USBTX, USBRX); |
khuang | 0:c80666325948 | 27 | /* Define timer for haptic feedback */ |
khuang | 0:c80666325948 | 28 | RtosTimer hapticTimer(StopHaptic, osTimerOnce); |
khuang | 0:c80666325948 | 29 | |
khuang | 0:c80666325948 | 30 | /* Instantiate the Hexi KW40Z Driver (UART TX, UART RX) */ |
khuang | 0:c80666325948 | 31 | KW40Z kw40z_device(PTE24, PTE25); |
khuang | 0:c80666325948 | 32 | |
khuang | 0:c80666325948 | 33 | /* Instantiate the SSD1351 OLED Driver */ |
khuang | 0:c80666325948 | 34 | SSD1351 oled(PTB22,PTB21,PTC13,PTB20,PTE6, PTD15); /* (MOSI,SCLK,POWER,CS,RST,DC) */ |
khuang | 0:c80666325948 | 35 | |
mikebenq | 4:f412749d800d | 36 | /* Instantiate the gyro */ |
mikebenq | 4:f412749d800d | 37 | FXAS21002 gyro(PTC11,PTC10); |
mikebenq | 4:f412749d800d | 38 | FXOS8700 accel(PTC11, PTC10); |
mikebenq | 4:f412749d800d | 39 | FXOS8700 mag(PTC11, PTC10); |
mikebenq | 4:f412749d800d | 40 | // Storage for the data from the sensor |
mikebenq | 4:f412749d800d | 41 | float gyro_data[3]; float gyro_rms=0.0; |
mikebenq | 4:f412749d800d | 42 | float accel_data[3]; float accel_rms=0.0; |
mikebenq | 4:f412749d800d | 43 | float mag_data[3]; float mag_rms=0.0; |
mikebenq | 4:f412749d800d | 44 | float roll,pitch,heading; |
mikebenq | 4:f412749d800d | 45 | float new_roll,new_pitch,new_heading; |
mikebenq | 4:f412749d800d | 46 | int del_roll,del_pitch; |
mikebenq | 4:f412749d800d | 47 | volatile bool flag=false; |
mikebenq | 4:f412749d800d | 48 | Timeout timeout; |
khuang | 1:a0d9eeedb771 | 49 | /*Create a Thread to handle sending BLE Sensor Data */ |
khuang | 1:a0d9eeedb771 | 50 | Thread txThread; |
khuang | 0:c80666325948 | 51 | |
khuang | 1:a0d9eeedb771 | 52 | /* Text Buffer */ |
khuang | 1:a0d9eeedb771 | 53 | char text[20]; |
khuang | 1:a0d9eeedb771 | 54 | |
cotigac | 3:c2ab3a0de448 | 55 | uint8_t battery = 100; |
cotigac | 3:c2ab3a0de448 | 56 | uint8_t light = 0; |
cotigac | 3:c2ab3a0de448 | 57 | uint16_t humidity = 4500; |
cotigac | 3:c2ab3a0de448 | 58 | uint16_t temperature = 2000; |
cotigac | 3:c2ab3a0de448 | 59 | uint16_t pressure = 9000; |
cotigac | 3:c2ab3a0de448 | 60 | uint16_t x = 0; |
cotigac | 3:c2ab3a0de448 | 61 | uint16_t y = 5000; |
cotigac | 3:c2ab3a0de448 | 62 | uint16_t z = 10000; |
cotigac | 3:c2ab3a0de448 | 63 | |
mikebenq | 4:f412749d800d | 64 | /*Get the new value of roll and pitch*/ |
mikebenq | 4:f412749d800d | 65 | void get_new_value(){ |
mikebenq | 4:f412749d800d | 66 | gyro.acquire_gyro_data_dps(gyro_data); |
mikebenq | 4:f412749d800d | 67 | accel.acquire_accel_data_g(accel_data); |
mikebenq | 4:f412749d800d | 68 | new_roll = atan2(accel_data[2],accel_data[1]); |
mikebenq | 4:f412749d800d | 69 | new_pitch = atan2(-accel_data[0],sqrt(accel_data[1]*accel_data[1] + accel_data[2]*accel_data[2])); |
mikebenq | 4:f412749d800d | 70 | new_pitch *= 180.0 / PI; |
mikebenq | 4:f412749d800d | 71 | new_roll *= 180.0 / PI; |
mikebenq | 4:f412749d800d | 72 | new_roll+=180; |
mikebenq | 4:f412749d800d | 73 | } |
mikebenq | 4:f412749d800d | 74 | /*Click the button to start it*/ |
mikebenq | 4:f412749d800d | 75 | void Start_Detect(){ |
mikebenq | 4:f412749d800d | 76 | if(flag==true) |
mikebenq | 4:f412749d800d | 77 | return; |
mikebenq | 4:f412749d800d | 78 | flag=true; |
mikebenq | 4:f412749d800d | 79 | } |
khuang | 1:a0d9eeedb771 | 80 | /****************************Call Back Functions*******************************/ |
khuang | 0:c80666325948 | 81 | void ButtonRight(void) |
khuang | 0:c80666325948 | 82 | { |
khuang | 0:c80666325948 | 83 | StartHaptic(); |
khuang | 0:c80666325948 | 84 | kw40z_device.ToggleAdvertisementMode(); |
khuang | 0:c80666325948 | 85 | } |
khuang | 0:c80666325948 | 86 | |
khuang | 0:c80666325948 | 87 | void ButtonLeft(void) |
khuang | 0:c80666325948 | 88 | { |
khuang | 0:c80666325948 | 89 | StartHaptic(); |
khuang | 0:c80666325948 | 90 | kw40z_device.ToggleAdvertisementMode(); |
khuang | 0:c80666325948 | 91 | } |
khuang | 0:c80666325948 | 92 | |
khuang | 0:c80666325948 | 93 | void PassKey(void) |
khuang | 0:c80666325948 | 94 | { |
khuang | 0:c80666325948 | 95 | StartHaptic(); |
khuang | 0:c80666325948 | 96 | strcpy((char *) text,"PAIR CODE"); |
khuang | 1:a0d9eeedb771 | 97 | oled.TextBox((uint8_t *)text,0,25,95,18); |
khuang | 0:c80666325948 | 98 | |
khuang | 0:c80666325948 | 99 | /* Display Bond Pass Key in a 95px by 18px textbox at x=0,y=40 */ |
khuang | 0:c80666325948 | 100 | sprintf(text,"%d", kw40z_device.GetPassKey()); |
khuang | 0:c80666325948 | 101 | oled.TextBox((uint8_t *)text,0,40,95,18); |
khuang | 0:c80666325948 | 102 | } |
khuang | 1:a0d9eeedb771 | 103 | |
khuang | 1:a0d9eeedb771 | 104 | /***********************End of Call Back Functions*****************************/ |
khuang | 1:a0d9eeedb771 | 105 | |
khuang | 1:a0d9eeedb771 | 106 | /********************************Main******************************************/ |
khuang | 1:a0d9eeedb771 | 107 | |
khuang | 0:c80666325948 | 108 | int main() |
khuang | 0:c80666325948 | 109 | { |
cotigac | 3:c2ab3a0de448 | 110 | /* Register callbacks to application functions */ |
cotigac | 3:c2ab3a0de448 | 111 | kw40z_device.attach_buttonLeft(&ButtonLeft); |
cotigac | 3:c2ab3a0de448 | 112 | kw40z_device.attach_buttonRight(&ButtonRight); |
cotigac | 3:c2ab3a0de448 | 113 | kw40z_device.attach_passkey(&PassKey); |
mikebenq | 4:f412749d800d | 114 | kw40z_device.attach_buttonUp(&Start_Detect);//Click the button to start it |
mikebenq | 4:f412749d800d | 115 | gyro.gyro_config(); |
mikebenq | 4:f412749d800d | 116 | accel.accel_config(); |
mikebenq | 4:f412749d800d | 117 | mag.mag_config(); |
khuang | 0:c80666325948 | 118 | /* Turn on the backlight of the OLED Display */ |
khuang | 0:c80666325948 | 119 | oled.DimScreenON(); |
khuang | 0:c80666325948 | 120 | |
khuang | 0:c80666325948 | 121 | /* Fills the screen with solid black */ |
khuang | 0:c80666325948 | 122 | oled.FillScreen(COLOR_BLACK); |
cotigac | 3:c2ab3a0de448 | 123 | |
cotigac | 3:c2ab3a0de448 | 124 | /* Get OLED Class Default Text Properties */ |
cotigac | 3:c2ab3a0de448 | 125 | oled_text_properties_t textProperties = {0}; |
cotigac | 3:c2ab3a0de448 | 126 | oled.GetTextProperties(&textProperties); |
khuang | 0:c80666325948 | 127 | |
khuang | 0:c80666325948 | 128 | /* Change font color to Blue */ |
khuang | 0:c80666325948 | 129 | textProperties.fontColor = COLOR_BLUE; |
khuang | 0:c80666325948 | 130 | oled.SetTextProperties(&textProperties); |
khuang | 0:c80666325948 | 131 | |
khuang | 0:c80666325948 | 132 | /* Display Bluetooth Label at x=17,y=65 */ |
khuang | 0:c80666325948 | 133 | strcpy((char *) text,"BLUETOOTH"); |
khuang | 0:c80666325948 | 134 | oled.Label((uint8_t *)text,17,65); |
khuang | 0:c80666325948 | 135 | |
khuang | 0:c80666325948 | 136 | /* Change font color to white */ |
khuang | 0:c80666325948 | 137 | textProperties.fontColor = COLOR_WHITE; |
khuang | 0:c80666325948 | 138 | textProperties.alignParam = OLED_TEXT_ALIGN_CENTER; |
khuang | 0:c80666325948 | 139 | oled.SetTextProperties(&textProperties); |
khuang | 0:c80666325948 | 140 | |
khuang | 1:a0d9eeedb771 | 141 | /* Display Label at x=22,y=80 */ |
khuang | 0:c80666325948 | 142 | strcpy((char *) text,"Tap Below"); |
khuang | 0:c80666325948 | 143 | oled.Label((uint8_t *)text,22,80); |
khuang | 0:c80666325948 | 144 | |
khuang | 1:a0d9eeedb771 | 145 | uint8_t prevLinkState = 0; |
cotigac | 3:c2ab3a0de448 | 146 | uint8_t currLinkState = 0; |
cotigac | 3:c2ab3a0de448 | 147 | |
khuang | 1:a0d9eeedb771 | 148 | txThread.start(txTask); /*Start transmitting Sensor Tag Data */ |
khuang | 0:c80666325948 | 149 | |
khuang | 1:a0d9eeedb771 | 150 | while (true) |
khuang | 1:a0d9eeedb771 | 151 | { |
khuang | 1:a0d9eeedb771 | 152 | blueLed = !kw40z_device.GetAdvertisementMode(); /*Indicate BLE Advertisment Mode*/ |
khuang | 1:a0d9eeedb771 | 153 | Thread::wait(50); |
khuang | 1:a0d9eeedb771 | 154 | } |
khuang | 1:a0d9eeedb771 | 155 | } |
khuang | 1:a0d9eeedb771 | 156 | |
khuang | 1:a0d9eeedb771 | 157 | /******************************End of Main*************************************/ |
khuang | 1:a0d9eeedb771 | 158 | |
khuang | 1:a0d9eeedb771 | 159 | |
khuang | 1:a0d9eeedb771 | 160 | /* txTask() transmits the sensor data */ |
khuang | 1:a0d9eeedb771 | 161 | void txTask(void){ |
khuang | 1:a0d9eeedb771 | 162 | |
khuang | 1:a0d9eeedb771 | 163 | while (true) |
khuang | 1:a0d9eeedb771 | 164 | { |
khuang | 0:c80666325948 | 165 | |
mikebenq | 4:f412749d800d | 166 | if(flag){ |
mikebenq | 4:f412749d800d | 167 | kw40z_device.SendSetApplicationMode(GUI_CURRENT_APP_SENSOR_TAG); |
mikebenq | 4:f412749d800d | 168 | gyro.acquire_gyro_data_dps(gyro_data); |
mikebenq | 4:f412749d800d | 169 | accel.acquire_accel_data_g(accel_data); |
mikebenq | 4:f412749d800d | 170 | roll = atan2(accel_data[2],accel_data[1]); |
mikebenq | 4:f412749d800d | 171 | pitch = atan2(-accel_data[0],sqrt(accel_data[1]*accel_data[1] + accel_data[2]*accel_data[2])); |
mikebenq | 4:f412749d800d | 172 | pitch *= 180.0 / PI; |
mikebenq | 4:f412749d800d | 173 | roll *= 180.0 / PI; |
mikebenq | 4:f412749d800d | 174 | roll+=180; |
mikebenq | 4:f412749d800d | 175 | |
mikebenq | 4:f412749d800d | 176 | Thread::wait(1000); |
mikebenq | 4:f412749d800d | 177 | get_new_value(); |
mikebenq | 4:f412749d800d | 178 | float cmd; |
mikebenq | 4:f412749d800d | 179 | |
mikebenq | 4:f412749d800d | 180 | if(abs(new_roll-roll)>abs(new_pitch-pitch)){ |
mikebenq | 4:f412749d800d | 181 | if(new_roll-roll>30) |
mikebenq | 4:f412749d800d | 182 | cmd=100; |
mikebenq | 4:f412749d800d | 183 | else if(new_roll-roll<-30) |
mikebenq | 4:f412749d800d | 184 | cmd=200; |
mikebenq | 4:f412749d800d | 185 | else |
mikebenq | 4:f412749d800d | 186 | cmd=0; |
mikebenq | 4:f412749d800d | 187 | }else if(abs(new_roll-roll)<abs(new_pitch-pitch)){ |
mikebenq | 4:f412749d800d | 188 | if(new_pitch-pitch>10) |
mikebenq | 4:f412749d800d | 189 | cmd=300; |
mikebenq | 4:f412749d800d | 190 | else if(new_pitch-pitch<-10) |
mikebenq | 4:f412749d800d | 191 | cmd=400; |
mikebenq | 4:f412749d800d | 192 | else |
mikebenq | 4:f412749d800d | 193 | cmd=0; |
mikebenq | 4:f412749d800d | 194 | }else |
mikebenq | 4:f412749d800d | 195 | cmd=0; |
mikebenq | 4:f412749d800d | 196 | pc.printf("cmd=%f",cmd); |
mikebenq | 4:f412749d800d | 197 | kw40z_device.SendTemperature(cmd); |
mikebenq | 4:f412749d800d | 198 | Thread::wait(6000);//Make some delay to wait the data upload to the server |
mikebenq | 4:f412749d800d | 199 | kw40z_device.SendTemperature(0);//send 0 to the server |
mikebenq | 4:f412749d800d | 200 | flag=false; |
mikebenq | 4:f412749d800d | 201 | } |
mikebenq | 4:f412749d800d | 202 | |
mikebenq | 4:f412749d800d | 203 | Thread::wait(500); |
khuang | 0:c80666325948 | 204 | } |
khuang | 0:c80666325948 | 205 | } |
khuang | 0:c80666325948 | 206 | |
khuang | 1:a0d9eeedb771 | 207 | void StartHaptic(void) { |
khuang | 0:c80666325948 | 208 | hapticTimer.start(50); |
khuang | 0:c80666325948 | 209 | haptic = 1; |
khuang | 0:c80666325948 | 210 | } |
khuang | 0:c80666325948 | 211 | |
khuang | 0:c80666325948 | 212 | void StopHaptic(void const *n) { |
khuang | 0:c80666325948 | 213 | haptic = 0; |
khuang | 0:c80666325948 | 214 | hapticTimer.stop(); |
khuang | 0:c80666325948 | 215 | } |
khuang | 1:a0d9eeedb771 | 216 |