File content as of revision 8:8ff6db217a17:

#include "states.h"

States::States():
    anglePID(KP, KI, KD,0.001),
    pad(padTX,padRX),
    gyro(GYROTX,GYRORX),
    serial(MDTX,MDRX,MDBAUD),
    emStop(STOP),
    mechanism(&serial)
{
    anglePID.setMode(AUTO_MODE);
    anglePID.setInputLimits(-360, 360);
    anglePID.setOutputLimits(minLimit, maxLimit);
    anglePID.setSetPoint(0.0);
    anglePID.setBias(0.0);
    state = 400;
    emStop = false;
} 

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<12; i++){
        b[i] = pad.getButton(i);
        b3[i] = b2[i] - b[i];
        b2[i] = b[i]; 
    }
    for(int i=0; i<4; i++) stick[i] = pad.getStick(i);
    stick[0] -= 128;
    stick[1] = 128-stick[1];
    norm[0] = hypotf(stick[0],stick[1]);
    if(norm[0] < 50) norm[0] = 0;
    for(int i=0; i<2; i++) trigger[i] = pad.getTrigger(i);
    mechanism.setBrake(trigger[0]);

    mechanism.getTrigger(trigger[1]);
    if(b3[5] == 1) state++;
    if(b3[4] == 1) state--;
    /*
    *ロボの方角
    * "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()
{
    while(1){
        tellPad();
        tellAngle();
        if(b[10]) emStop = true;
        if(b[11]) emStop = false;
        if(!pad.status){
            /*mechanism.stop()は止める*/
            mechanism.stop();
            emStop = false;
        }else{
            turnPower = anglePID.compute();
            //if(norm[0] < 70) mechanism.move(0,0,-turnPower,nowAngle, 0);
            /*else*/ mechanism.move(stick[0], stick[1], -turnPower, nowAngle, norm[0]);
           /*昇降機構*/    
            if(b[0]) {mechanism.up();}
            else if(b[1]) {mechanism.down();}
            /*回収機構*/
            else if(b[6]) {mechanism.collect();}
            /*つり上げ機構*/
            else if(b[7]) {mechanism.lift();}
            /*装填機構*/
            else if(b[2]) {mechanism.loadRight();}
            else if(b[3]) {mechanism.loadLeft();}
            /*かけ機構*/
            else if(b[9]) {mechanism.hookRight();}
            else if(b[8]) {mechanism.hookLeft();}
            
            else mechanism.noAct();
        }
    }
}