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@47:46db7f076cea, 2015-09-25 (annotated)

Committer:
DeguNaoto
Date:
Fri Sep 25 01:25:22 2015 +0000
Revision:
47:46db7f076cea
Parent:
45:e11ec4f6d37e
Child:
48:64d005c70df2
????????;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:bd4719e15f7e 1 /**
DeguNaoto 0:bd4719e15f7e 2 * This program is written in main micro computer "mbed" for 2015 NHK Robot Contest (Bteam).
DeguNaoto 0:bd4719e15f7e 3 */
DeguNaoto 2:cf8ca6742db9 4
DeguNaoto 34:f9ef622f4376 5 //速度コントローラと向きコントローラはそのまま
DeguNaoto 34:f9ef622f4376 6
DeguNaoto 47:46db7f076cea 7 #define BLUE
DeguNaoto 47:46db7f076cea 8 //#define RED
DeguNaoto 43:f9a75ecbe44e 9
DeguNaoto 45:e11ec4f6d37e 10 #if defined(BLUE) && defined(RED)
DeguNaoto 45:e11ec4f6d37e 11 #error Caution, You should define either BLUE or RED
DeguNaoto 45:e11ec4f6d37e 12 #endif
DeguNaoto 45:e11ec4f6d37e 13
DeguNaoto 0:bd4719e15f7e 14 #include "machine_ps3.h"
DeguNaoto 0:bd4719e15f7e 15
DeguNaoto 0:bd4719e15f7e 16 Serial pc(USBTX, USBRX);
DeguNaoto 0:bd4719e15f7e 17
DeguNaoto 2:cf8ca6742db9 18 int main()
DeguNaoto 2:cf8ca6742db9 19 {
DeguNaoto 0:bd4719e15f7e 20 initializeSBDBT();
DeguNaoto 0:bd4719e15f7e 21 initializeMotors();
DeguNaoto 0:bd4719e15f7e 22 initializeControllers();
DeguNaoto 36:fd04196b4b34 23 initializeEsaka();
DeguNaoto 43:f9a75ecbe44e 24 #ifdef BLUE
DeguNaoto 34:f9ef622f4376 25 sita=PI/4.0,targ_sita=PI/4.0;
DeguNaoto 43:f9a75ecbe44e 26 #else
DeguNaoto 43:f9a75ecbe44e 27 sita=-PI/4.0,targ_sita=-PI/4.0;
DeguNaoto 43:f9a75ecbe44e 28 #endif
DeguNaoto 21:79b94cb922f0 29 Indicator4=1;
DeguNaoto 26:8e6c736b6791 30 Enable=1;
DeguNaoto 47:46db7f076cea 31 /*sendData(4,31);
DeguNaoto 47:46db7f076cea 32 wait(0.1);
DeguNaoto 47:46db7f076cea 33 sendData(5,31);*/
DeguNaoto 0:bd4719e15f7e 34 while(1) {
DeguNaoto 21:79b94cb922f0 35 if(autoflag){
DeguNaoto 22:3996c3f41922 36 autoPs3();
DeguNaoto 43:f9a75ecbe44e 37 #ifdef BLUE
DeguNaoto 43:f9a75ecbe44e 38 //Blue
DeguNaoto 36:fd04196b4b34 39 if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.0,step=1;
DeguNaoto 36:fd04196b4b34 40 if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2;
DeguNaoto 38:0b8ec1fd64bf 41 if((step==2)&&(x<2000.0)) targ_sita=PI/4,step=3;
DeguNaoto 40:5d4e6d0e2a99 42 if((step==3)&&(x<10.0)) targ_velocity=0.0,step=4;
DeguNaoto 35:2db63dec2a67 43
DeguNaoto 47:46db7f076cea 44 if((x>3754.0)&&(CStep==1)) CStep=2,sendData(1,1);
DeguNaoto 36:fd04196b4b34 45 if((x>5824.0)&&(CStep==2)) CStep=3,sendData(1,2);
DeguNaoto 36:fd04196b4b34 46 if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,3);
DeguNaoto 47:46db7f076cea 47 // if((x<6074.0)&&(CStep==4)) CStep=5,sendData(1,5);
DeguNaoto 47:46db7f076cea 48 // if((x<5574.0)&&(CStep==5)) CStep=6,sendData(1,4);
DeguNaoto 43:f9a75ecbe44e 49 #else
DeguNaoto 43:f9a75ecbe44e 50 //Red
DeguNaoto 43:f9a75ecbe44e 51 if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.0,step=1;
DeguNaoto 43:f9a75ecbe44e 52 if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2;
DeguNaoto 43:f9a75ecbe44e 53 if((step==2)&&(x<2000.0)) targ_sita=-PI/4,step=3;
DeguNaoto 43:f9a75ecbe44e 54 if((step==3)&&(x<10.0)) targ_velocity=0.0,step=4;
DeguNaoto 43:f9a75ecbe44e 55
DeguNaoto 43:f9a75ecbe44e 56 if((x>3804.0)&&(CStep==1)) CStep=2,sendData(1,1);
DeguNaoto 43:f9a75ecbe44e 57 if((x>5824.0)&&(CStep==2)) CStep=3,sendData(1,2);
DeguNaoto 43:f9a75ecbe44e 58 if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,3);
DeguNaoto 47:46db7f076cea 59 // if((x<6074.0)&&(CStep==4)) CStep=5,sendData(1,5);
DeguNaoto 47:46db7f076cea 60 // if((x<5574.0)&&(CStep==5)) CStep=6,sendData(1,4);
DeguNaoto 43:f9a75ecbe44e 61 pc.printf("sita:%f, x:%f, y:%f ,x1:%f, x2:%f ,velocity:%f\r\n",sita,x,y,x1,x2,velocity);
DeguNaoto 43:f9a75ecbe44e 62 #endif
DeguNaoto 21:79b94cb922f0 63 move_following();
DeguNaoto 36:fd04196b4b34 64 // pc.printf("sita:%f, x:%f, y:%f ,x1:%f, x2:%f ,velocity:%f\r\n",sita,x,y,x1,x2,velocity);
DeguNaoto 31:74e77ef0831c 65 // pc.printf("r:%ld, l:%ld\r\n",Move_r_sense.getPulses(),Move_l_sense.getPulses());
DeguNaoto 4:51d87d2b698c 66 }
DeguNaoto 21:79b94cb922f0 67 else if(!autoflag) {
DeguNaoto 23:5c56747bcc43 68 manualMovePs3();
DeguNaoto 29:03bb1ddbe456 69 manualPs3();
DeguNaoto 4:51d87d2b698c 70 }
DeguNaoto 23:5c56747bcc43 71 /***update state***/
DeguNaoto 21:79b94cb922f0 72 mesure_state();
DeguNaoto 0:bd4719e15f7e 73 wait(RATE);
DeguNaoto 0:bd4719e15f7e 74 }
DeguNaoto 0:bd4719e15f7e 75 }