Robert Hill / Mbed 2 deprecated FRDM_AS_

This program tests and verifies the funcitonality of the Accelerometer as a means of calculating incline angle.

Dependencies:   FXOS8700Q mbed

Accel.cpp@0:101b684deb00, 2016-02-24 (annotated)

Committer:
roberthill04
Date:
Wed Feb 24 17:39:14 2016 +0000
Revision:
0:101b684deb00
Accelerometer Solo Functionality Program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
roberthill04 0:101b684deb00 1 /* FXOS8700Q Example Program
roberthill04 0:101b684deb00 2 * Copyright (c) 2014-2015 ARM Limited
roberthill04 0:101b684deb00 3 *
roberthill04 0:101b684deb00 4 * Licensed under the Apache License, Version 2.0 (the "License");
roberthill04 0:101b684deb00 5 * you may not use this file except in compliance with the License.
roberthill04 0:101b684deb00 6 * You may obtain a copy of the License at
roberthill04 0:101b684deb00 7 *
roberthill04 0:101b684deb00 8 * http://www.apache.org/licenses/LICENSE-2.0
roberthill04 0:101b684deb00 9 *
roberthill04 0:101b684deb00 10 * Unless required by applicable law or agreed to in writing, software
roberthill04 0:101b684deb00 11 * distributed under the License is distributed on an "AS IS" BASIS,
roberthill04 0:101b684deb00 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
roberthill04 0:101b684deb00 13 * See the License for the specific language governing permissions and
roberthill04 0:101b684deb00 14 * limitations under the License.
roberthill04 0:101b684deb00 15 */
roberthill04 0:101b684deb00 16
roberthill04 0:101b684deb00 17 #include "mbed.h"
roberthill04 0:101b684deb00 18 #include "FXOS8700Q.h"
roberthill04 0:101b684deb00 19
roberthill04 0:101b684deb00 20
roberthill04 0:101b684deb00 21
roberthill04 0:101b684deb00 22
roberthill04 0:101b684deb00 23 #define PI 3.14159265
roberthill04 0:101b684deb00 24
roberthill04 0:101b684deb00 25 Serial pc(USBTX, USBRX);
roberthill04 0:101b684deb00 26 I2C i2c(PTE25, PTE24);
roberthill04 0:101b684deb00 27 //FXOS8700Q fxos(i2c, FXOS8700CQ_SLAVE_ADDR1);
roberthill04 0:101b684deb00 28 FXOS8700QAccelerometer acc(i2c, FXOS8700CQ_SLAVE_ADDR1); // Configured for the FRDM-K64F with onboard sensors
roberthill04 0:101b684deb00 29 //FXOS8700QMagnetometer mag(i2c, FXOS8700CQ_SLAVE_ADDR1);
roberthill04 0:101b684deb00 30
roberthill04 0:101b684deb00 31 //FXOS8700QAccelerometer acc(i2c, FXOS8700CQ_SLAVE_ADDR0); // Configured for use with the FRDM-MULTI shield
roberthill04 0:101b684deb00 32 //FXOS8700QMagnetometer mag(i2c, FXOS8700CQ_SLAVE_ADDR0);
roberthill04 0:101b684deb00 33
roberthill04 0:101b684deb00 34 DigitalOut led_red(LED_RED);
roberthill04 0:101b684deb00 35 DigitalOut led_green(LED_GREEN);
roberthill04 0:101b684deb00 36 DigitalOut led_blue(LED_BLUE);
roberthill04 0:101b684deb00 37 DigitalIn sw2(SW2);
roberthill04 0:101b684deb00 38 DigitalIn sw3(SW3);
roberthill04 0:101b684deb00 39 double angleX, denomX_T, denomX_A, denomX_B, denomX_C; //intializing variable to hold the angle calculation
roberthill04 0:101b684deb00 40 double angleY, denomY_T, denomY_A, denomY_B, denomY_C; //and denominator pieces of algorithm
roberthill04 0:101b684deb00 41 bool Button_Pressed = true; //see algorithm in main for what denom_A & _B are defined as
roberthill04 0:101b684deb00 42
roberthill04 0:101b684deb00 43
roberthill04 0:101b684deb00 44
roberthill04 0:101b684deb00 45 int main(void)
roberthill04 0:101b684deb00 46 {
roberthill04 0:101b684deb00 47 motion_data_units_t acc_data;
roberthill04 0:101b684deb00 48 // motion_data_counts_t acc_raw; //commented out because we don't need this data
roberthill04 0:101b684deb00 49 float faX, faY, faZ, tmp_float;
roberthill04 0:101b684deb00 50 // int16_t raX, raY, raZ, tmp_int; //commented out because we don't need this data
roberthill04 0:101b684deb00 51
roberthill04 0:101b684deb00 52 acc.enable();
roberthill04 0:101b684deb00 53
roberthill04 0:101b684deb00 54 printf("FXOS8700QAccelerometer Who Am I= %X\r\n", acc.whoAmI());
roberthill04 0:101b684deb00 55 //printf("FXOS8700QMagnetometer Who Am I= %X\r\n", acc.whoAmI());
roberthill04 0:101b684deb00 56 while (1) {
roberthill04 0:101b684deb00 57 // counts based results
roberthill04 0:101b684deb00 58 /*
roberthill04 0:101b684deb00 59 acc.getAxis(acc_raw);
roberthill04 0:101b684deb00 60
roberthill04 0:101b684deb00 61 printf("ACC: X=%06dd Y=%06dd Z=%06dd \t \r\n", acc_raw.x, acc_raw.y, acc_raw.z);
roberthill04 0:101b684deb00 62 acc.getX(raX);
roberthill04 0:101b684deb00 63 acc.getY(raY);
roberthill04 0:101b684deb00 64 acc.getZ(raZ);
roberthill04 0:101b684deb00 65
roberthill04 0:101b684deb00 66 printf("ACC: X=%06dd Y=%06dd Z=%06dd \t \r\n", raX, raY, raZ);
roberthill04 0:101b684deb00 67 printf("ACC: X=%06dd Y=%06dd Z=%06dd \t \r\n", acc.getX(tmp_int), acc.getY(tmp_int), acc.getZ(tmp_int));
roberthill04 0:101b684deb00 68 */
roberthill04 0:101b684deb00 69
roberthill04 0:101b684deb00 70 // unit based results
roberthill04 0:101b684deb00 71 acc.getAxis(acc_data);
roberthill04 0:101b684deb00 72
roberthill04 0:101b684deb00 73 printf("ACC: X=%1.4ff Y=%1.4ff Z=%1.4ff \t \r\n", acc_data.x, acc_data.y, acc_data.z);
roberthill04 0:101b684deb00 74 acc.getX(faX);
roberthill04 0:101b684deb00 75 acc.getY(faY);
roberthill04 0:101b684deb00 76 acc.getZ(faZ);
roberthill04 0:101b684deb00 77
roberthill04 0:101b684deb00 78 printf("ACC: X=%1.4ff Y=%1.4ff Z=%1.4ff \t \r\n", faX, faY, faZ);
roberthill04 0:101b684deb00 79 printf("ACC: X=%1.4ff Y=%1.4ff Z=%1.4ff \t \r\n", acc.getX(tmp_float), acc.getY(tmp_float), acc.getZ(tmp_float));
roberthill04 0:101b684deb00 80
roberthill04 0:101b684deb00 81
roberthill04 0:101b684deb00 82 /*
roberthill04 0:101b684deb00 83 In order to calculate angle from accel data use the following algorithm:
roberthill04 0:101b684deb00 84 Ax = arctan( rawX/sqrt(rawY^2 + rawZ^2))
roberthill04 0:101b684deb00 85 Ay = arctan( rawY/sqrt(rawX^2 + rawZ^2))
roberthill04 0:101b684deb00 86 using Ax as an example:
roberthill04 0:101b684deb00 87 we have denom_T = the total denominator = (sqrt(rawY^2 + rawZ^2 )
roberthill04 0:101b684deb00 88 denom_A = rawY^2 && denom_B = rawZ^2 && denom_C = denom_A + denom_B
roberthill04 0:101b684deb00 89 we may only be concerned with one of these angles for our application
roberthill04 0:101b684deb00 90 also note value is output in radians will need to convert to degrees using: 180/PI
roberthill04 0:101b684deb00 91
roberthill04 0:101b684deb00 92 example of how to use inverse tangent function :
roberthill04 0:101b684deb00 93
roberthill04 0:101b684deb00 94 int main ()
roberthill04 0:101b684deb00 95 {
roberthill04 0:101b684deb00 96 double param, result;
roberthill04 0:101b684deb00 97 param = 1.0;
roberthill04 0:101b684deb00 98 result = atan (param) * 180 / PI;
roberthill04 0:101b684deb00 99 printf ("The arc tangent of %f is %f degrees\n", param, result );
roberthill04 0:101b684deb00 100 return 0;
roberthill04 0:101b684deb00 101 }
roberthill04 0:101b684deb00 102 */
roberthill04 0:101b684deb00 103
roberthill04 0:101b684deb00 104 /*
roberthill04 0:101b684deb00 105 Example of pow: 7 ^ 3 would be written as pow(7.0, 3.0);
roberthill04 0:101b684deb00 106 we can use this find squareroots by making the power exponent = 0.5
roberthill04 0:101b684deb00 107 */
roberthill04 0:101b684deb00 108
roberthill04 0:101b684deb00 109 //X-AXIS
roberthill04 0:101b684deb00 110 denomX_A = pow(faY, 2);
roberthill04 0:101b684deb00 111 denomX_B = pow(faZ, 2);
roberthill04 0:101b684deb00 112 denomX_C = denomX_A + denomX_B;
roberthill04 0:101b684deb00 113 denomX_T = pow(denomX_C, .5); //pow returns base raised to the power exponent
roberthill04 0:101b684deb00 114
roberthill04 0:101b684deb00 115 angleX = atan(faX/denomX_T) * 180/PI; //should calculate the angle on the X axis in degrees based on raw data
roberthill04 0:101b684deb00 116
roberthill04 0:101b684deb00 117 //Y-AXIS
roberthill04 0:101b684deb00 118 denomY_A = pow(faX, 2);
roberthill04 0:101b684deb00 119 denomY_B = pow(faZ, 2);
roberthill04 0:101b684deb00 120 denomY_C = denomY_A + denomY_B;
roberthill04 0:101b684deb00 121 denomY_T = pow(denomY_C, .5); //pow returns base raised to the power exponent
roberthill04 0:101b684deb00 122
roberthill04 0:101b684deb00 123 angleY = atan(faY/denomY_T) * 180/PI; //should calculate the angle on the Y axis in degrees based on raw data
roberthill04 0:101b684deb00 124
roberthill04 0:101b684deb00 125 printf("Approximate angle in the X-Axis =%f degrees \t \r\n", angleX); //should print out angle in degrees
roberthill04 0:101b684deb00 126 printf("Approximate angle in the Y-Axis =%f degrees \t \r\n", angleY);
roberthill04 0:101b684deb00 127 puts("");
roberthill04 0:101b684deb00 128 wait(5.0f);
roberthill04 0:101b684deb00 129
roberthill04 0:101b684deb00 130 if(sw3 == 0 && Button_Pressed == true)
roberthill04 0:101b684deb00 131 {
roberthill04 0:101b684deb00 132 acc.disable();
roberthill04 0:101b684deb00 133 printf("Program is paused, press button again to resume.\t \r\n");
roberthill04 0:101b684deb00 134 Button_Pressed =false;
roberthill04 0:101b684deb00 135 }
roberthill04 0:101b684deb00 136 wait(2);
roberthill04 0:101b684deb00 137 if(sw3 == 0 && Button_Pressed == false)
roberthill04 0:101b684deb00 138 {
roberthill04 0:101b684deb00 139 acc.enable();
roberthill04 0:101b684deb00 140 Button_Pressed = true;
roberthill04 0:101b684deb00 141 }
roberthill04 0:101b684deb00 142
roberthill04 0:101b684deb00 143
roberthill04 0:101b684deb00 144 }
roberthill04 0:101b684deb00 145 }