Dave Heitzman / mDot_Bob

drives the mdot used for bob demo

Dependencies:   h3lis331dl libmDot mbed-rtos mbed-src

main.cpp@0:910b4d5df41e, 2015-10-23 (annotated)

Committer:
daveheitzman
Date:
Fri Oct 23 18:27:42 2015 +0000
Revision:
0:910b4d5df41e
publishing;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
daveheitzman 0:910b4d5df41e 1 // ----------------
daveheitzman 0:910b4d5df41e 2 // CTIA Helmet Demo
daveheitzman 0:910b4d5df41e 3 // ----------------
daveheitzman 0:910b4d5df41e 4
daveheitzman 0:910b4d5df41e 5 #include "mbed.h"
daveheitzman 0:910b4d5df41e 6 #include "mDot.h"
daveheitzman 0:910b4d5df41e 7 #include "rtos.h"
daveheitzman 0:910b4d5df41e 8 #include <H3LIS331DL.h>
daveheitzman 0:910b4d5df41e 9 #include <string>
daveheitzman 0:910b4d5df41e 10 #include <vector>
daveheitzman 0:910b4d5df41e 11
daveheitzman 0:910b4d5df41e 12 #define G_THRESHOLD 5
daveheitzman 0:910b4d5df41e 13
daveheitzman 0:910b4d5df41e 14 //please get these value by running H3LIS331DL_AdjVal
daveheitzman 0:910b4d5df41e 15 #define VAL_X_AXIS 71 //68
daveheitzman 0:910b4d5df41e 16 #define VAL_Y_AXIS 97 //38
daveheitzman 0:910b4d5df41e 17 #define VAL_Z_AXIS 722 //664
daveheitzman 0:910b4d5df41e 18 #define GAIN 0.003
daveheitzman 0:910b4d5df41e 19
daveheitzman 0:910b4d5df41e 20 // these options must match the settings on your Conduit
daveheitzman 0:910b4d5df41e 21 static std::string config_network_name = "Escalation";
daveheitzman 0:910b4d5df41e 22 static std::string config_network_pass = "Escalation";
daveheitzman 0:910b4d5df41e 23 static uint8_t config_frequency_sub_band = 3;
daveheitzman 0:910b4d5df41e 24
daveheitzman 0:910b4d5df41e 25 //Globals
daveheitzman 0:910b4d5df41e 26 mDot* dot;
daveheitzman 0:910b4d5df41e 27 H3LIS331DL* h3lis=new H3LIS331DL(I2C_SDA, I2C_SCL);
daveheitzman 0:910b4d5df41e 28 DigitalOut led(LED1);
daveheitzman 0:910b4d5df41e 29 Ticker ledTick;
daveheitzman 0:910b4d5df41e 30 Ticker heart;
daveheitzman 0:910b4d5df41e 31 Timer timer;
daveheitzman 0:910b4d5df41e 32 volatile bool timeToSendHeartbeat = true;
daveheitzman 0:910b4d5df41e 33
daveheitzman 0:910b4d5df41e 34 //Function prototypes
daveheitzman 0:910b4d5df41e 35 void log_error(mDot* dot, const char* msg, int32_t retval);
daveheitzman 0:910b4d5df41e 36 bool setFrequencySubBand(uint8_t subBand);
daveheitzman 0:910b4d5df41e 37 bool setNetworkName(const std::string name);
daveheitzman 0:910b4d5df41e 38 bool setNetworkPassphrase(const std::string passphrase);
daveheitzman 0:910b4d5df41e 39 bool setAck(uint8_t retries);
daveheitzman 0:910b4d5df41e 40 bool joinNetwork();
daveheitzman 0:910b4d5df41e 41 bool sendData(const std::string text);
daveheitzman 0:910b4d5df41e 42 H3LIS331DL* rebuild(H3LIS331DL* h3lis );
daveheitzman 0:910b4d5df41e 43
daveheitzman 0:910b4d5df41e 44
daveheitzman 0:910b4d5df41e 45 void ledTock() {
daveheitzman 0:910b4d5df41e 46 led = !led;
daveheitzman 0:910b4d5df41e 47 }
daveheitzman 0:910b4d5df41e 48
daveheitzman 0:910b4d5df41e 49 void beat() {
daveheitzman 0:910b4d5df41e 50 timeToSendHeartbeat = true;
daveheitzman 0:910b4d5df41e 51 }
daveheitzman 0:910b4d5df41e 52 int begin;
daveheitzman 0:910b4d5df41e 53
daveheitzman 0:910b4d5df41e 54
daveheitzman 0:910b4d5df41e 55 int main() {
daveheitzman 0:910b4d5df41e 56 int32_t ret;
daveheitzman 0:910b4d5df41e 57 unsigned char id;
daveheitzman 0:910b4d5df41e 58 std::vector<uint8_t> data;
daveheitzman 0:910b4d5df41e 59 char data_str[40];
daveheitzman 0:910b4d5df41e 60 int16_t x=0, y=0, z=0, x_max=0, y_max=0, z_max=0;
daveheitzman 0:910b4d5df41e 61 uint16_t vector_sum=0, vector_sum_max=0;
daveheitzman 0:910b4d5df41e 62
daveheitzman 0:910b4d5df41e 63 ledTick.attach(&ledTock, 0.1);
daveheitzman 0:910b4d5df41e 64
daveheitzman 0:910b4d5df41e 65 // get the mDot handle
daveheitzman 0:910b4d5df41e 66 dot = mDot::getInstance();
daveheitzman 0:910b4d5df41e 67
daveheitzman 0:910b4d5df41e 68
daveheitzman 0:910b4d5df41e 69 // reset to default config so we know what state we're in
daveheitzman 0:910b4d5df41e 70 dot->resetConfig();
daveheitzman 0:910b4d5df41e 71 dot->getDeviceId();
daveheitzman 0:910b4d5df41e 72
daveheitzman 0:910b4d5df41e 73 printf("\r\n\r\n");
daveheitzman 0:910b4d5df41e 74 printf("=======================\r\n");
daveheitzman 0:910b4d5df41e 75 printf(" CTIA Helment Demo\r\n");
daveheitzman 0:910b4d5df41e 76 printf("=======================\r\n");
daveheitzman 0:910b4d5df41e 77
daveheitzman 0:910b4d5df41e 78 printf("Library version: %s\r\n\r\n", dot->getId().c_str());
daveheitzman 0:910b4d5df41e 79
daveheitzman 0:910b4d5df41e 80 // set up the mDot with our network information
daveheitzman 0:910b4d5df41e 81 setFrequencySubBand(config_frequency_sub_band);
daveheitzman 0:910b4d5df41e 82 setNetworkName(config_network_name);
daveheitzman 0:910b4d5df41e 83 setNetworkPassphrase(config_network_pass);
daveheitzman 0:910b4d5df41e 84 setAck(0); //disable acks
daveheitzman 0:910b4d5df41e 85
daveheitzman 0:910b4d5df41e 86 printf("Initializing accelerometer...\r\n");
daveheitzman 0:910b4d5df41e 87 // H3LIS331DL_FULLSCALE_4 = +- 200g, H3LIS331DL_FULLSCALE_2 = +- 100g
daveheitzman 0:910b4d5df41e 88 h3lis->init(H3LIS331DL_ODR_1000Hz, H3LIS331DL_NORMAL, H3LIS331DL_FULLSCALE_2);
daveheitzman 0:910b4d5df41e 89 // h3lis.init(H3LIS331DL_ODR_100Hz, H3LIS331DL_NORMAL, H3LIS331DL_FULLSCALE_2);
daveheitzman 0:910b4d5df41e 90
daveheitzman 0:910b4d5df41e 91
daveheitzman 0:910b4d5df41e 92 if (!h3lis->getWHO_AM_I(&id)) {
daveheitzman 0:910b4d5df41e 93 printf("Failed to initialize accelerometer!\r\n");
daveheitzman 0:910b4d5df41e 94 return 0;
daveheitzman 0:910b4d5df41e 95 }
daveheitzman 0:910b4d5df41e 96
daveheitzman 0:910b4d5df41e 97 // Join network
daveheitzman 0:910b4d5df41e 98 while (!joinNetwork()) { wait(5); };
daveheitzman 0:910b4d5df41e 99 ledTick.detach();
daveheitzman 0:910b4d5df41e 100 led = 1;
daveheitzman 0:910b4d5df41e 101
daveheitzman 0:910b4d5df41e 102 heart.attach(&beat, 10);
daveheitzman 0:910b4d5df41e 103 timer.start();
daveheitzman 0:910b4d5df41e 104 while (true) {
daveheitzman 0:910b4d5df41e 105 begin = timer.read_us();
daveheitzman 0:910b4d5df41e 106
daveheitzman 0:910b4d5df41e 107 h3lis->readXYZ(&x,&y,&z);
daveheitzman 0:910b4d5df41e 108 x = (x - VAL_X_AXIS) * GAIN;
daveheitzman 0:910b4d5df41e 109 y = (y - VAL_Y_AXIS) * GAIN;
daveheitzman 0:910b4d5df41e 110 z = (z - VAL_Z_AXIS) * GAIN;
daveheitzman 0:910b4d5df41e 111
daveheitzman 0:910b4d5df41e 112 // printf takes about 50 ms !
daveheitzman 0:910b4d5df41e 113 // printf("Reading: X:% d Y:% d Z:% d, timer: % d\r\n", x, y, z, begin);
daveheitzman 0:910b4d5df41e 114
daveheitzman 0:910b4d5df41e 115 vector_sum = sqrt((double)(x*x + y*y + z*z));
daveheitzman 0:910b4d5df41e 116
daveheitzman 0:910b4d5df41e 117 // If impact occuring
daveheitzman 0:910b4d5df41e 118 if (vector_sum > G_THRESHOLD) {
daveheitzman 0:910b4d5df41e 119 //Update peak forces if needed
daveheitzman 0:910b4d5df41e 120 if (vector_sum > vector_sum_max) {
daveheitzman 0:910b4d5df41e 121 vector_sum_max = vector_sum;
daveheitzman 0:910b4d5df41e 122 x_max = x;
daveheitzman 0:910b4d5df41e 123 y_max = y;
daveheitzman 0:910b4d5df41e 124 z_max = z;
daveheitzman 0:910b4d5df41e 125 }
daveheitzman 0:910b4d5df41e 126 printf("Reading: X:% d Y:% d Z:% d\r\n", x, y, z);
daveheitzman 0:910b4d5df41e 127 }
daveheitzman 0:910b4d5df41e 128 // If impact detected
daveheitzman 0:910b4d5df41e 129 else if (vector_sum_max > 0) {
daveheitzman 0:910b4d5df41e 130 printf("IMPACT: X:% d Y:% d Z:% d\r\n", x_max, y_max, z_max);
daveheitzman 0:910b4d5df41e 131 sprintf(data_str, "%d,%d,%d", x_max, y_max, z_max);
daveheitzman 0:910b4d5df41e 132
daveheitzman 0:910b4d5df41e 133 // send the data
daveheitzman 0:910b4d5df41e 134 sendData(data_str);
daveheitzman 0:910b4d5df41e 135
daveheitzman 0:910b4d5df41e 136 // Reset maximums
daveheitzman 0:910b4d5df41e 137 vector_sum_max = 0;
daveheitzman 0:910b4d5df41e 138 x_max = 0;
daveheitzman 0:910b4d5df41e 139 y_max = 0;
daveheitzman 0:910b4d5df41e 140 z_max = 0;
daveheitzman 0:910b4d5df41e 141 timeToSendHeartbeat = false;
daveheitzman 0:910b4d5df41e 142 }
daveheitzman 0:910b4d5df41e 143 //Idle
daveheitzman 0:910b4d5df41e 144 else {
daveheitzman 0:910b4d5df41e 145 if (timeToSendHeartbeat) {
daveheitzman 0:910b4d5df41e 146 sendData("heartbeat");
daveheitzman 0:910b4d5df41e 147 timeToSendHeartbeat = false;
daveheitzman 0:910b4d5df41e 148 }
daveheitzman 0:910b4d5df41e 149 }
daveheitzman 0:910b4d5df41e 150 wait_ms(10); //Accelerometer reading updates every 1ms
daveheitzman 0:910b4d5df41e 151 }
daveheitzman 0:910b4d5df41e 152
daveheitzman 0:910b4d5df41e 153 return 0;
daveheitzman 0:910b4d5df41e 154 }
daveheitzman 0:910b4d5df41e 155
daveheitzman 0:910b4d5df41e 156 void log_error(mDot* dot, const char* msg, int32_t retval) {
daveheitzman 0:910b4d5df41e 157 printf("%s - %ld:%s, %s\r\n", msg, retval, mDot::getReturnCodeString(retval).c_str(), dot->getLastError().c_str());
daveheitzman 0:910b4d5df41e 158 }
daveheitzman 0:910b4d5df41e 159
daveheitzman 0:910b4d5df41e 160 bool setFrequencySubBand(uint8_t subBand)
daveheitzman 0:910b4d5df41e 161 {
daveheitzman 0:910b4d5df41e 162 int32_t ret;
daveheitzman 0:910b4d5df41e 163 printf("Setting frequency sub band to '%d'...\r\n", subBand);
daveheitzman 0:910b4d5df41e 164 if ((ret = dot->setFrequencySubBand(subBand)) != mDot::MDOT_OK) {
daveheitzman 0:910b4d5df41e 165 log_error(dot, "Failed to set frequency sub band", ret);
daveheitzman 0:910b4d5df41e 166 return false;
daveheitzman 0:910b4d5df41e 167 }
daveheitzman 0:910b4d5df41e 168 return true;
daveheitzman 0:910b4d5df41e 169 }
daveheitzman 0:910b4d5df41e 170
daveheitzman 0:910b4d5df41e 171 bool setNetworkName(const std::string name)
daveheitzman 0:910b4d5df41e 172 {
daveheitzman 0:910b4d5df41e 173 int32_t ret;
daveheitzman 0:910b4d5df41e 174 printf("Setting network name to '%s'...\r\n", name.c_str());
daveheitzman 0:910b4d5df41e 175 if ((ret = dot->setNetworkName(name)) != mDot::MDOT_OK) {
daveheitzman 0:910b4d5df41e 176 log_error(dot, "Failed to set network name", ret);
daveheitzman 0:910b4d5df41e 177 return false;
daveheitzman 0:910b4d5df41e 178 }
daveheitzman 0:910b4d5df41e 179 return true;
daveheitzman 0:910b4d5df41e 180 }
daveheitzman 0:910b4d5df41e 181
daveheitzman 0:910b4d5df41e 182 bool setNetworkPassphrase(const std::string passphrase)
daveheitzman 0:910b4d5df41e 183 {
daveheitzman 0:910b4d5df41e 184 int32_t ret;
daveheitzman 0:910b4d5df41e 185 printf("Setting passphrase...\r\n");
daveheitzman 0:910b4d5df41e 186 if ((ret = dot->setNetworkPassphrase(passphrase)) != mDot::MDOT_OK) {
daveheitzman 0:910b4d5df41e 187 log_error(dot, "Failed to set network password", ret);
daveheitzman 0:910b4d5df41e 188 return false;
daveheitzman 0:910b4d5df41e 189 }
daveheitzman 0:910b4d5df41e 190 return true;
daveheitzman 0:910b4d5df41e 191 }
daveheitzman 0:910b4d5df41e 192
daveheitzman 0:910b4d5df41e 193 bool setAck(uint8_t retries)
daveheitzman 0:910b4d5df41e 194 {
daveheitzman 0:910b4d5df41e 195 int32_t ret;
daveheitzman 0:910b4d5df41e 196 printf("Setting ack...\r\n");
daveheitzman 0:910b4d5df41e 197 if ((ret = dot->setAck(retries)) != mDot::MDOT_OK)
daveheitzman 0:910b4d5df41e 198 {
daveheitzman 0:910b4d5df41e 199 log_error(dot, "Failed to set ack", ret);
daveheitzman 0:910b4d5df41e 200 return false;
daveheitzman 0:910b4d5df41e 201 }
daveheitzman 0:910b4d5df41e 202 return true;
daveheitzman 0:910b4d5df41e 203 }
daveheitzman 0:910b4d5df41e 204
daveheitzman 0:910b4d5df41e 205 bool joinNetwork()
daveheitzman 0:910b4d5df41e 206 {
daveheitzman 0:910b4d5df41e 207 int32_t ret;
daveheitzman 0:910b4d5df41e 208 printf("\r\nJoining network... ");
daveheitzman 0:910b4d5df41e 209 // reset to default config so we know what state we're in
daveheitzman 0:910b4d5df41e 210
daveheitzman 0:910b4d5df41e 211
daveheitzman 0:910b4d5df41e 212 if ((ret = dot->joinNetwork()) != mDot::MDOT_OK) {
daveheitzman 0:910b4d5df41e 213 log_error(dot, "Failed", ret);
daveheitzman 0:910b4d5df41e 214 dot->resetConfig();
daveheitzman 0:910b4d5df41e 215 dot->getDeviceId();
daveheitzman 0:910b4d5df41e 216 osDelay(200);
daveheitzman 0:910b4d5df41e 217 return false;
daveheitzman 0:910b4d5df41e 218 }
daveheitzman 0:910b4d5df41e 219 printf("Network Joined!\r\n");
daveheitzman 0:910b4d5df41e 220 return true;
daveheitzman 0:910b4d5df41e 221 }
daveheitzman 0:910b4d5df41e 222
daveheitzman 0:910b4d5df41e 223 bool sendData(const std::string text)
daveheitzman 0:910b4d5df41e 224 {
daveheitzman 0:910b4d5df41e 225 if (dot->getNextTxMs() != 0) {
daveheitzman 0:910b4d5df41e 226 printf("Sending in %lu ms...\r\n", dot->getNextTxMs());
daveheitzman 0:910b4d5df41e 227 return false;
daveheitzman 0:910b4d5df41e 228 }
daveheitzman 0:910b4d5df41e 229
daveheitzman 0:910b4d5df41e 230 int32_t ret;
daveheitzman 0:910b4d5df41e 231 printf("Sending: '%s' ", text.c_str());
daveheitzman 0:910b4d5df41e 232 std::vector<uint8_t> data(text.begin(), text.end());
daveheitzman 0:910b4d5df41e 233 if ((ret = dot->send(data, 1)) != mDot::MDOT_OK)
daveheitzman 0:910b4d5df41e 234 {
daveheitzman 0:910b4d5df41e 235 log_error(dot, "Failed to send data", ret);
daveheitzman 0:910b4d5df41e 236 return false;
daveheitzman 0:910b4d5df41e 237 }
daveheitzman 0:910b4d5df41e 238 printf("Data sent!\r\n");
daveheitzman 0:910b4d5df41e 239 return true;
daveheitzman 0:910b4d5df41e 240 }
daveheitzman 0:910b4d5df41e 241
daveheitzman 0:910b4d5df41e 242 H3LIS331DL* rebuild(H3LIS331DL* h3lis ){
daveheitzman 0:910b4d5df41e 243 H3LIS331DL* new_h3lis = new H3LIS331DL( I2C_SDA, I2C_SCL );
daveheitzman 0:910b4d5df41e 244 delete h3lis;
daveheitzman 0:910b4d5df41e 245 new_h3lis->init(H3LIS331DL_ODR_1000Hz, H3LIS331DL_NORMAL, H3LIS331DL_FULLSCALE_2);
daveheitzman 0:910b4d5df41e 246
daveheitzman 0:910b4d5df41e 247 return new_h3lis;
daveheitzman 0:910b4d5df41e 248 }
daveheitzman 0:910b4d5df41e 249
daveheitzman 0:910b4d5df41e 250