File content as of revision 4:f81029197ab2:

#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(){
    printf("****   BUTTON: IdleCommand   *****\n");
       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);
    }