File content as of revision 0:8d012d469eb4:

#include "states.h"

States::States():
    anglePID(KP, KI, KD,0.001),
    pad(padTX,padRX,ADDRESS),
    gyro(GYROTX,GYRORX),
    serial(MDTX,MDRX,MDBAUD),
    omni(&serial)
{
    anglePID.setMode(AUTO_MODE);
    anglePID.setInputLimits(-360, 360);
    anglePID.setOutputLimits(minLimit, maxLimit);
    anglePID.setSetPoint(0.0);
    anglePID.setBias(0.0);

    state = 400;
} 


void States::tellAngle(){
    gyro.update();
    nowAngle = gyro.getAngle();
    deviation = nowAngle - idealAngle;
    if(deviation > 180) deviation -= 360;
    if(deviation < -180) deviation += 360;

    anglePID.setProcessValue(deviation);
}

void States::tellPad(){
    for(int i=0; i<13; i++){
        b[i] = 1 - pad.getButton1(i);
        b3[i] = b2[i] - b[i];
        b2[i] = b[i];
    }
    stickRad = PI - pad.getRadian(0) + (nowAngle * (PI / 180));
    norm = pad.getNorm(0);
    X = norm * cos(stickRad);
    Y = norm * sin(stickRad);
    if(b3[0] == 1) state++;
    if(b3[1] == 1) state--;
    
    /*
    *move.tellState(方角)でロボの方角
    * "front" or 0 　で前
    * "right" or 90　で右
    * "back"  or 180 で後ろ
    * "left" or -90 で左
    */
    switch(state % 4){
        case 1: idealAngle = right; break;
        case 2: idealAngle = back; break;
        case 3: idealAngle = left; break;
        default: idealAngle = front; break;
        }
}

void States::move()
{
    if(pad.receiveState() == 0){
        /*omni.stop()は止める*/
        omni.stop();
    }else{
        tellAngle();
        tellPad();
//        tellState();
        turnPower = anglePID.compute();
        omni.move(X, Y, -turnPower);
    }
}