2015_robocon_bteam / Mbed 2 deprecated 2015robot_main

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   PID QEI mbed

Fork of 2015robot_main by Naoto Deguchi

main_ps3.cpp

Committer:
DeguNaoto
Date:
2015-10-27
Revision:
114:325e4c158141
Parent:
112:717acc7c99f9

File content as of revision 114:325e4c158141:

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

//回転機構wait PI/4 追加で待つ
//射出をスキップする機能を追加
//スタートゾーンにマシンを戻した後に暴走しないようにする
//手動時の操作性の改善
//一度手動モードになったらスタートゾーンに戻るまで再び自動モードにならないようにする
//flagfで前進・後退切り替え
//大和田射角
//午後に一定回転制御の実験
//モード切替時に時間を待つ
//赤チーム側を青チーム側のプログラムに合わせて実装
//横幅調整済み
//相手妨害モードいらない new!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//赤チームモード、相手チーム側の射出位置調整

short swingf=0;

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

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

/***回転機構測定***/
//#define MESURE

#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"

#define deff 70.0

Serial pc(USBTX, USBRX);
#ifdef MESURE
LocalFileSystem local("local");
#endif

Timeout MStop;
void Restart(){
    if(step==10) step=11;
}

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;
#ifdef MESURE
    FILE *fp_r = fopen("/local/velocity.dat", "w");
    double time=0.0;
