2015_robocon_bteam / Mbed 2 deprecated 2015robot_main_zenkoku

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

main_ps3.cpp

Committer:
DeguNaoto
Date:
2015-11-19
Revision:
43:a7423cfee3c8
Parent:
42:c420857a3a6f

File content as of revision 43:a7423cfee3c8:

/**
 * This program is written 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

#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(modeflag){
        if(autoflag){
            autoIM920(); /*IM920 button*/
#ifdef BLUE
            /********************************Own & Middle Mode*********************************/
//            if((step==0)&&((10000.0>x)&&(x>1000.0))) {
            if((step==0)&&((10000.0>x)&&(x>940.0))) {
                targ_sita=-0.03;
//                targ_sita=0.0;
                step=1;
            }
            if((step==1)&&(x>9000.0)) {
                targ_velocity=0.0;
                velocity_controller.setBias(0.0);
                velocity_controller.reset();
//                dpcount=speed;
                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.03;
            }
            if((step==3)&&(x<1800.0)) {
                targ_sita=PI/4;
                step=4;
            }
            if((step==4)&&(x<750.0)) {
                dpcount=speed;
                step=114;
            }
            
            //Cylinder
            if((x>3250.0)&&(CStep==0)) {
                if(!skip) sendData(1,1);
                CStep=1; 
            }
            if((x>6070.0)&&(CStep==1)) {
                if(!skip) sendData(1,2);
                CStep=2;
            }
            if((x>7720.0)&&(CStep==2)) {
                if(!skip) sendData(1,3);
                CStep=3;
            }
            if((x>8000.0)&&(CStep==3)) {
                if(!skip) sendData(1,8);
                CStep=4;
            }
            if((x>8500.0)&&(CStep==4)) CStep=5;
            /////////////////////////////////////////
            /*if((x<7200.0)&&(CStep==4)) {
                if(!skip) sendData(1,4); //over
                CStep=5;
            }
            if((x<6950.0)&&(CStep==5)) {
                if(!skip) sendData(1,6); //fornt
                CStep=6;
            }
            if((x<6700.0)&&(CStep==6)) {
                if(!skip) sendData(1,5); //middle
                CStep=7;
            }*/
            /////////////////////////////////////////
            if((x<7830.0)&&(CStep==5)) {
                if(!skip) sendData(1,6); //front
                CStep=6;
            }
            if((x<6710.0)&&(CStep==6)) {
                if(!skip) sendData(1,5); //middle
                CStep=7;
            }
            if((x<5500.0)&&(CStep==7)) {
                if(!skip) sendData(1,4); //over
                CStep=8;
            }
            /////////////////////////////////////////
            if((x<3000.0)&&(CStep==8)){
                sendData(7,0);
                CStep=114;
            }
            
            /********************************Own & Opponent Mode*********************************/
            if((step==10)&&((10000.0>x)&&(x>700.0))) {
                targ_sita=-0.03;
//                targ_sita=0.0;
                step=11;
            }
            if((step==11)&&(x>10000.0)) {
                targ_velocity=0.0;
                velocity_controller.setBias(0.0);
                velocity_controller.reset();
                step=12;
            }
            if((step==12)&&((velocity<500.0)&&(velocity>-500.0))){
                step=13;
                spcount=0.0;
                flagf=0;
//                flagf=3;
//                targ_sita=0.0;
                targ_sita=-0.03;
            }
            if((step==13)&&(x<1400.0)) {
                targ_sita=PI/4;
                step=14;
            }
            if((step==14)&&(x<700.0)) {
                dpcount=speed;
                step=114;
            }
            
            //Cylinderd
            if((x>3230.0)&&(CStep==10)) {
                if(!skip) sendData(1,1);
                CStep=11; 
            }
            if((x>6100.0)&&(CStep==11)) {
                if(!skip) sendData(1,2);
                CStep=12;
            }
            if((x>7750.0)&&(CStep==12)) {
                if(!skip) sendData(1,3);
                CStep=13;
            }
            if((x>9800.0)&&(CStep==13)) {
                sendData(1,8);
                CStep=14;
            }
            if((x>9900.0)&&(CStep==14)) CStep=15;
