File content as of revision 55:ccf2ac8f6f32:

#include "controller.h"

Controller::Controller() :
    FEP(FEPTX, FEPRX, ADDR1),
    data(),
    fepTemp(0),
    button1(),
    button2(),
    stick()
    // radian(),
    // norm()
{
}

Controller::Controller(PinName FEPtx, PinName FEPrx, int addr) :
    FEP(FEPtx, FEPrx, addr),
    data(),
    fepTemp(0),
    button1(),
    button2(),
    stick()
    // radian(),
    // norm()
{
}

bool Controller::receiveState()
{
    fepTemp = FEP::read(data, DATA_SIZE);
    if(fepTemp == FEP_SUCCESS) {
        for(int i = 0; i < 7; i++) {
            button1[i] = data[4] % 2;
            data[4] /= 2;
        }
        for(int i=0; i<6; i++) {
            button2[i] = data[5] % 2;
            data[5] /= 2;
        }
        for(int i = 0; i < 4; i++) {
            stick[i] = -((double)(data[i] / STICK_DIVIDE) * 2.0 - 1.0);
            if(stick[i] > 1.0) stick[i] = 1.0;
            if(stick[i] < -1.0) stick[i] = -1.0;
        }
        setStick();
    } else {
        return 0;
    }
    return 1;
}

void Controller::setStick()
{
    for(int i = 0; i < 4; i++) {
        if(stick[i] < STICK_NEWTRAL && stick[i] > -STICK_NEWTRAL) stick[i] = 0;
    }

    // radian[0] = atan2(stick[1], stick[0]);
    // radian[1] = atan2(stick[3], -stick[2]);
    //
    // norm[0] = hypot(stick[0], stick[1]);
    // norm[1] = hypot(stick[2], stick[3]);
    //
    // if(norm[0] < STICK_NEWTRAL) norm[0] = 0;
    // if(norm[1] < STICK_NEWTRAL) norm[1] = 0;
    // if(norm[0] > STICK_NORM_MAX) norm[0] = STICK_NORM_MAX;
    // if(norm[1] > STICK_NORM_MAX) norm[1] = STICK_NORM_MAX;
}

bool Controller::getButton1(int number) const
{
    return button1[number];
}

bool Controller::getButton2(int number) const
{
    return button2[number];
}

float Controller::getStick(int number) const
{
    return stick[number];
}

// float Controller::getRadian(int number) const
// {
//     return radian[number];
// }
//
// float Controller::getNorm(int number) const
// {
//     return norm[number];
// }