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@36:fd04196b4b34, 2015-09-23 (annotated)

Committer:
DeguNaoto
Date:
Wed Sep 23 07:55:33 2015 +0000
Revision:
36:fd04196b4b34
Parent:
35:2db63dec2a67
Child:
37:7c24cc0e731b
20150923 ???

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 0:bd4719e15f7e 7 #include "machine_ps3.h"
DeguNaoto 0:bd4719e15f7e 8
DeguNaoto 0:bd4719e15f7e 9 Serial pc(USBTX, USBRX);
DeguNaoto 0:bd4719e15f7e 10
DeguNaoto 2:cf8ca6742db9 11 int main()
DeguNaoto 2:cf8ca6742db9 12 {
DeguNaoto 0:bd4719e15f7e 13 initializeSBDBT();
DeguNaoto 0:bd4719e15f7e 14 initializeMotors();
DeguNaoto 0:bd4719e15f7e 15 initializeControllers();
DeguNaoto 36:fd04196b4b34 16 initializeEsaka();
DeguNaoto 34:f9ef622f4376 17 sita=PI/4.0,targ_sita=PI/4.0;
DeguNaoto 34:f9ef622f4376 18 // sita=0.0,targ_sita=0.0;
DeguNaoto 30:2923c6879e5f 19 int CStep=1;
DeguNaoto 21:79b94cb922f0 20 Indicator4=1;
DeguNaoto 26:8e6c736b6791 21 Enable=1;
DeguNaoto 0:bd4719e15f7e 22 while(1) {
DeguNaoto 21:79b94cb922f0 23 if(autoflag){
DeguNaoto 22:3996c3f41922 24 autoPs3();
DeguNaoto 36:fd04196b4b34 25 if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.0,step=1;
DeguNaoto 36:fd04196b4b34 26 if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2;
DeguNaoto 35:2db63dec2a67 27 if((step==2)&&(x<7400.0)) targ_velocity=0.0,targ_sita=0.0,step=3;
DeguNaoto 35:2db63dec2a67 28
DeguNaoto 36:fd04196b4b34 29 if((x>3804.0)&&(CStep==1)) CStep=2,sendData(1,1);
DeguNaoto 36:fd04196b4b34 30 if((x>5824.0)&&(CStep==2)) CStep=3,sendData(1,2);
DeguNaoto 36:fd04196b4b34 31 if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,3);
DeguNaoto 36:fd04196b4b34 32 if((x<6074.0)&&(CStep==4)) CStep=5,sendData(1,5);
DeguNaoto 36:fd04196b4b34 33 if((x<5574.0)&&(CStep==5)) CStep=6,sendData(1,4);
DeguNaoto 34:f9ef622f4376 34 // if(x>1000.0) targ_velocity=0.0;
DeguNaoto 21:79b94cb922f0 35 move_following();
DeguNaoto 36:fd04196b4b34 36 // 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 37 // pc.printf("r:%ld, l:%ld\r\n",Move_r_sense.getPulses(),Move_l_sense.getPulses());
DeguNaoto 4:51d87d2b698c 38 }
DeguNaoto 21:79b94cb922f0 39 else if(!autoflag) {
DeguNaoto 23:5c56747bcc43 40 manualMovePs3();
DeguNaoto 29:03bb1ddbe456 41 manualPs3();
DeguNaoto 4:51d87d2b698c 42 }
DeguNaoto 23:5c56747bcc43 43 /***update state***/
DeguNaoto 21:79b94cb922f0 44 mesure_state();
DeguNaoto 0:bd4719e15f7e 45 wait(RATE);
DeguNaoto 36:fd04196b4b34 46
DeguNaoto 36:fd04196b4b34 47 pc.printf("velocityE:%f\r\n",h);
DeguNaoto 0:bd4719e15f7e 48 }
DeguNaoto 0:bd4719e15f7e 49 }