File content as of revision 7:3b5aa5bea044:

//INCLUDES
#include "Accelerometer.h"
#include "PinAssignment.h"

//Constructor
Accelerometer::Accelerometer(I2C *i2cIn) : Panel(PIN_ACCEL_PANEL),R1(PIN_ACCEL_R1),
    R2(PIN_ACCEL_R2),acc(i2cIn)
{    
    //Set Output Control
    Panel = 0; //Panel Array
    R1 = 1; //Reflector 2
    R2 = 1; //Reflector 1
    mul = 1;
    
    //Set Range
    acc.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
}

//calibrate
void Accelerometer::calibrate(){
    AOffset[S_PANEL] = getAngleCal(S_PANEL,N_CAL);
    AOffset[S_R1] = getAngleCal(S_R1,N_CAL);
    AOffset[S_R2] = getAngleCal(S_R2,N_CAL);
}

//setSource
void Accelerometer::setSource(int n)
{
    switch(n)
    {
        case S_PANEL: //Panel Array
            Panel = 0;
            R1 = 1;
            R2 = 1;
            curOffset = AOffset[S_PANEL];
            mul = MUL_P;
            break;
        case S_R1: //Reflector 1
            Panel = 1;
            R1 = 0;
            R2 = 1;
            curOffset = AOffset[S_R1];
            mul = MUL_R1;
            break;
        case S_R2: //Reflector 2
            Panel = 1;
            R1 = 1;
            R2 = 0;
            curOffset = AOffset[S_R2];
            mul = MUL_R2;
            break;
    }
}

//checkConnection
bool Accelerometer::checkConnection()
{
    return acc.testConnection();
}

//getAngleCal
float Accelerometer::getAngleCal(int s, int n)
{
    float ang[3];
    float ang_out = 0;
    float a[n];
    Accelerometer::setSource(s);
    
    for(int i = 0;i < n;i++)
    {
        acc.getAcceleroAngle(ang);
        a[i] = ang[MEAS_AXIS];
        wait_us(100);
    }
    
    sort(a,a+n);
    
    return a[(int)n/2]*mul;
}

//getAngle
float Accelerometer::getAngle(int s)
{
    float ang[3];
    float ang_out = 0;
    float a[N_AVG];
    Accelerometer::setSource(s);
    
    for(int i = 0;i < N_AVG;i++)
    {
        acc.getAcceleroAngle(ang);
        a[i] = ang[MEAS_AXIS];
        wait_us(100);
    }
    
    sort(a,a+N_AVG);
    
    return a[(int)N_AVG/2]*mul-curOffset;
}


//checkAngle
bool Accelerometer::checkAngle(float ref, float cur)
{    
    if(cur >= (ref-ANGLE_TOL) && cur <= (ref+ANGLE_TOL))
        return 1;
    return 0;
}