Minh Nguyen / Accelerometer

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MPU6050

Accelerometer.cpp@7:3b5aa5bea044, 2021-03-12 (annotated)

Committer:
khaiminhvn
Date:
Fri Mar 12 04:13:29 2021 +0000
Revision:
7:3b5aa5bea044
Parent:
6:754d21f44075 
Minor changes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
khaiminhvn 0:ab4465742afd 1 //INCLUDES
khaiminhvn 0:ab4465742afd 2 #include "Accelerometer.h"
khaiminhvn 0:ab4465742afd 3 #include "PinAssignment.h"
khaiminhvn 0:ab4465742afd 4
khaiminhvn 0:ab4465742afd 5 //Constructor
khaiminhvn 5:de016948db9c 6 Accelerometer::Accelerometer(I2C *i2cIn) : Panel(PIN_ACCEL_PANEL),R1(PIN_ACCEL_R1),
khaiminhvn 5:de016948db9c 7 R2(PIN_ACCEL_R2),acc(i2cIn)
khaiminhvn 0:ab4465742afd 8 {
khaiminhvn 0:ab4465742afd 9 //Set Output Control
khaiminhvn 0:ab4465742afd 10 Panel = 0; //Panel Array
khaiminhvn 0:ab4465742afd 11 R1 = 1; //Reflector 2
khaiminhvn 0:ab4465742afd 12 R2 = 1; //Reflector 1
khaiminhvn 5:de016948db9c 13 mul = 1;
khaiminhvn 0:ab4465742afd 14
khaiminhvn 0:ab4465742afd 15 //Set Range
khaiminhvn 2:f81ab5d2f0a2 16 acc.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
khaiminhvn 5:de016948db9c 17 }
khaiminhvn 5:de016948db9c 18
khaiminhvn 5:de016948db9c 19 //calibrate
khaiminhvn 5:de016948db9c 20 void Accelerometer::calibrate(){
khaiminhvn 2:f81ab5d2f0a2 21 AOffset[S_PANEL] = getAngleCal(S_PANEL,N_CAL);
khaiminhvn 2:f81ab5d2f0a2 22 AOffset[S_R1] = getAngleCal(S_R1,N_CAL);
khaiminhvn 2:f81ab5d2f0a2 23 AOffset[S_R2] = getAngleCal(S_R2,N_CAL);
khaiminhvn 0:ab4465742afd 24 }
khaiminhvn 0:ab4465742afd 25
khaiminhvn 0:ab4465742afd 26 //setSource
khaiminhvn 0:ab4465742afd 27 void Accelerometer::setSource(int n)
khaiminhvn 0:ab4465742afd 28 {
khaiminhvn 0:ab4465742afd 29 switch(n)
khaiminhvn 0:ab4465742afd 30 {
khaiminhvn 0:ab4465742afd 31 case S_PANEL: //Panel Array
khaiminhvn 0:ab4465742afd 32 Panel = 0;
khaiminhvn 0:ab4465742afd 33 R1 = 1;
khaiminhvn 0:ab4465742afd 34 R2 = 1;
khaiminhvn 2:f81ab5d2f0a2 35 curOffset = AOffset[S_PANEL];
khaiminhvn 5:de016948db9c 36 mul = MUL_P;
khaiminhvn 0:ab4465742afd 37 break;
khaiminhvn 0:ab4465742afd 38 case S_R1: //Reflector 1
khaiminhvn 0:ab4465742afd 39 Panel = 1;
khaiminhvn 0:ab4465742afd 40 R1 = 0;
khaiminhvn 0:ab4465742afd 41 R2 = 1;
khaiminhvn 2:f81ab5d2f0a2 42 curOffset = AOffset[S_R1];
khaiminhvn 5:de016948db9c 43 mul = MUL_R1;
khaiminhvn 0:ab4465742afd 44 break;
khaiminhvn 0:ab4465742afd 45 case S_R2: //Reflector 2
khaiminhvn 0:ab4465742afd 46 Panel = 1;
khaiminhvn 0:ab4465742afd 47 R1 = 1;
khaiminhvn 0:ab4465742afd 48 R2 = 0;
khaiminhvn 2:f81ab5d2f0a2 49 curOffset = AOffset[S_R2];
khaiminhvn 5:de016948db9c 50 mul = MUL_R2;
khaiminhvn 0:ab4465742afd 51 break;
khaiminhvn 0:ab4465742afd 52 }
khaiminhvn 0:ab4465742afd 53 }
khaiminhvn 0:ab4465742afd 54
khaiminhvn 0:ab4465742afd 55 //checkConnection
khaiminhvn 0:ab4465742afd 56 bool Accelerometer::checkConnection()
khaiminhvn 0:ab4465742afd 57 {
khaiminhvn 0:ab4465742afd 58 return acc.testConnection();
khaiminhvn 0:ab4465742afd 59 }
khaiminhvn 0:ab4465742afd 60
khaiminhvn 2:f81ab5d2f0a2 61 //getAngleCal
khaiminhvn 2:f81ab5d2f0a2 62 float Accelerometer::getAngleCal(int s, int n)
khaiminhvn 2:f81ab5d2f0a2 63 {
khaiminhvn 2:f81ab5d2f0a2 64 float ang[3];
khaiminhvn 2:f81ab5d2f0a2 65 float ang_out = 0;
khaiminhvn 2:f81ab5d2f0a2 66 float a[n];
khaiminhvn 2:f81ab5d2f0a2 67 Accelerometer::setSource(s);
khaiminhvn 2:f81ab5d2f0a2 68
khaiminhvn 2:f81ab5d2f0a2 69 for(int i = 0;i < n;i++)
khaiminhvn 2:f81ab5d2f0a2 70 {
khaiminhvn 2:f81ab5d2f0a2 71 acc.getAcceleroAngle(ang);
khaiminhvn 2:f81ab5d2f0a2 72 a[i] = ang[MEAS_AXIS];
khaiminhvn 5:de016948db9c 73 wait_us(100);
khaiminhvn 2:f81ab5d2f0a2 74 }
khaiminhvn 5:de016948db9c 75
khaiminhvn 5:de016948db9c 76 sort(a,a+n);
khaiminhvn 5:de016948db9c 77
khaiminhvn 6:754d21f44075 78 return a[(int)n/2]*mul;
khaiminhvn 2:f81ab5d2f0a2 79 }
khaiminhvn 2:f81ab5d2f0a2 80
khaiminhvn 0:ab4465742afd 81 //getAngle
khaiminhvn 0:ab4465742afd 82 float Accelerometer::getAngle(int s)
khaiminhvn 0:ab4465742afd 83 {
khaiminhvn 0:ab4465742afd 84 float ang[3];
khaiminhvn 0:ab4465742afd 85 float ang_out = 0;
khaiminhvn 2:f81ab5d2f0a2 86 float a[N_AVG];
khaiminhvn 0:ab4465742afd 87 Accelerometer::setSource(s);
khaiminhvn 0:ab4465742afd 88
khaiminhvn 0:ab4465742afd 89 for(int i = 0;i < N_AVG;i++)
khaiminhvn 0:ab4465742afd 90 {
khaiminhvn 0:ab4465742afd 91 acc.getAcceleroAngle(ang);
khaiminhvn 2:f81ab5d2f0a2 92 a[i] = ang[MEAS_AXIS];
khaiminhvn 5:de016948db9c 93 wait_us(100);
khaiminhvn 0:ab4465742afd 94 }
khaiminhvn 5:de016948db9c 95
khaiminhvn 5:de016948db9c 96 sort(a,a+N_AVG);
khaiminhvn 5:de016948db9c 97
khaiminhvn 6:754d21f44075 98 return a[(int)N_AVG/2]*mul-curOffset;
khaiminhvn 0:ab4465742afd 99 }
khaiminhvn 0:ab4465742afd 100
khaiminhvn 2:f81ab5d2f0a2 101
khaiminhvn 0:ab4465742afd 102 //checkAngle
khaiminhvn 0:ab4465742afd 103 bool Accelerometer::checkAngle(float ref, float cur)
khaiminhvn 0:ab4465742afd 104 {
khaiminhvn 0:ab4465742afd 105 if(cur >= (ref-ANGLE_TOL) && cur <= (ref+ANGLE_TOL))
khaiminhvn 0:ab4465742afd 106 return 1;
khaiminhvn 0:ab4465742afd 107 return 0;
khaiminhvn 0:ab4465742afd 108 }