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@65:cebdfc0184d7, 2015-10-05 (annotated)

Committer:
DeguNaoto
Date:
Mon Oct 05 07:53:02 2015 +0000
Revision:
65:cebdfc0184d7
Parent:
64:a98fe602c26d
Child:
66:14df82661dfa
??????????8kHz->20kHz;

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 59:9d66edf3e734 6 //変更前のこのプログラムで実験
DeguNaoto 59:9d66edf3e734 7 //後で関数のinline宣言を外す
DeguNaoto 62:02a44f5bc51e 8 //大和田に最後にPIC1から信号を送り射角を変えるプログラムをに書かせる
DeguNaoto 62:02a44f5bc51e 9 //大和田にPIC1のシリンダの信号をタイマーで書かせる
DeguNaoto 34:f9ef622f4376 10
DeguNaoto 57:3fbd487e055e 11 /***コース選択***/
DeguNaoto 52:d9e1629da852 12 #define BLUE
DeguNaoto 52:d9e1629da852 13 //#define RED
DeguNaoto 43:f9a75ecbe44e 14
DeguNaoto 57:3fbd487e055e 15 /***コントローラ選択***/
DeguNaoto 57:3fbd487e055e 16 //#define IM920
DeguNaoto 57:3fbd487e055e 17 #define PS3
DeguNaoto 57:3fbd487e055e 18
DeguNaoto 57:3fbd487e055e 19 #if defined(IM920) && defined(PS3)
DeguNaoto 57:3fbd487e055e 20 #error Caution, You should define either IM920 or PS3
DeguNaoto 57:3fbd487e055e 21 #endif
DeguNaoto 57:3fbd487e055e 22
DeguNaoto 45:e11ec4f6d37e 23 #if defined(BLUE) && defined(RED)
DeguNaoto 45:e11ec4f6d37e 24 #error Caution, You should define either BLUE or RED
DeguNaoto 45:e11ec4f6d37e 25 #endif
DeguNaoto 45:e11ec4f6d37e 26
DeguNaoto 0:bd4719e15f7e 27 #include "machine_ps3.h"
DeguNaoto 0:bd4719e15f7e 28
DeguNaoto 0:bd4719e15f7e 29 Serial pc(USBTX, USBRX);
DeguNaoto 50:8ea4714316ce 30 //LocalFileSystem local("local");
DeguNaoto 0:bd4719e15f7e 31
DeguNaoto 61:e018207ae860 32 int main() {
DeguNaoto 57:3fbd487e055e 33 #ifdef IM920
DeguNaoto 51:cb430192b28b 34 initializeIM920();
DeguNaoto 57:3fbd487e055e 35 #else
DeguNaoto 57:3fbd487e055e 36 initializeSBDBT();
DeguNaoto 57:3fbd487e055e 37 #endif
DeguNaoto 0:bd4719e15f7e 38 initializeMotors();
DeguNaoto 0:bd4719e15f7e 39 initializeControllers();
DeguNaoto 48:64d005c70df2 40 initializeSwing();
DeguNaoto 59:9d66edf3e734 41 initializeRS485();
DeguNaoto 43:f9a75ecbe44e 42 #ifdef BLUE
DeguNaoto 34:f9ef622f4376 43 sita=PI/4.0,targ_sita=PI/4.0;
DeguNaoto 55:db1797ac6cb1 44 IndicatorBLUE = 1;
DeguNaoto 43:f9a75ecbe44e 45 #else
DeguNaoto 43:f9a75ecbe44e 46 sita=-PI/4.0,targ_sita=-PI/4.0;
DeguNaoto 55:db1797ac6cb1 47 IndicatorRED = 1;
DeguNaoto 43:f9a75ecbe44e 48 #endif
DeguNaoto 21:79b94cb922f0 49 Indicator4=1;
DeguNaoto 26:8e6c736b6791 50 Enable=1;
DeguNaoto 50:8ea4714316ce 51 // FILE *fp_r = fopen("/local/velocity.dat", "w");
DeguNaoto 62:02a44f5bc51e 52 wait(0.3);
DeguNaoto 52:d9e1629da852 53 sendData(4,31);
DeguNaoto 47:46db7f076cea 54 wait(0.1);
DeguNaoto 52:d9e1629da852 55 sendData(5,31);
DeguNaoto 52:d9e1629da852 56 wait(0.1);
DeguNaoto 50:8ea4714316ce 57 // double time=0.0;
DeguNaoto 0:bd4719e15f7e 58 while(1) {
DeguNaoto 21:79b94cb922f0 59 if(autoflag){
DeguNaoto 57:3fbd487e055e 60 #ifdef IM920
DeguNaoto 52:d9e1629da852 61 autoIM920(); /*IM920 button*/
DeguNaoto 57:3fbd487e055e 62 #else
DeguNaoto 57:3fbd487e055e 63 autoPS3(); /*PS3 button*/
DeguNaoto 57:3fbd487e055e 64 #endif
DeguNaoto 43:f9a75ecbe44e 65 #ifdef BLUE
DeguNaoto 60:4a75f3f3a934 66 //Blue
DeguNaoto 60:4a75f3f3a934 67 // if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.0,step=1;
DeguNaoto 64:a98fe602c26d 68 // if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.015,step=1; /*nomal*/
DeguNaoto 64:a98fe602c26d 69 if((step==0)&&((8650.0>x)&&(x>1000.0))) targ_sita=0.015,step=1; /*nomal*/
DeguNaoto 64:a98fe602c26d 70 if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2,flagf=0;
DeguNaoto 38:0b8ec1fd64bf 71 if((step==2)&&(x<2000.0)) targ_sita=PI/4,step=3;
DeguNaoto 61:e018207ae860 72 if((step==3)&&(x<10.0)) targ_velocity=0.0,step=8;
DeguNaoto 35:2db63dec2a67 73
DeguNaoto 64:a98fe602c26d 74 if((step==4)&&((5700.0>x)&&(x>600.0))) targ_sita=0.015,step=5; /*middle*/
DeguNaoto 65:cebdfc0184d7 75 if((step==5)&&(x>4850.0)) { /*middle shoot*/
DeguNaoto 61:e018207ae860 76 targ_velocity=-speed;
DeguNaoto 61:e018207ae860 77 step=6;
DeguNaoto 64:a98fe602c26d 78 sendData(1,5);
DeguNaoto 64:a98fe602c26d 79 flagf=0;
DeguNaoto 61:e018207ae860 80 }
DeguNaoto 65:cebdfc0184d7 81 if((step==6)&&(x<1650.0)) targ_sita=PI/4,step=7;
DeguNaoto 61:e018207ae860 82 if((step==7)&&(x<10.0)) targ_velocity=0.0,step=8;
DeguNaoto 61:e018207ae860 83
DeguNaoto 64:a98fe602c26d 84 if((x>3000.0)&&(CStep==1)) CStep=2,sendData(1,1); /*nomal*/
DeguNaoto 64:a98fe602c26d 85 if((x>6000.0)&&(CStep==2)) CStep=3,sendData(1,2);
DeguNaoto 64:a98fe602c26d 86 if((x>7500.0)&&(CStep==3)) CStep=4,sendData(1,3);
DeguNaoto 64:a98fe602c26d 87 if((x<6550.0)&&(CStep==4)) CStep=5,sendData(1,5);
DeguNaoto 64:a98fe602c26d 88 if((x<6100.0)&&(CStep==5)) CStep=6,sendData(1,4);
DeguNaoto 61:e018207ae860 89
DeguNaoto 64:a98fe602c26d 90 /*if((x>3344.0)&&(CStep==1)) CStep=2,sendData(1,1);
DeguNaoto 48:64d005c70df2 91 if((x>6234.0)&&(CStep==2)) CStep=3,sendData(1,2);
DeguNaoto 36:fd04196b4b34 92 if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,3);
DeguNaoto 61:e018207ae860 93 if((x<6750.0)&&(CStep==4)) CStep=5,sendData(1,5);
DeguNaoto 64:a98fe602c26d 94 if((x<6300.0)&&(CStep==5)) CStep=6,sendData(1,4);*/
DeguNaoto 61:e018207ae860 95
DeguNaoto 43:f9a75ecbe44e 96 #else
DeguNaoto 43:f9a75ecbe44e 97 //Red
DeguNaoto 62:02a44f5bc51e 98 if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=-0.015,step=1;
DeguNaoto 43:f9a75ecbe44e 99 if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2;
DeguNaoto 43:f9a75ecbe44e 100 if((step==2)&&(x<2000.0)) targ_sita=-PI/4,step=3;
DeguNaoto 62:02a44f5bc51e 101 if((step==3)&&(x<10.0)) targ_velocity=0.0,step=8;
DeguNaoto 62:02a44f5bc51e 102
DeguNaoto 62:02a44f5bc51e 103 if((step==4)&&((5700.0>x)&&(x>950.0))) targ_sita=-0.015,step=5; /*middle*/
DeguNaoto 62:02a44f5bc51e 104 if((step==5)&&(x>5300.0)) { /*middle shoot*/
DeguNaoto 62:02a44f5bc51e 105 targ_velocity=-speed;
DeguNaoto 62:02a44f5bc51e 106 step=6;
DeguNaoto 62:02a44f5bc51e 107 sendData(1,4);
DeguNaoto 62:02a44f5bc51e 108 }
DeguNaoto 62:02a44f5bc51e 109 if((step==6)&&(x<2000.0)) targ_sita=-PI/4,step=7;
DeguNaoto 62:02a44f5bc51e 110 if((step==7)&&(x<10.0)) targ_velocity=0.0,step=8;
DeguNaoto 43:f9a75ecbe44e 111
DeguNaoto 48:64d005c70df2 112 if((x>3344.0)&&(CStep==1)) CStep=2,sendData(1,1);
DeguNaoto 48:64d005c70df2 113 if((x>6234.0)&&(CStep==2)) CStep=3,sendData(1,3);
DeguNaoto 48:64d005c70df2 114 if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,2);
DeguNaoto 52:d9e1629da852 115 if((x<6700.0)&&(CStep==4)) CStep=5,sendData(1,4);
DeguNaoto 52:d9e1629da852 116 if((x<6300.0)&&(CStep==5)) CStep=6,sendData(1,5);
DeguNaoto 43:f9a75ecbe44e 117 #endif
DeguNaoto 21:79b94cb922f0 118 move_following();
DeguNaoto 4:51d87d2b698c 119 }
DeguNaoto 21:79b94cb922f0 120 else if(!autoflag) {
DeguNaoto 57:3fbd487e055e 121 #ifdef IM920
DeguNaoto 52:d9e1629da852 122 manualMoveIM920(); /*analogStick*/
DeguNaoto 52:d9e1629da852 123 manualIM920(); /*IM920 button*/
DeguNaoto 57:3fbd487e055e 124 #else
DeguNaoto 57:3fbd487e055e 125 manualMovePS3(); /*analogStick*/
DeguNaoto 57:3fbd487e055e 126 manualPS3(); /*PS3 button*/
DeguNaoto 57:3fbd487e055e 127 #endif
DeguNaoto 48:64d005c70df2 128 //Swing
DeguNaoto 60:4a75f3f3a934 129 // swingFollowing();
DeguNaoto 50:8ea4714316ce 130 /*if(square){
DeguNaoto 50:8ea4714316ce 131 IndicatorAuto=0;
DeguNaoto 50:8ea4714316ce 132 fclose(fp_r);
DeguNaoto 50:8ea4714316ce 133 }
DeguNaoto 50:8ea4714316ce 134 fprintf(fp_r, "time:%1.3f, %f[rad/s], pwm:%f\r\n",time,swingRadVelocity,cont);
DeguNaoto 50:8ea4714316ce 135 time+=0.01;*/
DeguNaoto 4:51d87d2b698c 136 }
DeguNaoto 55:db1797ac6cb1 137 /***update state***/
DeguNaoto 57:3fbd487e055e 138 #ifdef IM920
DeguNaoto 51:cb430192b28b 139 readIM920();
DeguNaoto 57:3fbd487e055e 140 #endif
DeguNaoto 21:79b94cb922f0 141 mesure_state();
DeguNaoto 48:64d005c70df2 142 mesureSwing();
DeguNaoto 0:bd4719e15f7e 143 wait(RATE);
DeguNaoto 55:db1797ac6cb1 144
DeguNaoto 56:ac01d6b46291 145 // 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 146 // pc.printf("A2 = %d, X = %d, Y = %d, B = %d, dead = %d\r\n", a2, X, Y, b, deadflag);
DeguNaoto 57:3fbd487e055e 147 // pc.printf("%f %f\r\n",cont,swingRadVelocity);
DeguNaoto 0:bd4719e15f7e 148 }
DeguNaoto 0:bd4719e15f7e 149 }