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@54:9b209c5f25bc, 2015-09-28 (annotated)

Committer:
DeguNaoto
Date:
Mon Sep 28 00:32:18 2015 +0000
Revision:
54:9b209c5f25bc
Parent:
50:8ea4714316ce 
????????????????????????

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 48:64d005c70df2 7 //#define BLUE
DeguNaoto 48:64d005c70df2 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 50:8ea4714316ce 17 //LocalFileSystem local("local");
DeguNaoto 0:bd4719e15f7e 18
DeguNaoto 2:cf8ca6742db9 19 int main()
DeguNaoto 2:cf8ca6742db9 20 {
DeguNaoto 0:bd4719e15f7e 21 initializeSBDBT();
DeguNaoto 0:bd4719e15f7e 22 initializeMotors();
DeguNaoto 0:bd4719e15f7e 23 initializeControllers();
DeguNaoto 48:64d005c70df2 24 initializeSwing();
DeguNaoto 43:f9a75ecbe44e 25 #ifdef BLUE
DeguNaoto 34:f9ef622f4376 26 sita=PI/4.0,targ_sita=PI/4.0;
DeguNaoto 54:9b209c5f25bc 27 IndicatorBLUE = 1;
DeguNaoto 43:f9a75ecbe44e 28 #else
DeguNaoto 43:f9a75ecbe44e 29 sita=-PI/4.0,targ_sita=-PI/4.0;
DeguNaoto 54:9b209c5f25bc 30 IndicatorRED = 1;
DeguNaoto 43:f9a75ecbe44e 31 #endif
DeguNaoto 21:79b94cb922f0 32 Indicator4=1;
DeguNaoto 26:8e6c736b6791 33 Enable=1;
DeguNaoto 50:8ea4714316ce 34 // FILE *fp_r = fopen("/local/velocity.dat", "w");
DeguNaoto 47:46db7f076cea 35 /*sendData(4,31);
DeguNaoto 47:46db7f076cea 36 wait(0.1);
DeguNaoto 47:46db7f076cea 37 sendData(5,31);*/
DeguNaoto 50:8ea4714316ce 38 // double time=0.0;
DeguNaoto 0:bd4719e15f7e 39 while(1) {
DeguNaoto 21:79b94cb922f0 40 if(autoflag){
DeguNaoto 49:9276fda93084 41 autoPs3(); /*ps3 button*/
DeguNaoto 43:f9a75ecbe44e 42 #ifdef BLUE
DeguNaoto 43:f9a75ecbe44e 43 //Blue
DeguNaoto 36:fd04196b4b34 44 if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.0,step=1;
DeguNaoto 36:fd04196b4b34 45 if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2;
DeguNaoto 38:0b8ec1fd64bf 46 if((step==2)&&(x<2000.0)) targ_sita=PI/4,step=3;
DeguNaoto 40:5d4e6d0e2a99 47 if((step==3)&&(x<10.0)) targ_velocity=0.0,step=4;
DeguNaoto 35:2db63dec2a67 48
DeguNaoto 48:64d005c70df2 49 if((x>3344.0)&&(CStep==1)) CStep=2,sendData(1,1);
DeguNaoto 48:64d005c70df2 50 if((x>6234.0)&&(CStep==2)) CStep=3,sendData(1,2);
DeguNaoto 36:fd04196b4b34 51 if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,3);
DeguNaoto 54:9b209c5f25bc 52 // if((x<6700.0)&&(CStep==4)) CStep=5,sendData(1,5);
DeguNaoto 54:9b209c5f25bc 53 if((x<6750.0)&&(CStep==4)) CStep=5,sendData(1,5);
DeguNaoto 54:9b209c5f25bc 54 if((x<6300.0)&&(CStep==5)) CStep=6,sendData(1,4);
DeguNaoto 43:f9a75ecbe44e 55 #else
DeguNaoto 43:f9a75ecbe44e 56 //Red
DeguNaoto 43:f9a75ecbe44e 57 if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.0,step=1;
DeguNaoto 43:f9a75ecbe44e 58 if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2;
DeguNaoto 43:f9a75ecbe44e 59 if((step==2)&&(x<2000.0)) targ_sita=-PI/4,step=3;
DeguNaoto 43:f9a75ecbe44e 60 if((step==3)&&(x<10.0)) targ_velocity=0.0,step=4;
DeguNaoto 43:f9a75ecbe44e 61
DeguNaoto 48:64d005c70df2 62 if((x>3344.0)&&(CStep==1)) CStep=2,sendData(1,1);
DeguNaoto 48:64d005c70df2 63 if((x>6234.0)&&(CStep==2)) CStep=3,sendData(1,3);
DeguNaoto 48:64d005c70df2 64 if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,2);
DeguNaoto 54:9b209c5f25bc 65 // if((x<6700.0)&&(CStep==4)) CStep=5,sendData(1,5);
DeguNaoto 54:9b209c5f25bc 66 if((x<6750.0)&&(CStep==4)) CStep=5,sendData(1,4);
DeguNaoto 54:9b209c5f25bc 67 if((x<6300.0)&&(CStep==5)) CStep=6,sendData(1,5);
DeguNaoto 50:8ea4714316ce 68 // 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 69 #endif
DeguNaoto 21:79b94cb922f0 70 move_following();
DeguNaoto 54:9b209c5f25bc 71 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 72 // pc.printf("r:%ld, l:%ld\r\n",Move_r_sense.getPulses(),Move_l_sense.getPulses());
DeguNaoto 4:51d87d2b698c 73 }
DeguNaoto 21:79b94cb922f0 74 else if(!autoflag) {
DeguNaoto 48:64d005c70df2 75 manualMovePs3(); /*analogStick*/
DeguNaoto 49:9276fda93084 76 manualPs3(); /*ps3 button*/
DeguNaoto 48:64d005c70df2 77 //Swing
DeguNaoto 48:64d005c70df2 78 swingFollowing();
DeguNaoto 50:8ea4714316ce 79 /*if(square){
DeguNaoto 50:8ea4714316ce 80 IndicatorAuto=0;
DeguNaoto 50:8ea4714316ce 81 fclose(fp_r);
DeguNaoto 50:8ea4714316ce 82 }
DeguNaoto 50:8ea4714316ce 83 fprintf(fp_r, "time:%1.3f, %f[rad/s], pwm:%f\r\n",time,swingRadVelocity,cont);
DeguNaoto 50:8ea4714316ce 84 time+=0.01;*/
DeguNaoto 4:51d87d2b698c 85 }
DeguNaoto 23:5c56747bcc43 86 /***update state***/
DeguNaoto 21:79b94cb922f0 87 mesure_state();
DeguNaoto 48:64d005c70df2 88 mesureSwing();
DeguNaoto 0:bd4719e15f7e 89 wait(RATE);
DeguNaoto 50:8ea4714316ce 90
DeguNaoto 54:9b209c5f25bc 91 // pc.printf("%f %f\r\n",cont,swingRadVelocity);
DeguNaoto 0:bd4719e15f7e 92 }
DeguNaoto 0:bd4719e15f7e 93 }