File content as of revision 24:6d2573d6f2b6:

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

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

/***マシン状態計測***/
//#define MESURE

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

#ifdef MESURE
#include "mbed.h"
short mesureflag=0;
LocalFileSystem local("local")
short testflag=0;
FILE *fp_r = fopen("/local/velocity.dat", "w");
#endif
/*#include "mbed.h"
LocalFileSystem local("local");
FILE *fp_r = fopen("/local/velocity.dat", "w");*/

#include "machine_ps3.h"

Serial pc(USBTX, USBRX);

//LocalFileSystem local("local");

int main() {
//    FILE *fp_r = fopen("/local/velocity.dat", "w");
    Com.attach(&Call,RATE);
    initializeMotors();
    initializeControllers();
    initializeRS485();
    initializeMbedSerial();
    initializeSwing();
#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;
    wait(0.3);
    sendData(7,0);
    while(1) {
        if(autoflag){
#ifdef BLUE
            autoIM920(); /*IM920 button*/
            /********************************Nomal Mode*********************************/
            if((step==0)&&((8600.0>x)&&(x>800.0))) {
                targ_sita=-0.04;
//                targ_sita=0.0;
                step=1;
            }
//            if((step==1)&&(x>8600.0+deff)) {
            if((step==1)&&(x>8600.0)) {
                targ_velocity=0.0;
                direction_controller.setBias(0.0);
                direction_controller.reset();
                velocity_controller.setBias(0.0);
                velocity_controller.reset();
                step=2;
            }
            if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
                step=3;
                spcount=0.0;
                flagf=0;
//                targ_sita=0.0;
                targ_sita=-0.04;
            }
            if((step==3)&&(x<1400.0)) {
                targ_sita=PI/4;
                step=4;
            }
            if((step==4)&&(x<800.0)) {
                dpcount=speed;
                step=114;
            }
            /*if((step==4)&&(x<10.0)) {
                targ_velocity=0.0;
                step=114;
            }*/
            /***Cylinder***/
            if((x>3300.0)&&(CStep==1)) { 
                if(!skip) sendData(1,1);
                CStep=2; 
            }
            if((x>6100.0)&&(CStep==2)) {
                if(!skip) sendData(1,3);
                CStep=3;
            }
            if((x>7880.0)&&(CStep==3)) {
                if(!skip) sendData(1,2);
                CStep=4;
            }
            if((x<6880.0)&&(CStep==4)) {
                if(!skip) sendData(1,5);
                CStep=5;
            }
            if((x<6800.0)&&(CStep==5)) {
                if(!skip) sendData(1,6);
                CStep=6;
            }
            if((x<6300.0)&&(CStep==6)) {
                if(!skip) sendData(1,4);
                CStep=7;
            }
            if((x<4000.0)&&(CStep==7)){
                sendData(7,0);
                CStep=114;
            }
#else
#endif
        }
        else if(!autoflag) {
            flaga=0;
            manualMoveIM920(); /*analogStick*/
            manualIM920();     /*IM920 button*/
//            mesureSwing();
//            swingFollowing();
//            wait(RATE);
//            fprintf(fp_r, "%f\r\n",swingRadVelocity);
//            if(b==11) fclose(fp_r);
            pc.printf("Swing:%f\r\n",swingRadVelocity);
        }
        /***update state***/ 
//        pc.printf("Swing:%f\r\n",SwingSens.getPulses());
//        pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
    }
}