Stanley Cohen / Mbed 2 deprecated ACC_LCD_341_MID_hys

8.1 MT solution for accelerometer program with hysteresis

Dependencies:   MMA8451Q SLCD mbed

Fork of ACC_LCD_341_MID by Stanley Cohen

acc_341.cpp@3:b425cdf7c99c, 2014-10-06 (annotated)

Committer:
scohennm
Date:
Mon Oct 06 19:31:52 2014 +0000
Revision:
3:b425cdf7c99c
Parent:
2:6003ed409def 
8.1 MT solution for accelerometer program with hysteresis

Who changed what in which revision?

UserRevisionLine numberNew contents of line
scohennm 0:203b4129a213 1 #include "mbed.h"
scohennm 0:203b4129a213 2 #include "MMA8451Q.h"
scohennm 0:203b4129a213 3 #include "SLCD.h"
scohennm 0:203b4129a213 4
scohennm 0:203b4129a213 5 /*
scohennm 0:203b4129a213 6 Test of the accelerometer, digital I/O, on-board LCD screen.
scohennm 0:203b4129a213 7 Looing at vector product of the x-y components of the accelerometer.
scohennm 0:203b4129a213 8 Works pretty well. Still rough, program wise - sc 140710
scohennm 0:203b4129a213 9 */
scohennm 3:b425cdf7c99c 10 // solution UG 1
scohennm 3:b425cdf7c99c 11 #define NUMLEDS 2
scohennm 3:b425cdf7c99c 12 #define PORTRAIT 0
scohennm 3:b425cdf7c99c 13 #define PORTRAIT1 1 // Add bandwidth
scohennm 3:b425cdf7c99c 14 #define LANDSCAPE 10
scohennm 3:b425cdf7c99c 15 #define LANDSCAPE1 9
scohennm 3:b425cdf7c99c 16 #define LEDON 0
scohennm 3:b425cdf7c99c 17 #define LEDOFF 1
scohennm 3:b425cdf7c99c 18 #define RED 0
scohennm 3:b425cdf7c99c 19 #define GREEN 1
scohennm 3:b425cdf7c99c 20 #define ACCSCALING 10.0
scohennm 3:b425cdf7c99c 21 #define HYSTOFFSET 4
scohennm 3:b425cdf7c99c 22 DigitalOut LEDs[NUMLEDS]={LED_RED, LED_GREEN}; // plan for scalability
scohennm 3:b425cdf7c99c 23 // end UG 1
scohennm 0:203b4129a213 24 #define DATATIME 0.200
scohennm 0:203b4129a213 25
scohennm 0:203b4129a213 26 #define PROGNAME "ACCLCD341\r/n"
scohennm 0:203b4129a213 27
scohennm 3:b425cdf7c99c 28 // #define PRINTDBUG
scohennm 2:6003ed409def 29 //
scohennm 0:203b4129a213 30 #if defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
scohennm 2:6003ed409def 31 PinName const SDA = PTE25; // Data pins for the accelerometer/magnetometer.
scohennm 2:6003ed409def 32 PinName const SCL = PTE24; // DO NOT CHANGE
scohennm 0:203b4129a213 33 #elif defined (TARGET_KL05Z)
scohennm 0:203b4129a213 34 PinName const SDA = PTB4;
scohennm 0:203b4129a213 35 PinName const SCL = PTB3;
scohennm 0:203b4129a213 36 #else
scohennm 0:203b4129a213 37 #error TARGET NOT DEFINED
scohennm 0:203b4129a213 38 #endif
scohennm 0:203b4129a213 39
scohennm 0:203b4129a213 40 #define MMA8451_I2C_ADDRESS (0x1d<<1)
scohennm 0:203b4129a213 41
scohennm 0:203b4129a213 42 SLCD slcd; //define LCD display
scohennm 0:203b4129a213 43
scohennm 0:203b4129a213 44 MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
scohennm 0:203b4129a213 45 Serial pc(USBTX, USBRX);
scohennm 0:203b4129a213 46
scohennm 2:6003ed409def 47 float sqrt_newt(float argument) {
scohennm 3:b425cdf7c99c 48 int i = 0;
scohennm 3:b425cdf7c99c 49 float xnew = 0.0;
scohennm 3:b425cdf7c99c 50 int itermax = 20;
scohennm 3:b425cdf7c99c 51 float epsilon = 1e-7;
scohennm 3:b425cdf7c99c 52 float xold = argument/2.0;
scohennm 3:b425cdf7c99c 53 float delta = 1;
scohennm 3:b425cdf7c99c 54 while ((delta > epsilon) && (i < itermax)){
scohennm 3:b425cdf7c99c 55 xnew = 0.5*(xold + (argument/xold));
scohennm 3:b425cdf7c99c 56 delta = abs(xnew-xold);
scohennm 3:b425cdf7c99c 57 xold = xnew;
scohennm 3:b425cdf7c99c 58 i++;
scohennm 3:b425cdf7c99c 59 #ifdef PRINTDBUG
scohennm 3:b425cdf7c99c 60 // wait(0.1);
scohennm 3:b425cdf7c99c 61 pc.printf("%5.4f %5.4e\r\n",xold, delta);
scohennm 3:b425cdf7c99c 62 #endif
scohennm 3:b425cdf7c99c 63 } // end while
scohennm 3:b425cdf7c99c 64 return (xold);
scohennm 3:b425cdf7c99c 65
scohennm 2:6003ed409def 66 }
scohennm 2:6003ed409def 67
scohennm 0:203b4129a213 68
scohennm 0:203b4129a213 69 void LCDMess(char *lMess, float dWait){
scohennm 0:203b4129a213 70 slcd.Home();
scohennm 0:203b4129a213 71 slcd.clear();
scohennm 0:203b4129a213 72 slcd.printf(lMess);
scohennm 0:203b4129a213 73 wait(dWait);
scohennm 0:203b4129a213 74 }
scohennm 0:203b4129a213 75
scohennm 0:203b4129a213 76
scohennm 0:203b4129a213 77 int main() {
scohennm 3:b425cdf7c99c 78 int i;
scohennm 0:203b4129a213 79 float xAcc;
scohennm 0:203b4129a213 80 float yAcc;
scohennm 3:b425cdf7c99c 81 float zAcc;
scohennm 0:203b4129a213 82 float vector;
scohennm 3:b425cdf7c99c 83 int positionState;
scohennm 0:203b4129a213 84 char lcdData[10]; //buffer needs places dor decimal pt and colon
scohennm 3:b425cdf7c99c 85 Timer DATATimer;
scohennm 3:b425cdf7c99c 86 int hyst = HYSTOFFSET;
scohennm 3:b425cdf7c99c 87
scohennm 3:b425cdf7c99c 88
scohennm 0:203b4129a213 89
scohennm 0:203b4129a213 90 #ifdef PRINTDBUG
scohennm 0:203b4129a213 91 pc.printf(PROGNAME);
scohennm 0:203b4129a213 92 #endif
scohennm 3:b425cdf7c99c 93 DATATimer.start();
scohennm 3:b425cdf7c99c 94 DATATimer.reset();
scohennm 0:203b4129a213 95 // main loop forever
scohennm 0:203b4129a213 96 while(true) {
scohennm 3:b425cdf7c99c 97 while (DATATimer.read_ms() > DATATIME) {
scohennm 3:b425cdf7c99c 98
scohennm 0:203b4129a213 99 //Get accelerometer data - tilt angles minus offset for zero mark.
scohennm 3:b425cdf7c99c 100 xAcc = abs(acc.getAccX());
scohennm 3:b425cdf7c99c 101 yAcc = abs(acc.getAccY());
scohennm 3:b425cdf7c99c 102 zAcc = abs(acc.getAccZ());
scohennm 0:203b4129a213 103 // Calulate vector sum of x and y reading.
scohennm 3:b425cdf7c99c 104 vector = sqrt_newt(pow(xAcc,2) + pow(yAcc,2));
scohennm 3:b425cdf7c99c 105 // Calculate the state based on orientaion
scohennm 3:b425cdf7c99c 106 positionState = int(ACCSCALING * xAcc) + hyst; // Create a Deadband
scohennm 3:b425cdf7c99c 107 if (positionState >= LANDSCAPE){
scohennm 3:b425cdf7c99c 108 positionState = LANDSCAPE;
scohennm 3:b425cdf7c99c 109 } else {
scohennm 3:b425cdf7c99c 110 positionState = PORTRAIT;
scohennm 3:b425cdf7c99c 111 }
scohennm 3:b425cdf7c99c 112
scohennm 3:b425cdf7c99c 113 // State Machine
scohennm 3:b425cdf7c99c 114 switch (positionState){
scohennm 3:b425cdf7c99c 115 case LANDSCAPE:
scohennm 3:b425cdf7c99c 116 case LANDSCAPE1: {
scohennm 3:b425cdf7c99c 117 LEDs[RED].write(LEDON);
scohennm 3:b425cdf7c99c 118 LEDs[GREEN].write (LEDOFF);
scohennm 3:b425cdf7c99c 119 hyst = HYSTOFFSET;
scohennm 3:b425cdf7c99c 120 break;
scohennm 3:b425cdf7c99c 121 }
scohennm 3:b425cdf7c99c 122 case PORTRAIT:
scohennm 3:b425cdf7c99c 123 case PORTRAIT1: {
scohennm 3:b425cdf7c99c 124 LEDs[RED].write(LEDOFF);
scohennm 3:b425cdf7c99c 125 LEDs[GREEN].write (LEDON);
scohennm 3:b425cdf7c99c 126 hyst= 0;
scohennm 3:b425cdf7c99c 127 break;
scohennm 3:b425cdf7c99c 128 }
scohennm 3:b425cdf7c99c 129 default: {
scohennm 3:b425cdf7c99c 130 for (i = 0; i< NUMLEDS; i++){
scohennm 3:b425cdf7c99c 131 LEDs[i].write(LEDON);
scohennm 3:b425cdf7c99c 132 }
scohennm 3:b425cdf7c99c 133 break;
scohennm 3:b425cdf7c99c 134 }
scohennm 3:b425cdf7c99c 135 } //switch
scohennm 0:203b4129a213 136
scohennm 0:203b4129a213 137 #ifdef PRINTDBUG
scohennm 1:9340a340e588 138 pc.printf("xAcc = %f\r\n", xAcc);
scohennm 1:9340a340e588 139 pc.printf("yAcc = %f\r\n", yAcc);
scohennm 3:b425cdf7c99c 140 pc.printf("xAcc = %f\r\n", zAcc);
scohennm 0:203b4129a213 141 pc.printf("vector = %f\r\n", vector);
scohennm 0:203b4129a213 142 #endif
scohennm 0:203b4129a213 143
scohennm 3:b425cdf7c99c 144 sprintf (lcdData,"%4.3f",vector);
scohennm 3:b425cdf7c99c 145 LCDMess(lcdData, vector);
scohennm 3:b425cdf7c99c 146 DATATimer.reset();
scohennm 0:203b4129a213 147 // Wait then do the whole thing again.
scohennm 3:b425cdf7c99c 148 } // end whle for timer
scohennm 3:b425cdf7c99c 149 }// end forever while
scohennm 0:203b4129a213 150 }