//            if((x<9500.0)&&(CStep==15)) { //7.8
            if((x<9600.0)&&(CStep==15)) { //7.8
                if(!skip) sendData(1,4);
                CStep=16;
            }
            if((x<7750.0)&&(CStep==16)) { //7.8
                if(!skip) sendData(1,5);
                CStep=17;
            }
            if((x<5500.0)&&(CStep==17)) { //7.8
//            if((x<5750.0)&&(CStep==17)) { //7.8
                if(!skip) sendData(1,6);
                CStep=18;
            }
            if((x<1000.0)&&(CStep==18)){
                sendData(7,0);
                CStep=114;
            }

#else
            /********************************Own & Middle Mode*********************************/
//            if((step==0)&&((10000.0>x)&&(x>1000.0))) {
            if((step==0)&&((10000.0>x)&&(x>850.0))) {
                targ_sita=0.02;
//                targ_sita=0.0;
                step=1;
            }
            if((step==1)&&(x>9000.0)) {
                targ_velocity=0.0;
                velocity_controller.setBias(0.0);
                velocity_controller.reset();
//                dpcount=speed;
                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.025;
            }
            if((step==3)&&(x<2000.0)) {
                targ_sita=-PI/4;
                step=4;
            }
            if((step==4)&&(x<800.0)) {
                dpcount=speed;
                step=114;
            }
            
            //Cylinder
            if((x>3250.0)&&(CStep==0)) {
                if(!skip) sendData(1,1);
                CStep=1; 
            }
            if((x>6100.0)&&(CStep==1)) {
                if(!skip) sendData(1,3);
                CStep=2;
            }
            if((x>7750.0)&&(CStep==2)) {
                if(!skip) sendData(1,2);
                CStep=3;
            }
            if((x>8000.0)&&(CStep==3)) {
                if(!skip) sendData(1,8);
                CStep=4;
            }
            if((x>8500.0)&&(CStep==4)) CStep=5;
            /////////////////////////////////////////
            /*if((x<7200.0)&&(CStep==4)) {
                if(!skip) sendData(1,4); //over
                CStep=5;
            }
            if((x<6950.0)&&(CStep==5)) {
                if(!skip) sendData(1,6); //fornt
                CStep=6;
            }
            if((x<6700.0)&&(CStep==6)) {
                if(!skip) sendData(1,5); //middle
                CStep=7;
            }*/
            /////////////////////////////////////////
            if((x<7970.0)&&(CStep==5)) {
                if(!skip) sendData(1,4); //front
                CStep=6;
            }
            if((x<6800.0)&&(CStep==6)) {
                if(!skip) sendData(1,5); //middle
                CStep=7;
            }
            if((x<5580.0)&&(CStep==7)) {
                if(!skip) sendData(1,6); //over
                CStep=8;
            }
            /////////////////////////////////////////
            if((x<3000.0)&&(CStep==8)){
                sendData(7,0);
                CStep=114;
            }
            
            /********************************Own & Opponent Mode*********************************/
            if((step==10)&&((10000.0>x)&&(x>630.0))) {
                targ_sita=0.025;
//                targ_sita=0.0;
                step=11;
            }
            if((step==11)&&(x>10000.0)) {
                targ_velocity=0.0;
                velocity_controller.setBias(0.0);
                velocity_controller.reset();
                step=12;
            }
            if((step==12)&&((velocity<500.0)&&(velocity>-500.0))){
                step=13;
                spcount=0.0;
                flagf=0;
//                flagf=3;
//                targ_sita=0.0;
                targ_sita=0.02;
            }
            if((step==13)&&(x<1450.0)) {
                targ_sita=-PI/4;
                step=14;
            }
            if((step==14)&&(x<700.0)) {
                dpcount=speed;
                step=114;
            }
            
            //Cylinderd
            if((x>3100.0)&&(CStep==10)) {
                if(!skip) sendData(1,1);
                CStep=11; 
            }
            if((x>6000.0)&&(CStep==11)) {
                if(!skip) sendData(1,3);
                CStep=12;
            }
            if((x>7650.0)&&(CStep==12)) {
                if(!skip) sendData(1,2);
                CStep=13;
            }
            if((x>9800.0)&&(CStep==13)) {
                sendData(1,8);
                CStep=14;
            }
            if((x>9900.0)&&(CStep==14)) CStep=15;
            if((x<9750.0)&&(CStep==15)) { //7.8
                if(!skip) sendData(1,6);
                CStep=16;
            }
            if((x<7900.0)&&(CStep==16)) { //7.8
//            if((x<7950.0)&&(CStep==16)) { //7.8
                if(!skip) sendData(1,5);
                CStep=17;
            }
            if((x<5670.0)&&(CStep==17)) { //7.8
//            if((x<5750.0)&&(CStep==17)) { //7.8
                if(!skip) sendData(1,4);
                CStep=18;
            }
            if((x<1000.0)&&(CStep==18)){
                sendData(7,0);
                CStep=114;
            }
