example_acc.cpp

#include "MyRIO.h"
#include <math.h>

using namespace myRIO;
using namespace std;

#define ACC_G 1.042968750

// https://www.arduino.cc/reference/en/language/functions/math/map/
double map(double x, double in_min, double in_max, double out_min, double out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

int main() {
    if(!myRIO_init()) {cout << "Error initializing myRIO"; return -1;}

    Acc acc;
    double xAcc, yAcc, zAcc;
    double lastxAcc = 0, lastyAcc = 0, lastzAcc = 0;

    while(1) {
        acc.all(xAcc, yAcc, zAcc);

        xAcc = asin(xAcc/ACC_G)*180./3.1415926535898;
        if(xAcc!=xAcc) xAcc = lastxAcc; // if acc is NaN, keep the old value
        // mapping accelerometer values to make them linear (no values between 78 - 103)
        if(xAcc<90) xAcc = map(xAcc, 0, 78.894271, 0, 90);
        else xAcc = map(xAcc, 103.148749, 180, 90, 180);
        lastxAcc = xAcc;

        yAcc = asin(yAcc/ACC_G)*180./3.1415926535898;
        if(yAcc!=yAcc) yAcc = lastyAcc;
        if(yAcc<90) yAcc = map(yAcc, 0, 78.894271, 0, 90);
        else yAcc = map(yAcc, 103.148749, 180, 90, 180);
        lastyAcc = yAcc;

        zAcc = asin(zAcc/ACC_G)*180./3.1415926535898;
        if(zAcc!=zAcc) zAcc = lastzAcc;
        if(zAcc<90) zAcc = map(zAcc, 0, 78.894271, 0, 90);
        else zAcc = map(zAcc, 103.148749, 180, 90, 180);
        lastzAcc = zAcc;

        printf("x = %f y = %f z = %f\n", xAcc, yAcc, zAcc);

        Time::wait_ms(100);
    }

    return 0;
}