File content as of revision 42:c420857a3a6f:

#ifndef MANUALMODE_H
#define MANUALMODE_H

void manualIM920()
{
    if(modeflag){  //mode change
        autoflag=1;
        Indicator4=1;
        IndicatorAuto=0;
        resetState(1);
        flaga=0;
        step=114;
        CStep=114;
        mstep=114;
        mCStep=114;
        spcount=speed;
        dpcount=0.0;
        swingmoved=0;
    }
    if(b==9){  //swing start&stop
        if(edge9){
            edge9=0;
            toggle9=!toggle9;
            if(toggle9){
//              swingspeed=15.0;
//                swingspeed=20.0;
//                swingspeed=5.0;
                resetSwingSpeed = 1;
                interruptSwingSpeed.attach(&countSwingSpeed, 0.05);
                enableSwing=1;
                /*for(float i=0.0;i<1.0;i+=0.01){
                    Motor_swing=i;
                    wait(0.01);
                }
                Motor_swing = 1.0;*/
            }
            else{
                interruptSwingSpeed.detach();
                targSwingRadVelocity = 0.0;
                enableSwing=0;
                contSwing.reset();
                Motor_swing = 0.0;
            }
        }
    }
    else if(b==10){ //shoot
        if(edge10){
            edge10=0;
            enableShoot=!enableShoot;
            if(enableShoot) sendData(1,11);
            else sendData(1,12);
        }
    }
    else if((b==5)&&(!flaga)){ //Swing Start(middle)
        if(edge5){
            edge5=0;
            resetState(0);
            flagf=0;
            spcount=0.0;
            dpcount=0.0;
            direction_controller.setBias(0.0);
            sendData(5,60); //right
            wait(0.05);
            sendData(4,60); //left
            wait(0.05);
            sendData(6,30); //middle
            stateR = 90;
            stateL = 90;
            mstep   = 0;
            mCStep  = 114;
            flaga  = 1;
#ifdef BLUE
            swingspeed = 16.0;
#else
            swingspeed = 16.0;
#endif
        }
    }
#ifdef BLUE
    else if((b==6)&&(!flaga)){ //Swing Start(over)
        if(edge6){
            edge6=0;
#else
    else if((b==7)&&(!flaga)){ //Swing Start(over)
        if(edge7){
            edge7=0;
#endif
            resetState(0);
            flagf=0;
            spcount=0.0;
            dpcount=0.0;
            direction_controller.setBias(0.0);
            sendData(5,60); //right
            wait(0.05);
            sendData(4,60); //left
            wait(0.05);
            sendData(6,30); //middle
            stateR = 90;
            stateL = 90;
            mstep   = 10;
            mCStep  = 114;
            flaga  = 1;
#ifdef BLUE
//            swingspeed = 18.015;
            swingspeed = 18.02;
#else
            swingspeed = 18.02;
#endif
        }
    }
#ifdef BLUE
    else if((b==7)&&(!flaga)){ //Swing Start(front)
        if(edge7){
            edge7=0;
#else
    else if((b==6)&&(!flaga)){ //Swing Start(front)
        if(edge6){
            edge6=0;
#endif
            resetState(0);
            flagf=0;
            spcount=0.0;
            dpcount=0.0;
            direction_controller.setBias(0.0);
            sendData(5,60); //right
            wait(0.05);
            sendData(4,60); //left
            wait(0.05);
            sendData(6,30); //middle
            stateR = 90;
            stateL = 90;
            mstep   = 20;
            mCStep  = 114;
            flaga  = 1;
#ifdef BLUE
            swingspeed = 14.5; //入った
#else
            swingspeed = 14.5;
#endif
        }
    }
    else if((b==8)&&(swingmoved!=0)){
        if(edge8){
            edge8=0;
            if(swingmoved==1){
                mstep=2;
                swingmoved=0;
            }
            else if(swingmoved==2){
                mstep=12;
                swingmoved=0;
            }
            else if(swingmoved==3){
                mstep=22;
                swingmoved=0;
            }
        }
    }
    //Move
    if(b==2){
        targ_velocity=speed;
#ifdef BLUE
        flagf=1;
#else
        flagf=0;
#endif
        move=1;
    }
    else if(b==3){
        targ_velocity=speed;
#ifdef BLUE
        flagf=0;
#else
        flagf=1;
#endif
        move=1;
    }
    else if(move){
        move=0;
        targ_velocity=0.0;
    }
    if(b==1){
        if(edge1){
            edge1=0;
            if(swingmoved!=0){
#ifdef BLUE
                targ_sita-=PI/60.0;
#else 
                targ_sita+=PI/60.0;
#endif                
            }
            else{
#ifdef BLUE
//                targ_sita-=PI/4.0;
                targ_sita-=PI/16.0;
#else 
//                targ_sita+=PI/4.0;
                targ_sita+=PI/16.0;
#endif
            }
        }
    }
    else if(b==4){
        if(edge4){
            edge4=0;
            if(swingmoved!=0){
#ifdef BLUE
                targ_sita+=PI/60.0;
#else 
                targ_sita-=PI/60.0;
#endif                
            }
            else{
#ifdef BLUE
//                targ_sita+=PI/4.0;
                targ_sita+=PI/16.0;
#else 
//                targ_sita-=PI/4.0;
                targ_sita-=PI/16.0;
#endif
            }
        }
    }
    if(b!=1) edge1=1;
    if(b!=2) edge2=1;
    if(b!=3) edge3=1;
    if(b!=4) edge4=1;
    if(b!=5) edge5=1;
    if(b!=6) edge6=1;
    if(b!=7) edge7=1;
    if(b!=8) edge8=1;
    if(b!=9) edge9=1;
    if(b!=10) edge10=1;
    if(b!=11) edge11=1;
}

#endif /*manualMode.h*/