File content as of revision 5:47cb37923f58:

#include "IdleCommand.h"

IdleCommand::IdleCommand(Sensors* sensors, GonioService* gonioService)
{
    this->sensors = sensors;
    this->gonioService = gonioService;
}

void IdleCommand::initialize()
{

    printf("IdleCommand\n");
    sensors->disableIMU();
    setTicker(0.5);
}

void IdleCommand::execute() {}

void IdleCommand::button()
{
    int16_t angle = (sensors->getAngle(0) << 8) + sensors->getAngle(1);

    //printf("%.2f,%d\n", ((float)angle) * 0.087912087, angle);

    sensors->updateAngle();
    sensors->updateIMU();

    int16_t accel[3] = {0,0,0};
    int16_t gyro[3] = {0,0,0};

    accel[0] = (sensors->getIMU(0)<<8)+sensors->getIMU(1);
    accel[1] = (sensors->getIMU(2)<<8)+sensors->getIMU(3);
    accel[2] = (sensors->getIMU(4)<<8)+sensors->getIMU(5);
    gyro[0]  = (sensors->getIMU(6)<<8)+sensors->getIMU(7);
    gyro[1]  = (sensors->getIMU(8)<<8)+sensors->getIMU(9);
    gyro[2]  = (sensors->getIMU(10)<<8)+sensors->getIMU(11);


    if(gonioService->isConnected()) {
        gonioService->updateGonio(angle,gyro[0],accel[0]);
    }
}

void IdleCommand::finish()
{
    detachTicker();
    setLed(0);
}