Christine Chen / Mbed OS cooking_mama

Cooking mama hexiwear sensor game for Spring M119 class

Dependencies:   FXOS8700CQ FXOS8700 FXAS21002 Hexi_KW40Z Hexi_OLED_SSD1351 FXOS8700Q

main.cpp@0:86410c1144d1, 2019-06-03 (annotated)

Committer:
christine222
Date:
Mon Jun 03 01:12:33 2019 +0000
Revision:
0:86410c1144d1
publishing cooking mama for group collaboration

Who changed what in which revision?

UserRevisionLine numberNew contents of line
christine222 0:86410c1144d1 1 #include "mbed.h"
christine222 0:86410c1144d1 2 #include "Hexi_KW40Z.h"
christine222 0:86410c1144d1 3 #include "Hexi_OLED_SSD1351.h"
christine222 0:86410c1144d1 4 #include "FXAS21002.h"
christine222 0:86410c1144d1 5 #include "FXOS8700.h"
christine222 0:86410c1144d1 6
christine222 0:86410c1144d1 7 Serial pc(USBTX, USBRX);
christine222 0:86410c1144d1 8 Ticker systick;
christine222 0:86410c1144d1 9
christine222 0:86410c1144d1 10 DigitalOut redLed(LED1,1);
christine222 0:86410c1144d1 11 DigitalOut greenLed(LED2,1);
christine222 0:86410c1144d1 12 DigitalOut blueLed(LED3,1);
christine222 0:86410c1144d1 13 DigitalOut haptic(PTB9);
christine222 0:86410c1144d1 14
christine222 0:86410c1144d1 15 //void StartHaptic(void);
christine222 0:86410c1144d1 16 //void StopHaptic(void const *n);
christine222 0:86410c1144d1 17
christine222 0:86410c1144d1 18 //RtosTimer hapticTimer(StopHaptic, osTimerOnce);
christine222 0:86410c1144d1 19 KW40Z kw40z_device(PTE24, PTE25);
christine222 0:86410c1144d1 20
christine222 0:86410c1144d1 21 // setup sensorss
christine222 0:86410c1144d1 22 FXAS21002 gyro(PTC11,PTC10);
christine222 0:86410c1144d1 23 FXOS8700 accel(PTC11, PTC10);
christine222 0:86410c1144d1 24 FXOS8700 mag(PTC11, PTC10);
christine222 0:86410c1144d1 25
christine222 0:86410c1144d1 26 const float SYSTICK_PERIOD = .01; // seconds
christine222 0:86410c1144d1 27
christine222 0:86410c1144d1 28 const int tick_duration = 5; // duration/period
christine222 0:86410c1144d1 29 volatile int tick_counter = 0;
christine222 0:86410c1144d1 30
christine222 0:86410c1144d1 31 // flags to indicate state
christine222 0:86410c1144d1 32 volatile bool buttonPressed = 0;
christine222 0:86410c1144d1 33 volatile bool doMotion = 0;
christine222 0:86410c1144d1 34 volatile bool done = 1;
christine222 0:86410c1144d1 35 volatile bool overflow = 0;
christine222 0:86410c1144d1 36 volatile bool startHaptic = 0;
christine222 0:86410c1144d1 37
christine222 0:86410c1144d1 38 // accelerometer data
christine222 0:86410c1144d1 39 volatile float x_accel = 0;
christine222 0:86410c1144d1 40 volatile float x_velocity = 0;
christine222 0:86410c1144d1 41 volatile float x_distance = 0;
christine222 0:86410c1144d1 42
christine222 0:86410c1144d1 43 volatile float y_accel = 0;
christine222 0:86410c1144d1 44 volatile float y_velocity = 0;
christine222 0:86410c1144d1 45 volatile float y_distance = 0;
christine222 0:86410c1144d1 46
christine222 0:86410c1144d1 47 volatile float z_accel = 0;
christine222 0:86410c1144d1 48 volatile float z_velocity = 0;
christine222 0:86410c1144d1 49 volatile float z_distance = 0;
christine222 0:86410c1144d1 50
christine222 0:86410c1144d1 51 // gyroscope data
christine222 0:86410c1144d1 52 volatile float x_omega = 0;
christine222 0:86410c1144d1 53 volatile float x_theta = 0;
christine222 0:86410c1144d1 54
christine222 0:86410c1144d1 55 volatile float y_omega = 0;
christine222 0:86410c1144d1 56 volatile float y_theta = 0;
christine222 0:86410c1144d1 57
christine222 0:86410c1144d1 58 volatile float z_omega = 0;
christine222 0:86410c1144d1 59 volatile float z_theta = 0;
christine222 0:86410c1144d1 60
christine222 0:86410c1144d1 61 // arrays to store sample data
christine222 0:86410c1144d1 62 // data[0][] = accel
christine222 0:86410c1144d1 63 // data[1][] = velocity
christine222 0:86410c1144d1 64 // data[2][] = position
christine222 0:86410c1144d1 65 const int DATA_SIZE = 500;
christine222 0:86410c1144d1 66 volatile float x_data[3][DATA_SIZE];
christine222 0:86410c1144d1 67 volatile float y_data[3][DATA_SIZE];
christine222 0:86410c1144d1 68 volatile float z_data[3][DATA_SIZE];
christine222 0:86410c1144d1 69
christine222 0:86410c1144d1 70 // angle[0][] = angular velocity
christine222 0:86410c1144d1 71 // angle[1][] = angle
christine222 0:86410c1144d1 72 volatile float x_angle[2][DATA_SIZE];
christine222 0:86410c1144d1 73 volatile float y_angle[2][DATA_SIZE];
christine222 0:86410c1144d1 74 volatile float z_angle[2][DATA_SIZE];
christine222 0:86410c1144d1 75
christine222 0:86410c1144d1 76 volatile int data_index = 0;
christine222 0:86410c1144d1 77
christine222 0:86410c1144d1 78
christine222 0:86410c1144d1 79 void updateValues(float dt) {
christine222 0:86410c1144d1 80 float gyro_data[3];
christine222 0:86410c1144d1 81 float accel_data[3];
christine222 0:86410c1144d1 82 float mag_data[3];
christine222 0:86410c1144d1 83
christine222 0:86410c1144d1 84 accel.acquire_accel_data_g(accel_data);
christine222 0:86410c1144d1 85 gyro.acquire_gyro_data_dps(gyro_data);
christine222 0:86410c1144d1 86 mag.acquire_mag_data_uT(mag_data);
christine222 0:86410c1144d1 87
christine222 0:86410c1144d1 88 // convert to m/s^2
christine222 0:86410c1144d1 89 x_accel = accel_data[0] * 9.8f;
christine222 0:86410c1144d1 90 y_accel = accel_data[1] * 9.8f;
christine222 0:86410c1144d1 91 z_accel = accel_data[2] * 9.8f;
christine222 0:86410c1144d1 92
christine222 0:86410c1144d1 93 x_omega = gyro_data[0];
christine222 0:86410c1144d1 94 y_omega = gyro_data[1];
christine222 0:86410c1144d1 95 z_omega = gyro_data[2];
christine222 0:86410c1144d1 96
christine222 0:86410c1144d1 97 // zero out readings for when stationary
christine222 0:86410c1144d1 98 if (abs(x_accel) < 0.3f) {
christine222 0:86410c1144d1 99 x_accel = 0;
christine222 0:86410c1144d1 100 }
christine222 0:86410c1144d1 101 if (abs(y_accel) < 0.3f) {
christine222 0:86410c1144d1 102 y_accel = 0;
christine222 0:86410c1144d1 103 }
christine222 0:86410c1144d1 104 if (abs(z_accel) < 0.3f) {
christine222 0:86410c1144d1 105 z_accel = 0;
christine222 0:86410c1144d1 106 }
christine222 0:86410c1144d1 107
christine222 0:86410c1144d1 108 if (abs(x_omega) < 0.5f) {
christine222 0:86410c1144d1 109 x_omega = 0;
christine222 0:86410c1144d1 110 }
christine222 0:86410c1144d1 111 if (abs(y_omega) < 0.5f) {
christine222 0:86410c1144d1 112 y_omega = 0;
christine222 0:86410c1144d1 113 }
christine222 0:86410c1144d1 114 if (abs(z_omega) < 0.5f) {
christine222 0:86410c1144d1 115 z_omega = 0;
christine222 0:86410c1144d1 116 }
christine222 0:86410c1144d1 117
christine222 0:86410c1144d1 118 // integrate acceleration
christine222 0:86410c1144d1 119 x_velocity += x_accel * dt;
christine222 0:86410c1144d1 120 y_velocity += y_accel * dt;
christine222 0:86410c1144d1 121 z_velocity += z_accel * dt;
christine222 0:86410c1144d1 122
christine222 0:86410c1144d1 123 // integrate velocity
christine222 0:86410c1144d1 124 x_distance += x_velocity * dt;
christine222 0:86410c1144d1 125 y_distance += y_velocity * dt;
christine222 0:86410c1144d1 126 z_distance += z_velocity * dt;
christine222 0:86410c1144d1 127
christine222 0:86410c1144d1 128 // integrate angular velocity
christine222 0:86410c1144d1 129 x_theta += x_omega * dt;
christine222 0:86410c1144d1 130 y_theta += y_omega * dt;
christine222 0:86410c1144d1 131 z_theta += z_omega * dt;
christine222 0:86410c1144d1 132
christine222 0:86410c1144d1 133 // store values to print later
christine222 0:86410c1144d1 134 x_data[0][data_index] = x_accel;
christine222 0:86410c1144d1 135 x_data[1][data_index] = x_velocity;
christine222 0:86410c1144d1 136 x_data[2][data_index] = x_distance;
christine222 0:86410c1144d1 137
christine222 0:86410c1144d1 138 y_data[0][data_index] = y_accel;
christine222 0:86410c1144d1 139 y_data[1][data_index] = y_velocity;
christine222 0:86410c1144d1 140 y_data[2][data_index] = y_distance;
christine222 0:86410c1144d1 141
christine222 0:86410c1144d1 142 z_data[0][data_index] = z_accel;
christine222 0:86410c1144d1 143 z_data[1][data_index] = z_velocity;
christine222 0:86410c1144d1 144 z_data[2][data_index] = z_distance;
christine222 0:86410c1144d1 145
christine222 0:86410c1144d1 146 x_angle[0][data_index] = x_omega;
christine222 0:86410c1144d1 147 x_angle[1][data_index] = x_theta;
christine222 0:86410c1144d1 148
christine222 0:86410c1144d1 149 y_angle[0][data_index] = y_omega;
christine222 0:86410c1144d1 150 y_angle[1][data_index] = y_theta;
christine222 0:86410c1144d1 151
christine222 0:86410c1144d1 152 z_angle[0][data_index] = z_omega;
christine222 0:86410c1144d1 153 z_angle[1][data_index] = z_theta;
christine222 0:86410c1144d1 154
christine222 0:86410c1144d1 155 data_index++;
christine222 0:86410c1144d1 156
christine222 0:86410c1144d1 157 // stop if index equals array size
christine222 0:86410c1144d1 158 if (data_index == DATA_SIZE) {
christine222 0:86410c1144d1 159 overflow = 1;
christine222 0:86410c1144d1 160 doMotion = 0;
christine222 0:86410c1144d1 161 }
christine222 0:86410c1144d1 162 }
christine222 0:86410c1144d1 163
christine222 0:86410c1144d1 164 void printValues() {
christine222 0:86410c1144d1 165 pc.printf("***Data Dump***\ntime,x_accel,y_accel,z_accel,x_velo,y_velo,z_velo,x_pos,y_pos,z_pos,x_omega,y_omega,z_omega,x_angle,y_angle,z_angle\n");
christine222 0:86410c1144d1 166 for (int i = 0; i < data_index; i+=4) {
christine222 0:86410c1144d1 167 pc.printf(
christine222 0:86410c1144d1 168 "%0.2f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\n%0.2f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\n%0.2f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\n%0.2f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\n",
christine222 0:86410c1144d1 169 (i+1)*0.01,x_data[0][i],y_data[0][i],z_data[0][i],x_data[1][i],y_data[1][i],z_data[1][i],
christine222 0:86410c1144d1 170 x_data[2][i],y_data[2][i],z_data[2][i],x_angle[0][i],y_angle[0][i],z_angle[0][i],
christine222 0:86410c1144d1 171 x_angle[1][i],y_angle[1][i],z_angle[1][i],
christine222 0:86410c1144d1 172
christine222 0:86410c1144d1 173 (i+1+1)*0.01,x_data[0][i+1],y_data[0][i+1],z_data[0][i+1],x_data[1][i+1],y_data[1][i+1],z_data[1][i+1],
christine222 0:86410c1144d1 174 x_data[2][i+1],y_data[2][i+1],z_data[2][i+1],x_angle[0][i+1],y_angle[0][i+1],z_angle[0][i+1],
christine222 0:86410c1144d1 175 x_angle[1][i+1],y_angle[1][i+1],z_angle[1][i+1],
christine222 0:86410c1144d1 176
christine222 0:86410c1144d1 177 (i+2+1)*0.01,x_data[0][i+2],y_data[0][i+2],z_data[0][i+2],x_data[1][i+2],y_data[1][i+2],z_data[1][i+2],
christine222 0:86410c1144d1 178 x_data[2][i+2],y_data[2][i+2],z_data[2][i+2],x_angle[0][i+2],y_angle[0][i+2],z_angle[0][i+2],
christine222 0:86410c1144d1 179 x_angle[1][i+2],y_angle[1][i+2],z_angle[1][i+2],
christine222 0:86410c1144d1 180
christine222 0:86410c1144d1 181 (i+3+1)*0.01,x_data[0][i+3],y_data[0][i+3],z_data[0][i+3],x_data[1][i+3],y_data[1][i+3],z_data[1][i+3],
christine222 0:86410c1144d1 182 x_data[2][i+3],y_data[2][i+3],z_data[2][i+3],x_angle[0][i+3],y_angle[0][i+3],z_angle[0][i+3],
christine222 0:86410c1144d1 183 x_angle[1][i+3],y_angle[1][i+3],z_angle[1][i+3]
christine222 0:86410c1144d1 184 );
christine222 0:86410c1144d1 185 }
christine222 0:86410c1144d1 186 }
christine222 0:86410c1144d1 187
christine222 0:86410c1144d1 188 // zero out all variables
christine222 0:86410c1144d1 189 void resetValues() {
christine222 0:86410c1144d1 190 x_accel = 0;
christine222 0:86410c1144d1 191 x_velocity = 0;
christine222 0:86410c1144d1 192 x_distance = 0;
christine222 0:86410c1144d1 193
christine222 0:86410c1144d1 194 y_accel = 0;
christine222 0:86410c1144d1 195 y_velocity = 0;
christine222 0:86410c1144d1 196 y_distance = 0;
christine222 0:86410c1144d1 197
christine222 0:86410c1144d1 198 z_accel = 0;
christine222 0:86410c1144d1 199 z_velocity = 0;
christine222 0:86410c1144d1 200 z_distance = 0;
christine222 0:86410c1144d1 201
christine222 0:86410c1144d1 202 x_omega = 0;
christine222 0:86410c1144d1 203 x_theta = 0;
christine222 0:86410c1144d1 204
christine222 0:86410c1144d1 205 y_omega = 0;
christine222 0:86410c1144d1 206 y_theta = 0;
christine222 0:86410c1144d1 207
christine222 0:86410c1144d1 208 z_omega = 0;
christine222 0:86410c1144d1 209 z_theta = 0;
christine222 0:86410c1144d1 210
christine222 0:86410c1144d1 211 data_index = 0;
christine222 0:86410c1144d1 212 for (int i = 0; i < DATA_SIZE; i++) {
christine222 0:86410c1144d1 213 x_data[0][i] = 0;
christine222 0:86410c1144d1 214 x_data[1][i] = 0;
christine222 0:86410c1144d1 215 x_data[2][i] = 0;
christine222 0:86410c1144d1 216
christine222 0:86410c1144d1 217 y_data[0][i] = 0;
christine222 0:86410c1144d1 218 y_data[1][i] = 0;
christine222 0:86410c1144d1 219 y_data[2][i] = 0;
christine222 0:86410c1144d1 220
christine222 0:86410c1144d1 221 z_data[0][i] = 0;
christine222 0:86410c1144d1 222 z_data[1][i] = 0;
christine222 0:86410c1144d1 223 z_data[2][i] = 0;
christine222 0:86410c1144d1 224
christine222 0:86410c1144d1 225 x_angle[0][i] = 0;
christine222 0:86410c1144d1 226 x_angle[1][i] = 0;
christine222 0:86410c1144d1 227
christine222 0:86410c1144d1 228 y_angle[0][i] = 0;
christine222 0:86410c1144d1 229 y_angle[1][i] = 0;
christine222 0:86410c1144d1 230
christine222 0:86410c1144d1 231 z_angle[0][i] = 0;
christine222 0:86410c1144d1 232 z_angle[1][i] = 0;
christine222 0:86410c1144d1 233 }
christine222 0:86410c1144d1 234 }
christine222 0:86410c1144d1 235
christine222 0:86410c1144d1 236 void btnLeft(void) {
christine222 0:86410c1144d1 237 startHaptic = 1;
christine222 0:86410c1144d1 238 haptic = 1;
christine222 0:86410c1144d1 239 doMotion = !doMotion; // start and stop motion with a button press
christine222 0:86410c1144d1 240 done = 1;
christine222 0:86410c1144d1 241 }
christine222 0:86410c1144d1 242
christine222 0:86410c1144d1 243 void btnRight(void) {
christine222 0:86410c1144d1 244 startHaptic = 1;
christine222 0:86410c1144d1 245 haptic = 1;
christine222 0:86410c1144d1 246 doMotion = !doMotion; // start and stop motion with a button press
christine222 0:86410c1144d1 247 done = 1;
christine222 0:86410c1144d1 248 }
christine222 0:86410c1144d1 249
christine222 0:86410c1144d1 250 void systick_function() {
christine222 0:86410c1144d1 251 if (doMotion) {
christine222 0:86410c1144d1 252 done = 0;
christine222 0:86410c1144d1 253 blueLed = 0;
christine222 0:86410c1144d1 254 updateValues(SYSTICK_PERIOD);
christine222 0:86410c1144d1 255 } else {
christine222 0:86410c1144d1 256 blueLed = 1;
christine222 0:86410c1144d1 257 }
christine222 0:86410c1144d1 258 if (startHaptic) {
christine222 0:86410c1144d1 259 tick_counter++;
christine222 0:86410c1144d1 260
christine222 0:86410c1144d1 261 if (tick_counter == tick_duration) {
christine222 0:86410c1144d1 262 haptic = 0;
christine222 0:86410c1144d1 263 startHaptic = 0;
christine222 0:86410c1144d1 264 tick_counter = 0;
christine222 0:86410c1144d1 265 }
christine222 0:86410c1144d1 266 }
christine222 0:86410c1144d1 267 }
christine222 0:86410c1144d1 268
christine222 0:86410c1144d1 269 int main() {
christine222 0:86410c1144d1 270 kw40z_device.attach_buttonLeft(&btnLeft);
christine222 0:86410c1144d1 271 kw40z_device.attach_buttonRight(&btnRight);
christine222 0:86410c1144d1 272
christine222 0:86410c1144d1 273 pc.printf("----This program has started----\n");
christine222 0:86410c1144d1 274 redLed = 0;
christine222 0:86410c1144d1 275
christine222 0:86410c1144d1 276 gyro.gyro_config();
christine222 0:86410c1144d1 277 accel.accel_config();
christine222 0:86410c1144d1 278 mag.mag_config();
christine222 0:86410c1144d1 279
christine222 0:86410c1144d1 280 // read sensors in systick
christine222 0:86410c1144d1 281 systick.attach(&systick_function, SYSTICK_PERIOD);
christine222 0:86410c1144d1 282
christine222 0:86410c1144d1 283 while (1) {
christine222 0:86410c1144d1 284 doMotion = 0;
christine222 0:86410c1144d1 285 updateValues(SYSTICK_PERIOD);
christine222 0:86410c1144d1 286 pc.printf("x_ang,%.3f,%.3f,\t", x_omega, x_theta);
christine222 0:86410c1144d1 287 pc.printf("y_ang,%.3f,%.3f,\t", y_omega, y_theta);
christine222 0:86410c1144d1 288 pc.printf("z_ang,%.3f,%.3f,\n", z_omega, z_theta);
christine222 0:86410c1144d1 289 wait(SYSTICK_PERIOD);
christine222 0:86410c1144d1 290 if (done) {
christine222 0:86410c1144d1 291 printValues();
christine222 0:86410c1144d1 292 resetValues();
christine222 0:86410c1144d1 293 done = 0;
christine222 0:86410c1144d1 294 }
christine222 0:86410c1144d1 295 if (overflow) {
christine222 0:86410c1144d1 296 pc.printf("Max number of samples exceeded\n");
christine222 0:86410c1144d1 297 printValues();
christine222 0:86410c1144d1 298 resetValues();
christine222 0:86410c1144d1 299 overflow = 0;
christine222 0:86410c1144d1 300 }
christine222 0:86410c1144d1 301 }
christine222 0:86410c1144d1 302 }