File content as of revision 4:437778dbdf8c:

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

//Constructor
Accelerometer::Accelerometer() : Panel(PIN_ACCEL_PANEL),R1(PIN_ACCEL_R1),
    R2(PIN_ACCEL_R2),acc(PIN_SDA,PIN_SCL)
{    
    //Set Output Control
    Panel = 0; //Panel Array
    R1 = 1; //Reflector 2
    R2 = 1; //Reflector 1
    
    //Initialize I2C
    I2C i2c(D14,D15);
    i2c.frequency(ACC_FREQ);
    
    //Set Range
    acc.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
    
    //Calibration
    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];
            break;
        case S_R1: //Reflector 1
            Panel = 1;
            R1 = 0;
            R2 = 1;
            curOffset = AOffset[S_R1];
            break;
        case S_R2: //Reflector 2
            Panel = 1;
            R1 = 1;
            R2 = 0;
            curOffset = AOffset[S_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];
    }
    return a[(int)n/2];
}

//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];
    }
    return a[(int)N_AVG/2];
}


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