#endif
        }
//        else if(!modeflag) {
        else if(!autoflag){
            manualIM920();     /*IM920 button*/
#ifdef BLUE
            /********************************Swing Mode(middle)*********************************/
            if((mstep==0)&&((10000.0>x)&&(x>1400.0))) {
                dpcount=speed;
                mstep=1;
            }
            if((mstep==1)&&((velocity<5.0)&&(velocity>-5.0))){
                targ_sita=PI/4.0+PI/30.0+PI/180.0;
                swingmoved=2;
                mstep=251;
            }
            if((mstep==251)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
                flagf=2;
            }
            if(mstep==2){
                mstep=3;
                targ_sita=3.0*PI/4.0-PI/8.0;
                sendData(7,0);
                flagf=0;
            }
            if((mstep==3)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
                flagf=1;
                targ_velocity=speed;
                mstep=4;
            }
            if((mstep==4)&&(x<1300.0)){
                targ_sita=5*PI/6.0;
                mstep=5;
            }
            if((mstep==5)&&(x<-10.0)){
                targ_velocity=0.0;
                targ_sita=3.0*PI/4.0;
                mstep=114;
            }
            
            
            /********************************Swing Mode(over)*********************************/
//            if((mstep==10)&&((10000.0>x)&&(x>1500.0))) {
            if((mstep==10)&&((10000.0>x)&&(x>1550.0))) {
                dpcount=speed;
                mstep=11;
            }
            if((mstep==11)&&((velocity<5.0)&&(velocity>-5.0))){
                targ_sita=PI/4.0+PI/16.0+PI/45.0+PI/90.0+PI/180.0;
                swingmoved=2;
                mstep=252;
            }
            if((mstep==252)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
                flagf=2;
            }
            if(mstep==12){
                mstep=13;
                targ_sita=3.0*PI/4.0-PI/8.0;
                sendData(7,0);
                flagf=0;
            }
            if((mstep==13)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
                flagf=1;
                targ_velocity=speed;
                mstep=14;
            }
            if((mstep==14)&&(x<1400.0)){
                targ_sita=5*PI/6.0;
                mstep=15;
            }
            if((mstep==15)&&(x<-10.0)){
                targ_velocity=0.0;
                targ_sita=3.0*PI/4.0;
                mstep=114;
            }
            
            /********************************Swing Mode(front)*********************************/
            if((mstep==20)&&((10000.0>x)&&(x>1550.0))) {
                dpcount=speed;
                mstep=21;
            }
            if((mstep==21)&&((velocity<5.0)&&(velocity>-5.0))){
                targ_sita=PI/4.0-PI/8.0+PI/60.0+PI/90.0;
                swingmoved=2;
                mstep=253;
            }
            if((mstep==252)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
                flagf=2;
            }
            if(mstep==22){
                mstep=23;
                targ_sita=3.0*PI/4.0-PI/8.0;
                sendData(7,0);
                flagf=0;
            }
            if((mstep==23)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
                flagf=1;
                targ_velocity=speed;
                mstep=24;
            }
            if((mstep==24)&&(x<1300.0)){
                targ_sita=5*PI/6.0;
                mstep=25;
            }
            if((mstep==25)&&(x<-10.0)){
                targ_velocity=0.0;
                targ_sita=3.0*PI/4.0;
                mstep=114;
            }
            
#else
            /********************************Swing Mode(middle)*********************************/
            if((mstep==0)&&((10000.0>x)&&(x>1550.0))) {
                dpcount=speed;
                mstep=1;
            }
            if((mstep==1)&&((velocity<5.0)&&(velocity>-5.0))){
                targ_sita=-PI/8.0-PI/30.0+PI/40.0;
                swingmoved=2;
                mstep=251;
            }
            if((mstep==251)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
                flagf=2;
            }
            if(mstep==2){
                mstep=3;
                targ_sita=-PI/18.0;
                sendData(7,0);
                flagf=0;
            }
            if((mstep==3)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
                flagf=1;
                targ_velocity=speed;
                mstep=4;
            }
            if((mstep==4)&&(x<1600.0)){
                targ_sita=-5.0*PI/6.0;
                mstep=5;
            }
            if((mstep==5)&&(x<-10.0)){
                targ_velocity=0.0;
                targ_sita=-3.0*PI/4.0;
                mstep=114;
            }
            
            
            /********************************Swing Mode(over)*********************************/
//            if((mstep==10)&&((10000.0>x)&&(x>1500.0))) {
            if((mstep==10)&&((10000.0>x)&&(x>1550.0))) {
                dpcount=speed;
                mstep=11;
            }
            if((mstep==11)&&((velocity<5.0)&&(velocity>-5.0))){
                targ_sita=-PI/3.0-PI/10.0+PI/4.0-PI/38.0;
                swingmoved=2;
                mstep=252;
            }
            if((mstep==252)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
                flagf=2;
            }
            if(mstep==12){
                mstep=13;
                targ_sita=-3.0*PI/4.0+PI/8.0;
                sendData(7,0);
                flagf=0;
            }
            if((mstep==13)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
                flagf=1;
                targ_velocity=speed;
                mstep=14;
            }
            if((mstep==14)&&(x<1600.0)){
                targ_sita=-5.0*PI/6.0;
                mstep=15;
            }
            if((mstep==15)&&(x<-10.0)){
                targ_velocity=0.0;
                targ_sita=-3.0*PI/4.0;
                mstep=114;
            }
            
            /********************************Swing Mode(front)*********************************/
            if((mstep==20)&&((10000.0>x)&&(x>1550.0))) {
                dpcount=speed;
                mstep=21;
            }
            if((mstep==21)&&((velocity<5.0)&&(velocity>-5.0))){
                targ_sita=0.0;
                swingmoved=2;
                mstep=253;
            }
            if((mstep==252)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
                flagf=2;
            }
            if(mstep==22){
                mstep=23;
                targ_sita=PI/6.0;
                sendData(7,0);
                flagf=0;
            }
            if((mstep==23)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
                flagf=1;
                targ_velocity=speed;
                mstep=24;
            }
            if((mstep==24)&&(x<1600.0)){
                targ_sita=-5.0*PI/6.0;
                mstep=25;
            }
            if((mstep==25)&&(x<-10.0)){
                targ_velocity=0.0;
                targ_sita=-3.0*PI/4.0;
                mstep=114;
            }
#endif
        }
//        pc.printf("b:%d\r\n",swingRadVelocity);
//        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);
    }
}