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@58:6b73e683fa70, 2015-09-30 (annotated)

Committer:
DeguNaoto
Date:
Wed Sep 30 12:31:48 2015 +0000
Revision:
58:6b73e683fa70
Parent:
57:3fbd487e055e 
????????????????

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 57:3fbd487e055e 7 /***コース選択***/
DeguNaoto 52:d9e1629da852 8 #define BLUE
DeguNaoto 52:d9e1629da852 9 //#define RED
DeguNaoto 43:f9a75ecbe44e 10
DeguNaoto 57:3fbd487e055e 11 /***コントローラ選択***/
DeguNaoto 57:3fbd487e055e 12 //#define IM920
DeguNaoto 57:3fbd487e055e 13 #define PS3
DeguNaoto 57:3fbd487e055e 14
DeguNaoto 57:3fbd487e055e 15 #if defined(IM920) && defined(PS3)
DeguNaoto 57:3fbd487e055e 16 #error Caution, You should define either IM920 or PS3
DeguNaoto 57:3fbd487e055e 17 #endif
DeguNaoto 57:3fbd487e055e 18
DeguNaoto 45:e11ec4f6d37e 19 #if defined(BLUE) && defined(RED)
DeguNaoto 45:e11ec4f6d37e 20 #error Caution, You should define either BLUE or RED
DeguNaoto 45:e11ec4f6d37e 21 #endif
DeguNaoto 45:e11ec4f6d37e 22
DeguNaoto 0:bd4719e15f7e 23 #include "machine_ps3.h"
DeguNaoto 0:bd4719e15f7e 24
DeguNaoto 0:bd4719e15f7e 25 Serial pc(USBTX, USBRX);
DeguNaoto 58:6b73e683fa70 26 LocalFileSystem local("local");
DeguNaoto 0:bd4719e15f7e 27
DeguNaoto 2:cf8ca6742db9 28 int main()
DeguNaoto 2:cf8ca6742db9 29 {
DeguNaoto 57:3fbd487e055e 30 #ifdef IM920
DeguNaoto 51:cb430192b28b 31 initializeIM920();
DeguNaoto 57:3fbd487e055e 32 #else
DeguNaoto 57:3fbd487e055e 33 initializeSBDBT();
DeguNaoto 57:3fbd487e055e 34 #endif
DeguNaoto 0:bd4719e15f7e 35 initializeMotors();
DeguNaoto 0:bd4719e15f7e 36 initializeControllers();
DeguNaoto 48:64d005c70df2 37 initializeSwing();
DeguNaoto 43:f9a75ecbe44e 38 #ifdef BLUE
DeguNaoto 34:f9ef622f4376 39 sita=PI/4.0,targ_sita=PI/4.0;
DeguNaoto 55:db1797ac6cb1 40 IndicatorBLUE = 1;
DeguNaoto 43:f9a75ecbe44e 41 #else
DeguNaoto 43:f9a75ecbe44e 42 sita=-PI/4.0,targ_sita=-PI/4.0;
DeguNaoto 55:db1797ac6cb1 43 IndicatorRED = 1;
DeguNaoto 43:f9a75ecbe44e 44 #endif
DeguNaoto 21:79b94cb922f0 45 Indicator4=1;
DeguNaoto 26:8e6c736b6791 46 Enable=1;
DeguNaoto 58:6b73e683fa70 47 FILE *fp_r = fopen("/local/velocity.dat", "w");
DeguNaoto 52:d9e1629da852 48 sendData(4,31);
DeguNaoto 47:46db7f076cea 49 wait(0.1);
DeguNaoto 52:d9e1629da852 50 sendData(5,31);
DeguNaoto 52:d9e1629da852 51 wait(0.1);
DeguNaoto 50:8ea4714316ce 52 // double time=0.0;
DeguNaoto 0:bd4719e15f7e 53 while(1) {
DeguNaoto 21:79b94cb922f0 54 if(autoflag){
DeguNaoto 57:3fbd487e055e 55 #ifdef IM920
DeguNaoto 52:d9e1629da852 56 autoIM920(); /*IM920 button*/
DeguNaoto 57:3fbd487e055e 57 #else
DeguNaoto 57:3fbd487e055e 58 autoPS3(); /*PS3 button*/
DeguNaoto 57:3fbd487e055e 59 #endif
DeguNaoto 43:f9a75ecbe44e 60 #ifdef BLUE
DeguNaoto 43:f9a75ecbe44e 61 //Blue
DeguNaoto 58:6b73e683fa70 62 if((step==0)&&((8650.0>x)&&(x>1380.0))) targ_sita=0.0,step=1;
DeguNaoto 58:6b73e683fa70 63 if((step==1)&&(x>8550.0)) targ_velocity=-speed,step=2;
DeguNaoto 38:0b8ec1fd64bf 64 if((step==2)&&(x<2000.0)) targ_sita=PI/4,step=3;
DeguNaoto 58:6b73e683fa70 65 if((step==3)&&(x<10.0)) targ_velocity=0.0,autoflag=0;
DeguNaoto 35:2db63dec2a67 66
DeguNaoto 48:64d005c70df2 67 if((x>3344.0)&&(CStep==1)) CStep=2,sendData(1,1);
DeguNaoto 48:64d005c70df2 68 if((x>6234.0)&&(CStep==2)) CStep=3,sendData(1,2);
DeguNaoto 36:fd04196b4b34 69 if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,3);
DeguNaoto 52:d9e1629da852 70 if((x<6750.0)&&(CStep==4)) CStep=5,sendData(1,5);
DeguNaoto 58:6b73e683fa70 71 if((x<6300.0)&&(CStep==5)) CStep=0,sendData(1,4);
DeguNaoto 58:6b73e683fa70 72
DeguNaoto 58:6b73e683fa70 73 //後でデバッグ
DeguNaoto 58:6b73e683fa70 74 /*if((step==4)&&((6500.0>x)&&(x>1400.0))) targ_sita=0.0,step=5;
DeguNaoto 58:6b73e683fa70 75 if((step==5)&&(x>6500.0)) targ_velocity=-speed,step=6;
DeguNaoto 58:6b73e683fa70 76 if((step==6)&&(x<2000.0)) targ_sita=PI/4,step=7;
DeguNaoto 58:6b73e683fa70 77 if((step==7)&&(x<10.0)) targ_velocity=0.0,autoflag=0;
DeguNaoto 58:6b73e683fa70 78
DeguNaoto 58:6b73e683fa70 79 if((x>6000.0)&&(CStep==4)) CStep=0,sendData(1,5);*/
DeguNaoto 43:f9a75ecbe44e 80 #else
DeguNaoto 43:f9a75ecbe44e 81 //Red
DeguNaoto 58:6b73e683fa70 82 if((step==0)&&((8650.0>x)&&(x>1380.0))) targ_sita=0.0,step=1;
DeguNaoto 43:f9a75ecbe44e 83 if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2;
DeguNaoto 43:f9a75ecbe44e 84 if((step==2)&&(x<2000.0)) targ_sita=-PI/4,step=3;
DeguNaoto 58:6b73e683fa70 85 if((step==3)&&(x<10.0)) targ_velocity=0.0,step=4,autoflag=0;
DeguNaoto 58:6b73e683fa70 86
DeguNaoto 48:64d005c70df2 87 if((x>3344.0)&&(CStep==1)) CStep=2,sendData(1,1);
DeguNaoto 48:64d005c70df2 88 if((x>6234.0)&&(CStep==2)) CStep=3,sendData(1,3);
DeguNaoto 48:64d005c70df2 89 if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,2);
DeguNaoto 58:6b73e683fa70 90 if((x<6750.0)&&(CStep==4)) CStep=5,sendData(1,4);
DeguNaoto 58:6b73e683fa70 91 if((x<6300.0)&&(CStep==5)) CStep=0,sendData(1,5);
DeguNaoto 58:6b73e683fa70 92
DeguNaoto 58:6b73e683fa70 93 /*if((step==4)&&((6500.0>x)&&(x>1400.0))) targ_sita=0.0,step=5;
DeguNaoto 58:6b73e683fa70 94 if((step==5)&&(x>6500.0)) targ_velocity=-speed,step=6;
DeguNaoto 58:6b73e683fa70 95 if((step==6)&&(x<2000.0)) targ_sita=-PI/4,step=7;
DeguNaoto 58:6b73e683fa70 96 if((step==7)&&(x<10.0)) targ_velocity=0.0,autoflag=0;
DeguNaoto 58:6b73e683fa70 97
DeguNaoto 58:6b73e683fa70 98 if((x>6000.0)&&(CStep==4)) CStep=0,sendData(1,4);*/
DeguNaoto 43:f9a75ecbe44e 99 #endif
DeguNaoto 21:79b94cb922f0 100 move_following();
DeguNaoto 4:51d87d2b698c 101 }
DeguNaoto 21:79b94cb922f0 102 else if(!autoflag) {
DeguNaoto 57:3fbd487e055e 103 #ifdef IM920
DeguNaoto 52:d9e1629da852 104 manualMoveIM920(); /*analogStick*/
DeguNaoto 52:d9e1629da852 105 manualIM920(); /*IM920 button*/
DeguNaoto 57:3fbd487e055e 106 #else
DeguNaoto 57:3fbd487e055e 107 manualMovePS3(); /*analogStick*/
DeguNaoto 57:3fbd487e055e 108 manualPS3(); /*PS3 button*/
DeguNaoto 57:3fbd487e055e 109 #endif
DeguNaoto 48:64d005c70df2 110 //Swing
DeguNaoto 58:6b73e683fa70 111 // swingFollowing();
DeguNaoto 50:8ea4714316ce 112 /*if(square){
DeguNaoto 50:8ea4714316ce 113 IndicatorAuto=0;
DeguNaoto 50:8ea4714316ce 114 fclose(fp_r);
DeguNaoto 50:8ea4714316ce 115 }
DeguNaoto 50:8ea4714316ce 116 fprintf(fp_r, "time:%1.3f, %f[rad/s], pwm:%f\r\n",time,swingRadVelocity,cont);
DeguNaoto 50:8ea4714316ce 117 time+=0.01;*/
DeguNaoto 4:51d87d2b698c 118 }
DeguNaoto 58:6b73e683fa70 119 fprintf(fp_r, "%d, %f, %f\r\n",step,x,sita);
DeguNaoto 58:6b73e683fa70 120 if(triangle) fclose(fp_r);
DeguNaoto 55:db1797ac6cb1 121 /***update state***/
DeguNaoto 57:3fbd487e055e 122 #ifdef IM920
DeguNaoto 51:cb430192b28b 123 readIM920();
DeguNaoto 57:3fbd487e055e 124 #endif
DeguNaoto 21:79b94cb922f0 125 mesure_state();
DeguNaoto 48:64d005c70df2 126 mesureSwing();
DeguNaoto 0:bd4719e15f7e 127 wait(RATE);
DeguNaoto 55:db1797ac6cb1 128
DeguNaoto 56:ac01d6b46291 129 // pc.printf("sita:%f, x:%f, y:%f ,x1:%f, x2:%f ,velocity:%f\r\n",sita,x,y,x1,x2,velocity);
DeguNaoto 52:d9e1629da852 130 // pc.printf("A2 = %d, X = %d, Y = %d, B = %d, dead = %d\r\n", a2, X, Y, b, deadflag);
DeguNaoto 58:6b73e683fa70 131 pc.printf("%f %f\r\n",cont,swingRadVelocity);
DeguNaoto 0:bd4719e15f7e 132 }
DeguNaoto 0:bd4719e15f7e 133 }