#endif
    wait(0.3);
    sendData(7,0);
    while(1) {
        if(autoflag){
#ifdef IM920
            autoIM920(); /*IM920 button*/
#else
            autoPS3();   /*PS3 button*/
#endif
#ifdef BLUE
            //Blue            
            if(spcount<speed){
                spcount+=speed/100.0;
                targ_velocity=spcount;
            }
            /********************************Nomal Mode*********************************/
            if((step==0)&&((8650.0>x)&&(x>800.0))) {
                targ_sita=0.025;
                step=1;
            }
            if((step==1)&&(x>8600.0+deff)) {
                targ_velocity=0.0;
                step=2;
            }
            if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
                step=3;
                spcount=0.0;
                flagf=0;
                targ_sita=0.035;
            }
            if((step==3)&&(x<2000.0)) {
                targ_sita=PI/4;
                step=4;
            }
            if((step==4)&&(x<0.0)) {
                targ_velocity=0.0;
                step=114;
            }
            /***Cylinder***/
            if((x>3030.0+deff)&&(CStep==1)) { 
                if(!skip) sendData(1,1);
                CStep=2; 
            }
            if((x>5900.0+deff)&&(CStep==2)) {
                if(!skip) sendData(1,3);
                CStep=3;
            }
            if((x>7680.0+deff)&&(CStep==3)) {
                if(!skip) sendData(1,2);
                CStep=4;
            }
//            if((x<6650.0+deff)&&(CStep==4)) {
            if((x<6580.0+deff)&&(CStep==4)) {
                if(!skip) sendData(1,5);
                CStep=5;
            }
//            if((x<6170.0+deff)&&(CStep==5)) {
            if((x<6100.0+deff)&&(CStep==5)) {
                if(!skip) sendData(1,4);
                CStep=6;
            }
            if((x<4000.0)&&(CStep==6)){
                sendData(7,0);
                CStep=114;
            }
            /////////////////////////////////////////////////////////////////////////
            /******************************Middle Mode******************************/
            if((step==5)&&((5700.0>x)&&(x>800.0))) {
                targ_sita=0.025;
                step=6;
            }
            if((step==6)&&(x>7000.0)){
                targ_velocity=0.0;
                step=7;
            }
            if((step==7)&&((velocity<500.0)&&(velocity>-500.0))){
                flagf=0;
                spcount=0.0;
                targ_sita=0.035;
                step=8;
            }
            if((step==8)&&(x<1900.0)) {
                targ_sita=PI/4;
                step=9;
            }
            if((step==9)&&(x<-10.0)) {
                targ_velocity=0.0;
                step=114;
            }
            
            if((x>4620.0+deff)&&(CStep==7)){
                sendData(1,5);
                CStep=8;
            }
            if((x>5720.0+deff)&&(CStep==8)){
                sendData(1,4);
                CStep=9;
            }
            if((x<4000.0)&&(CStep==9)){
                sendData(7,0);
                CStep=114;
            }
            /////////////////////////////////////////////////////////////////////////
            /*****************************Opponents Mode****************************/
            if((step==15)&&((5700.0>x)&&(x>50.0))) {
                targ_sita=0.025;
                step=16;
            }
            if((step==16)&&(x>2850.0+deff)){
                targ_velocity=0.0;
                step=17;
            }
            if((step==17)&&((velocity<5000.0)&&(velocity>-5000.0))){
                flagf=0;
                spcount=0.0;
                targ_sita=0.035;
                step=18;
            }
            if((step==18)&&(x<-100.0)){
                targ_velocity=0.0;
                targ_sita=PI/4.0;
                step=114;
            }
            
//            if((x>1200.0+deff)&&(CStep==15)){
//            if((x>1000.0+deff)&&(CStep==15)){
            if((x>1050.0+deff)&&(CStep==15)){
                sendData(1,5);
                CStep=16;
            }
            if((x>2505.0+deff)&&(CStep==16)){
//            if((x>2455.0+deff)&&(CStep==16)){
//            if((x>2630.0)&&(CStep==16)){
                sendData(1,4);
                CStep=17;
            }
            if((x<2400.0+deff)&&(CStep==17)){
                sendData(7,0);
                CStep=114;
            }
            /////////////////////////////////////////////////////////////////////////
            /*****************************Disturb Mode****************************/
            if((step==50)&&((8650.0>x)&&(x>800.0))) {
                targ_sita=0.025;
                step=51;
            }
            if((step==51)&&(x>1500.0+deff)) {
                targ_velocity=0.0;
                velocity_controller.reset();
                direction_controller.reset();
                Move_l(0.0);
                Move_r(0.0);
                targ_sita=PI/2.0;
                step=52;
            }
            if((step==52)&&((sita<PI/2.0+0.5)&&(sita>PI/2.0-0.5))){
                step=53;
                autoflag=0;
                flaga=0;
            }
            /////////////////////////////////////////////////////////////////////////
#else       
            //Red
            if(spcount<speed){
                spcount+=speed/100.0;
                targ_velocity=spcount;
            }
            /********************************Nomal Mode*********************************/
            if((step==0)&&((8650.0>x)&&(x>800.0))) {
                targ_sita=-0.025;
                step=1;
            }
            if((step==1)&&(x>8600.0+deff)) {
                targ_velocity=0.0;
                step=2;
            }
            if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
                step=3;
                spcount=0.0;
                flagf=0;
                targ_sita=-0.035;
            }
            if((step==3)&&(x<2000.0)) {
                targ_sita=-PI/4;
                step=4;
            }
            if((step==4)&&(x<0.0)) {
                targ_velocity=0.0;
                step=114;
            }
            /***Cylinder***/
//            if((x>3030.0+deff)&&(CStep==1)) { 
            if((x>3100.0+deff)&&(CStep==1)) { 
                if(!skip) sendData(1,1);
                CStep=2; 
            }
