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@3:de79cb7b645b, 2021-02-16 (annotated)

Committer:
khaiminhvn
Date:
Tue Feb 16 17:59:49 2021 +0000
Revision:
3:de79cb7b645b
Parent:
0:ab4465742afd 
Added compensation

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 0:ab4465742afd 6 Accelerometer::Accelerometer(int freq) : Panel(PIN_ACCEL_PANEL),R1(PIN_ACCEL_R1),
khaiminhvn 0:ab4465742afd 7 R2(PIN_ACCEL_R2),acc(PIN_SDA,PIN_SCL)
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 0:ab4465742afd 13
khaiminhvn 0:ab4465742afd 14 //Initialize I2C
khaiminhvn 0:ab4465742afd 15 I2C i2c(D14,D15);
khaiminhvn 0:ab4465742afd 16 i2c.frequency(freq);
khaiminhvn 0:ab4465742afd 17
khaiminhvn 0:ab4465742afd 18 //Set Range
khaiminhvn 3:de79cb7b645b 19 acc.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
khaiminhvn 3:de79cb7b645b 20
khaiminhvn 3:de79cb7b645b 21 //Calibration
khaiminhvn 3:de79cb7b645b 22 AOffset[S_PANEL] = getAngleCal(S_PANEL,N_CAL);
khaiminhvn 3:de79cb7b645b 23 AOffset[S_R1] = getAngleCal(S_R1,N_CAL);
khaiminhvn 3:de79cb7b645b 24 AOffset[S_R2] = getAngleCal(S_R2,N_CAL);
khaiminhvn 0:ab4465742afd 25 }
khaiminhvn 0:ab4465742afd 26
khaiminhvn 0:ab4465742afd 27 //setSource
khaiminhvn 0:ab4465742afd 28 void Accelerometer::setSource(int n)
khaiminhvn 0:ab4465742afd 29 {
khaiminhvn 0:ab4465742afd 30 switch(n)
khaiminhvn 0:ab4465742afd 31 {
khaiminhvn 0:ab4465742afd 32 case S_PANEL: //Panel Array
khaiminhvn 0:ab4465742afd 33 Panel = 0;
khaiminhvn 0:ab4465742afd 34 R1 = 1;
khaiminhvn 0:ab4465742afd 35 R2 = 1;
khaiminhvn 3:de79cb7b645b 36 curOffset = AOffset[S_PANEL];
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 3:de79cb7b645b 42 curOffset = AOffset[S_R1];
khaiminhvn 0:ab4465742afd 43 break;
khaiminhvn 0:ab4465742afd 44 case S_R2: //Reflector 2
khaiminhvn 0:ab4465742afd 45 Panel = 1;
khaiminhvn 0:ab4465742afd 46 R1 = 1;
khaiminhvn 0:ab4465742afd 47 R2 = 0;
khaiminhvn 3:de79cb7b645b 48 curOffset = AOffset[S_R2];
khaiminhvn 0:ab4465742afd 49 break;
khaiminhvn 0:ab4465742afd 50 }
khaiminhvn 0:ab4465742afd 51 }
khaiminhvn 0:ab4465742afd 52
khaiminhvn 0:ab4465742afd 53 //checkConnection
khaiminhvn 0:ab4465742afd 54 bool Accelerometer::checkConnection()
khaiminhvn 0:ab4465742afd 55 {
khaiminhvn 0:ab4465742afd 56 return acc.testConnection();
khaiminhvn 0:ab4465742afd 57 }
khaiminhvn 0:ab4465742afd 58
khaiminhvn 3:de79cb7b645b 59 //getAngleCal
khaiminhvn 3:de79cb7b645b 60 float Accelerometer::getAngleCal(int s, int n)
khaiminhvn 3:de79cb7b645b 61 {
khaiminhvn 3:de79cb7b645b 62 float ang[3];
khaiminhvn 3:de79cb7b645b 63 float ang_out = 0;
khaiminhvn 3:de79cb7b645b 64 float a[n];
khaiminhvn 3:de79cb7b645b 65 Accelerometer::setSource(s);
khaiminhvn 3:de79cb7b645b 66
khaiminhvn 3:de79cb7b645b 67 for(int i = 0;i < n;i++)
khaiminhvn 3:de79cb7b645b 68 {
khaiminhvn 3:de79cb7b645b 69 acc.getAcceleroAngle(ang);
khaiminhvn 3:de79cb7b645b 70 a[i] = ang[MEAS_AXIS];
khaiminhvn 3:de79cb7b645b 71 }
khaiminhvn 3:de79cb7b645b 72 return a[(int)n/2];
khaiminhvn 3:de79cb7b645b 73 }
khaiminhvn 3:de79cb7b645b 74
khaiminhvn 0:ab4465742afd 75 //getAngle
khaiminhvn 0:ab4465742afd 76 float Accelerometer::getAngle(int s)
khaiminhvn 0:ab4465742afd 77 {
khaiminhvn 0:ab4465742afd 78 float ang[3];
khaiminhvn 0:ab4465742afd 79 float ang_out = 0;
khaiminhvn 3:de79cb7b645b 80 float a[N_AVG];
khaiminhvn 0:ab4465742afd 81 Accelerometer::setSource(s);
khaiminhvn 0:ab4465742afd 82
khaiminhvn 0:ab4465742afd 83 for(int i = 0;i < N_AVG;i++)
khaiminhvn 0:ab4465742afd 84 {
khaiminhvn 0:ab4465742afd 85 acc.getAcceleroAngle(ang);
khaiminhvn 3:de79cb7b645b 86 a[i] = ang[MEAS_AXIS];
khaiminhvn 0:ab4465742afd 87 }
khaiminhvn 3:de79cb7b645b 88 return a[(int)N_AVG/2]-curOffset;
khaiminhvn 0:ab4465742afd 89 }
khaiminhvn 0:ab4465742afd 90
khaiminhvn 3:de79cb7b645b 91
khaiminhvn 0:ab4465742afd 92 //checkAngle
khaiminhvn 0:ab4465742afd 93 bool Accelerometer::checkAngle(float ref, float cur)
khaiminhvn 0:ab4465742afd 94 {
khaiminhvn 0:ab4465742afd 95 if(cur >= (ref-ANGLE_TOL) && cur <= (ref+ANGLE_TOL))
khaiminhvn 0:ab4465742afd 96 {
khaiminhvn 0:ab4465742afd 97 return 1;
khaiminhvn 0:ab4465742afd 98 }
khaiminhvn 0:ab4465742afd 99 return 0;
khaiminhvn 0:ab4465742afd 100 }