acc_341.cpp

00001 /* 
00002  Test of the accelerometer, digital I/O, on-board LCD screen.
00003  RED: Until absolute vertical is reached
00004  GREEN: Until absolute horizontal is reached
00005  
00006  Author: Siphamandla P. Simelane
00007  Date: 10/6/2014
00008  */
00009 
00010 #include "mbed.h"
00011 #include "MMA8451Q.h"
00012 #include "SLCD.h"
00013 #include "math.h"
00014 
00015 // Configuring Accelerometer/Magnetometer: 
00016 #if   defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
00017   PinName const SDA = PTE25;  // Data pins for the accelerometer/magnetometer.
00018   PinName const SCL = PTE24;  // DO NOT CHANGE
00019 #elif defined (TARGET_KL05Z)
00020   PinName const SDA = PTB4;
00021   PinName const SCL = PTB3;
00022 #else
00023   #error TARGET NOT DEFINED
00024 #endif
00025 
00026 #define MMA8451_I2C_ADDRESS (0x1d<<1)
00027 
00028 #define PI 3.14159265
00029 #define DATATIME 0.150
00030 #define PROGNAME "ACCLCD341VB\r/n"
00031 #define PRINTDBUG
00032 #define LEDON false
00033 #define LEDOFF true
00034 #define LCDLEN 10
00035 #define LOW_POINT 0.0
00036 #define LOW_LIMIT 0.1
00037 #define HIGH_LIMIT 1.1
00038 #define BLINKTIME 0.2// milliseconds
00039 
00040 
00041 enum orientStates {INTERMEDIATE, LANDSCAPE, PORTRAIT}; // define the states
00042 PwmOut greenLed(LED_GREEN);
00043 PwmOut redLed(LED_RED);
00044 SLCD slcd; //define LCD display
00045 
00046 MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
00047 Serial pc(USBTX, USBRX);
00048 
00049 float sqrt_newt(float arg) {
00050     int i = 0;
00051     float X1 = 0.0;
00052     int intMax = 30;
00053     int epsilon = 1e-7;
00054     float my_x = float(arg/2.0);
00055     int delta = 1;
00056     for(i=0; i<intMax; i++){
00057         X1 = 0.5*(my_x + (arg/my_x));
00058         delta = abs(X1-my_x);
00059         if (delta < epsilon)
00060             break;
00061         else
00062             my_x = X1;
00063     }
00064     return my_x;
00065     
00066 //    return (sqrt(arg));
00067 }
00068 
00069 // Print using LCD screen
00070 void LCDMess(char *lMess, float dWait){
00071     slcd.Home();
00072     slcd.clear();
00073     slcd.printf(lMess);
00074     wait(dWait);
00075 } 
00076 
00077 
00078 int main() {
00079     float xAcc;
00080     float yAcc;
00081     float vector;
00082     float angle;  
00083     char lcdData[LCDLEN]; //buffer needs places dor decimal pt and colon
00084     orientStates PGMState = INTERMEDIATE;
00085         
00086     #ifdef PRINTDBUG
00087         pc.printf(PROGNAME);
00088     #endif
00089 
00090   // main loop forever 
00091   while(true) {
00092     switch (PGMState){ 
00093             case INTERMEDIATE: 
00094                 //Get accelerometer data - tilt angles minus offset for zero mark.
00095                 xAcc = abs(acc.getAccX());
00096                 yAcc = abs(acc.getAccY()); 
00097                 
00098                 // Calulate vector sum and angle of x and y reading.       
00099                 vector = sqrt_newt(pow(xAcc,2) + pow(yAcc,2));
00100                 angle = atan(yAcc / xAcc)* 180 / PI;                
00101                 
00102                 #ifdef PRINTDBUG
00103                    pc.printf("xAcc = %f\r\n", xAcc);
00104                    pc.printf("yAcc = %f\r\n", yAcc);
00105                    pc.printf("vector = %f\r\n",  vector);
00106                    pc.printf("Angle = %f\r\n",  angle);
00107                 #endif                 
00108                 sprintf (lcdData,"%4.3f", vector);
00109                 LCDMess(lcdData, DATATIME);                                
00110                 
00111                 // Define the landscape and portrait position using x and y readings
00112                 if(yAcc < LOW_LIMIT && xAcc > LOW_POINT && xAcc < HIGH_LIMIT){
00113                     PGMState = PORTRAIT;
00114                 } else if (xAcc < LOW_LIMIT && yAcc > LOW_POINT && yAcc < HIGH_LIMIT){
00115                     PGMState = LANDSCAPE;
00116                 } else {
00117                     PGMState = INTERMEDIATE;    
00118                 }
00119                 break;
00120               
00121              case PORTRAIT:   
00122                 // Green led ON and red OFF            
00123                 redLed.write(LEDON);
00124                 greenLed.write(LEDOFF);
00125                 PGMState = INTERMEDIATE;  // go idle state
00126                 break;                                
00127               
00128              case LANDSCAPE:  
00129                 // Green led OFF and red ON
00130                 redLed.write(LEDOFF);
00131                 greenLed.write(LEDON);
00132                 PGMState = INTERMEDIATE;  // go idle state
00133                 break;
00134                 
00135             default:
00136                 PGMState = INTERMEDIATE;  // go idle state
00137                 break;
00138               
00139         } // end state machine
00140     }
00141 }