//            if((x>5940.0+deff)&&(CStep==2)) {
            if((x>6000.0+deff)&&(CStep==2)) {
                if(!skip) sendData(1,2);
                CStep=3;
            }
            if((x>7680.0+deff)&&(CStep==3)) {
                if(!skip) sendData(1,3);
                CStep=4;
            }
            if((x<6730.0+deff)&&(CStep==4)) {
                if(!skip) sendData(1,4);
                CStep=5;
            }
            if((x<6170.0+deff)&&(CStep==5)) {
                if(!skip) sendData(1,5);
                CStep=6;
            }
            if((x<4000.0)&&(CStep==6)){
                sendData(7,0);
                CStep=114;
            }
            /////////////////////////////////////////////////////////////////////////
            /******************************Middle Mode******************************/
            if((step==5)&&((5700.0>x)&&(x>800.0))) {
                targ_sita=-0.025;
                step=6;
            }
            if((step==6)&&(x>7000.0)){
                targ_velocity=0.0;
                step=7;
            }
            if((step==7)&&((velocity<500.0)&&(velocity>-500.0))){
                flagf=0;
                spcount=0.0;
                targ_sita=-0.035;
                step=8;
            }
            if((step==8)&&(x<1820.0)) {
                targ_sita=-PI/4;
                step=9;
            }
            if((step==9)&&(x<-10.0)) {
                targ_velocity=0.0;
                step=114;
            }
            
            if((x>4690.0+deff)&&(CStep==7)){
                sendData(1,4);
                CStep=8;
            }
            if((x>5790.0+deff)&&(CStep==8)){
                sendData(1,5);
                CStep=9;
            }
            if((x<4000.0)&&(CStep==9)){
                sendData(7,0);
                CStep=114;
            }
            /////////////////////////////////////////////////////////////////////////
            /*****************************Opponents Mode****************************/
            if((step==15)&&((5700.0>x)&&(x>50.0))) {
//                targ_sita=-0.035;
                targ_sita=-0.025;
                step=16;
            }
//            if((step==16)&&(x>2800.0+deff)){
            if((step==16)&&(x>2850.0+deff)){
                targ_velocity=0.0;
                step=17;
            }
            if((step==17)&&((velocity<5000.0)&&(velocity>-5000.0))){
                flagf=0;
                spcount=0.0;
                targ_sita=-0.035;
                step=18;
            }
            if((step==18)&&(x<-100.0)){
                targ_velocity=0.0;
                targ_sita=-PI/4.0;
                step=114;
            }
            
//            if((x>1350.0+deff)&&(CStep==15)){
            if((x>1400.0+deff)&&(CStep==15)){
                sendData(1,4);
                CStep=16;
            }
            if((x>2700.0+deff)&&(CStep==16)){
//            if((x>2650.0+deff)&&(CStep==16)){
//            if((x>2850.0+deff)&&(CStep==16)){
                sendData(1,5);
                CStep=17;
            }
            if((x<2400.0+deff)&&(CStep==17)){
                sendData(7,0);
                CStep=114;
                
            }
            /////////////////////////////////////////////////////////////////////////
            /*****************************Disturb Mode****************************/
            if((step==50)&&((8650.0>x)&&(x>800.0))) {
                targ_sita=-0.025;
                step=51;
            }
            if((step==51)&&(x>1000.0+deff)) {
                targ_velocity=0.0;
                Move_l(0.0);
                Move_r(0.0);
                velocity_controller.reset();
                direction_controller.reset();
                targ_sita=-PI/2.0;
                step=52;
            }
            if((step==52)&&((sita<PI/2.0+0.5)&&(sita>PI/2.0-0.5))){
                step=53;
                autoflag=0;
                flaga=0;
            }
            /////////////////////////////////////////////////////////////////////////
#endif
            mesure_state();   /*位置測定*/
            move_following(); /*移動制御*/
        }
        else if(!autoflag) {
#ifdef IM920
            manualMoveIM920(); /*analogStick*/
            manualIM920();     /*IM920 button*/
#else
            manualMovePS3();   /*analogStick*/
            manualPS3();       /*PS3 button*/
#endif
            //Swing
            if(swingf) swingFollowing();
            flaga=0;
#ifdef MESURE
            if(down){
                IndicatorAuto=0;
                fclose(fp_r);
            }
            fprintf(fp_r, "time:%1.3f, %f[rad/s], pwm:%f\r\n",time,swingRadVelocity,cont);
            time+=0.01;
#endif
        }
        /***update state***/ 
#ifdef IM920
        readIM920();
#endif
        mesureSwing();
        wait(RATE);
//        pc.printf("%f\r\n",(float)((((2.0 * PI) / swingRadVelocity) / 4.0)));
//        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 %f\r\n",cont,swingRadVelocity, (double)SwingSens.getPulses());
    }
}