File content as of revision 25:e72246ed7ec7:

#ifndef MANUALMODE_H
#define MANUALMODE_H

void Call(){
    mesure_state();
    if(autoflag){
        move_following();
        if(spcount<speed){
            spcount+=speed/100.0;
            targ_velocity=spcount;
        }
        if(dpcount>0.0){
            dpcount-=speed/100.0;
            targ_velocity=dpcount;
        }
    }
    #ifdef MESURE
//    if(mesureflag){
        fprintf(fp_r, "time:%1.3f ,x:%f ,y:%f ,sita:%f ,testflag:%d\r\n",time,x,y,sita,testflag);
//    }
    #endif
}

void mCall(){
    mesureSwing();
    if(enableSwing) swingFollowing();
}

void manualMoveIM920()
{
#ifdef BLUE
    if(Y<2) {
        Move_l(-0.6);
        Move_r(0.6);
    } 
    else if(Y>6) {
        Move_l(0.6);
        Move_r(-0.6);
    } 
    else {
        if(X<2) {
            Move_l(0.6);
            Move_r(0.6);
        } else if(X>6) {
            Move_l(-0.6);
            Move_r(-0.6);
        } else {
            Move_l(0.0);
            Move_r(0.0);
        }
    }
#else
    if(Y<2) {
        Move_l(0.6);
        Move_r(-0.6);
    }
    else if(Y>6) {
        Move_l(-0.6);
        Move_r(0.6);
    }
    else {
        if(X<2) {
            Move_l(-0.6);
            Move_r(-0.6);
        } else if(X>6) {
            Move_l(0.6);
            Move_r(0.6);
        } else {
            Move_l(0.0);
            Move_r(0.0);
        }
    }
#endif
}
void manualIM920()
{
    if(modeflag){  /*mode change*/
        Com.detach();
        autoflag=1;
        Indicator4=1;
        IndicatorAuto=0;
        Move_l(0.0);
        Move_r(0.0);
        direction_controller.setTunings(36.0,3.0,0.0);
        resetState();
        flaga=0;
        step=114;
        CStep=114;
        spcount=speed;
        Com.attach(&Call,RATE);
    }
    else if(b==5){
        if(edge5){
            edge5=0;
            resetSwingSpeed = 1;
            interruptSwingSpeed.attach(&countSwingSpeed, 0.05);
            enableSwing=1;
            /*for(float i=0.0;i<0.6;i+=0.01){
                Motor_swing=i;
                wait(0.01);
            }
            Motor_swing = 0.6;*/
        }
    }
    else if(b==9){
        interruptSwingSpeed.detach();
        targSwingRadVelocity = 0.0;
        enableSwing=0;
        contSwing.reset();
        sendData(2,8);
        Motor_swing = 0.0;
    }
    else if(b==6){
        if(edge6){
            edge6=0;
            sendData(1,1);
        }
    }
    else if(b==8){
        if(edge8){
            edge8=0;
            sendData(1,4);
        }
    }
    /*else if(b==5){
        if(edge5){
            edge5=0;
            sendData(1,4);
        }
    }
    else if(b==6){
        if(edge6){
            edge6=0;
            sendData(1,5);
        }
    }*/
    else if(b==1){  /*L down*/
        if(edge1){
            edge1=0;
            if(stateL!=1) stateL--;
            sendData(4,stateL);
        }
    }
    else if(b==2){  /*L up*/
        if(edge2){
            edge2=0;
            if(stateL!=92) stateL++;
            sendData(4,stateL);
        }
    }
    else if(b==3){  /*R down*/
        if(edge3){
            edge3=0;
            if(stateR!=1) stateR--;
            sendData(5,stateR);
        }
    }
    else if(b==4){  /*R up*/
        if(edge4){
            edge4=0;
            if(stateR!=92) stateR++;
            sendData(5,stateR);
        }
    }
    if(b!=7) edge7=1;
    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!=8) edge8=1;
    if(b!=9) edge9=1;
}

#endif /*manualMode.h*/