File content as of revision 75:4cdcd50ee666:

/**
 * This program is written in main micro computer "mbed" for 2015 NHK Robot Contest (Bteam).
 */

//速度コントローラと向きコントローラはそのまま
//大和田にPIC1のシリンダの信号をタイマーで書かせる
//回転機構wait PI/4 追加で待つ
//射出をスキップする機能を追加(サイクリックでCstepを変える)
//スタートゾーンにマシンを戻した後に暴走しないようにする
//手動時の操作性の改善
//一度手動モードになったらスタートゾーンに戻るまで再び自動モードにならないようにする

/***コース選択***/
//#define BLUE
#define RED

/***コントローラ選択***/
//#define IM920
#define PS3

#if defined(IM920) && defined(PS3)
#error Caution, You should define either IM920 or PS3
#endif

#if defined(BLUE) && defined(RED)
#error Caution, You should define either BLUE or RED
#endif

#include "machine_ps3.h"

Serial pc(USBTX, USBRX);
//LocalFileSystem local("local");

int main() {
#ifdef IM920
    initializeIM920();
#else
    initializeSBDBT();
#endif
    initializeMotors();
    initializeControllers();
    initializeSwing();
    initializeRS485();
#ifdef BLUE
    sita=PI/4.0,targ_sita=PI/4.0;
    IndicatorBLUE = 1;
#else
    sita=-PI/4.0,targ_sita=-PI/4.0;
    IndicatorRED = 1;
#endif
    Indicator4=1;
    Enable=1;
//    FILE *fp_r = fopen("/local/velocity.dat", "w");
    wait(0.3);
    sendData(7,31);
//    double time=0.0;
    while(1) {
        if(autoflag){
#ifdef IM920
            autoIM920(); /*IM920 button*/
#else
            autoPS3();   /*PS3 button*/
#endif
#ifdef BLUE
            //Blue            
            if((step==0)&&((8650.0>x)&&(x>900.0))) targ_sita=0.03,step=1; /*nomal*/
            if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2,flagf=0;
            if((step==2)&&(x<1700.0)) {
                targ_sita=PI/4;
                step=3;
                sendData(7,31);
            }
            if((step==3)&&(x<-100.0)) {
                targ_velocity=0.0;
                step=8;
                autoflag = 0;
            }
            
            if((step==4)&&((5700.0>x)&&(x>600.0))) targ_sita=0.03,step=5; /*middle*/
            if((step==5)&&(x>5000.0)) { /*middle shoot*/
                targ_velocity=-speed;
                step=6;
                sendData(1,5);
                flagf=0;
            }
            if((step==6)&&(x<1600.0)) {
                targ_sita=PI/4;
                step=7;
                sendData(7,31);
            }
            if((step==7)&&(x<-100.0)) {
                targ_velocity=0.0;
                step=8;
                autoflag = 0;
            }
            
            if((x>2900.0)&&(CStep==1)) { /*nomal*/
                if(!skip) sendData(1,1);
                CStep=2;
            }
            if((x>5850.0)&&(CStep==2)) {
                if(!skip) sendData(1,2);
                CStep=3;
            }
            if((x>7600.0)&&(CStep==3)) {
                if(!skip) sendData(1,3);
                CStep=4;
            }
            if((x<6600.0)&&(CStep==4)) {
                if(!skip) sendData(1,5);
                CStep=5;
            }
            if((x<6100.0)&&(CStep==5)) {
                if(!skip) sendData(1,4);
                CStep=6;
            }

#else       
            //Red
            if((step==0)&&((8650.0>x)&&(x>900.0))) targ_sita=-0.03,step=1;
            if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2,flagf=0;
            if((step==2)&&(x<1700.0)) {
                targ_sita=-PI/4;
                step=3;
                sendData(7,31);
            }
            if((step==3)&&(x<-100.0)) {
                targ_velocity=0.0;
                step=8;
                autoflag=0;
            }
            
            if((step==4)&&((5700.0>x)&&(x>600.0))) targ_sita=-0.03,step=5; /*middle*/
            if((step==5)&&(x>5000.0)) { /*middle shoot*/
                targ_velocity=-speed;
                step=6;
                sendData(1,4);
                flagf=0;
            }
            if((step==6)&&(x<2000.0)) {
                targ_sita=-PI/4;
                step=7;
                sendData(7,31);
            }
            if((step==7)&&(x<-100.0)) {
                targ_velocity=0.0;
                step=8;
                autoflag=0;
            }
            
            if((x>2900.0)&&(CStep==1)) {
                if(!skip) sendData(1,1);
                CStep=2;
            }
            if((x>5850.0)&&(CStep==2)) {
                if(!skip) sendData(1,3);
                CStep=3;
            }
            if((x>7600.0)&&(CStep==3)) {
                if(!skip) sendData(1,2);
                CStep=6;
            }
            if((x<6600.0)&&(CStep==4)) {
                if(!skip) sendData(1,4);
                CStep=5;
            }
            if((x<6100.0)&&(CStep==5)) {
                if(!skip) sendData(1,5);
                CStep=6;
            }
#endif
            mesure_state();   /*位置測定*/
            move_following(); /*位置移動*/
        }
        else if(!autoflag) {
            resetState();
#ifdef IM920
            manualMoveIM920(); /*analogStick*/
            manualIM920();     /*IM920 button*/
#else
            manualMovePS3();   /*analogStick*/
            manualPS3();       /*PS3 button*/
#endif
            //Swing
            swingFollowing();
            /*if(square){
                IndicatorAuto=0;
                fclose(fp_r);
            }
            fprintf(fp_r, "time:%1.3f, %f[rad/s], pwm:%f\r\n",time,swingRadVelocity,cont);
            time+=0.01;*/
        }
        /***update state***/ 
#ifdef IM920
        readIM920();
#endif
        mesureSwing();
        wait(RATE);

//        pc.printf("sita:%f, x:%f, y:%f ,x1:%f, x2:%f ,velocity:%f\r\n",sita,x,y,x1,x2,velocity);
//        pc.printf("A2 = %d, X = %d, Y = %d, B = %d, dead = %d\r\n", a2, X, Y, b, deadflag);
//        pc.printf("%f %f\r\n",cont,swingRadVelocity);
    }
}