File content as of revision 5:3273db2411d8:

#include "states.h"

States::States():
    anglePID(KP, KI, KD,0.001),
    pad(padTX,padRX),
    gyro(GYROTX,GYRORX),
    serial(MDTX,MDRX,MDBAUD),
    omni(&serial),
    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;
    check = 0;
    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<13; i++){
        b[i] = pad.getButton(i);
        b3[i] = b2[i] - b[i];
        b2[i] = b[i];
        check += b[i]; 
    }
    for(int i=0; i<4; i++) stick[i] = pad.getStick(i);
    stick[1] = 255-stick[1];
    stick[3] = 255-stick[3];
    norm[0] = hypotf(stick[0],stick[1]);
    norm[1] = hypotf(stick[2],stick[3]);
    if(norm[0] > 20) norm[0] = 0;
    if(norm[1] > 20) norm[1] = 0;
    for(int i=0; i<2; i++) trigger[i] = pad.getTrigger(i);
    omni.setBrake(trigger[0]);
    nano.sendTrigger(trigger[0]);
    mechanism.getTrigger(trigger[1]);
    rad[0] = atan2f(stick[1],stick[0]) + (nowAngle * (PI / 180));
    rad[1] = atan2f(stick[2],stick[3]);
    if(b3[4] == 1) state++;
    if(b3[5] == 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()
{
    tellPad();
    if(b[10]) emStop = true;
    if(b[11]) emStop = false;
    nano.sendHeader();
    if(emStop == false)nano.sendMove(0xff);
    else if(!pad.status){
        /*omni.stop()は止める*/
        omni.stop();
        emStop = false;
    }else{
        tellAngle();
        turnPower = anglePID.compute();
        if(norm[0] && norm[0] < 200) omni.stop();
        else omni.move(stick[0], stick[1], -turnPower);
       /*昇降機構*/    
        if(b[0] && check == 1) mechanism.up();
        else if(b[1] && check == 1) mechanism.down();
        /*回収機構*/
        else if(b[6] && check == 1) mechanism.collect();
        /*つり上げ機構*/
        else if(b[7] && check == 1) mechanism.lift();
        /*装填機構*/
        else if(b[2] && check == 1) mechanism.loadRight();
        else if(b[3]) mechanism.loadLeft();
        /*かけ機構*/
        else if(b[9] && check == 1) mechanism.hookRight();
        else if(b[8] && check == 1) mechanism.hookLeft();
        
        else nano.sendMove(0x0f);
    }
}