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-09-24
Revision:
43:f9a75ecbe44e
Parent:
42:4546283ed5d4
Child:
45:e11ec4f6d37e

File content as of revision 43:f9a75ecbe44e:

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

//速度コントローラと向きコントローラはそのまま

//#define BLUE
#define RED

#include "machine_ps3.h"

Serial pc(USBTX, USBRX);

int main()
{
    initializeSBDBT();
    initializeMotors();
    initializeControllers();
    initializeEsaka();
#ifdef BLUE
    sita=PI/4.0,targ_sita=PI/4.0;
#else
    sita=-PI/4.0,targ_sita=-PI/4.0;
#endif
    Indicator4=1;
    Enable=1;
    while(1) {
        if(autoflag){
            autoPs3();
#ifdef BLUE
            //Blue
            if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.0,step=1;
            if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2;
            if((step==2)&&(x<2000.0)) targ_sita=PI/4,step=3;
            if((step==3)&&(x<10.0)) targ_velocity=0.0,step=4;
            
            if((x>3804.0)&&(CStep==1)) CStep=2,sendData(1,1);
            if((x>5824.0)&&(CStep==2)) CStep=3,sendData(1,2);
            if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,3);
            if((x<6074.0)&&(CStep==4)) CStep=5,sendData(1,5);
            if((x<5574.0)&&(CStep==5)) CStep=6,sendData(1,4);
#else
            //Red
            if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.0,step=1;
            if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2;
            if((step==2)&&(x<2000.0)) targ_sita=-PI/4,step=3;
            if((step==3)&&(x<10.0)) targ_velocity=0.0,step=4;
            
            if((x>3804.0)&&(CStep==1)) CStep=2,sendData(1,1);
            if((x>5824.0)&&(CStep==2)) CStep=3,sendData(1,2);
            if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,3);
            if((x<6074.0)&&(CStep==4)) CStep=5,sendData(1,5);
            if((x<5574.0)&&(CStep==5)) CStep=6,sendData(1,4);
            pc.printf("sita:%f, x:%f, y:%f ,x1:%f, x2:%f ,velocity:%f\r\n",sita,x,y,x1,x2,velocity);
#endif            
            move_following();
//            pc.printf("sita:%f, x:%f, y:%f ,x1:%f, x2:%f ,velocity:%f\r\n",sita,x,y,x1,x2,velocity);
//            pc.printf("r:%ld, l:%ld\r\n",Move_r_sense.getPulses(),Move_l_sense.getPulses());
        }
        else if(!autoflag) {
            manualMovePs3();
            manualPs3();
        }
        /***update state***/
        mesure_state();
        wait(RATE);
    }
}