project_angle.cpp

#include "MyRIO.h"

using namespace myRIO;
using namespace std;

// 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;}

    // ACCELEROMETER
    Acc acc;
    double xAcc, lastxAcc = 180, zAcc;
    const double ACC_G = 1.042968750;
    // accelerometer filter
    double accFilterA = 0.6321, accFilterB = 0.3679;
    double accFiltered = 180, oldAccFiltered = 180, oldAcc = 180;

    // GYROSCOPE
    Gyro gyro;
    double angle = 180;
    // initializing and calibrating the gyro
    if(myRIO_error()) {cout << "Gyro - Error while initializing" << endl; return -1;}
    gyro.calibrate();
    if(myRIO_error()) {cout << "Gyro - Error while calibrating" << endl; return -1;}

    // COMPLEMENTARY FILTER
    const double GYRO_WEIGHT = 0.9;

    // WIFI
    Wifi w([&](short setpoint) {});
    while(!w.isConnected());

    // starting the angle thread
    gyro.startFreeRunningMode([&](double &vxRot, double &dt){
        system("clear");

        // reading accelerometer values
        acc.z(zAcc);
        acc.y(xAcc); // gyro.x orthogonal à acc.x -> acc.y est l'axe qui nous intéresse.
        xAcc = asin(xAcc/ACC_G)*180./3.1415926535898; // converting to degrees
        if(zAcc>=0) xAcc=180-xAcc; // tombe en avant : positif

        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);

        // filter accelerometer data
        accFiltered = accFilterA*oldAcc + accFilterB*oldAccFiltered;
        oldAccFiltered = accFiltered;
        oldAcc = xAcc;

        // complementary filter
        angle = GYRO_WEIGHT*(angle + vxRot * dt * 1e3) + (1.-GYRO_WEIGHT)*accFiltered;

        w.updateAngle((short)angle);
    });

    while(1);

    return 0;
}