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@111:846ea36dc1dc, 2015-10-20 (annotated)

Committer:
DeguNaoto
Date:
Tue Oct 20 09:05:43 2015 +0000
Revision:
111:846ea36dc1dc
Parent:
110:2ecb2f8aff81
Child:
112:717acc7c99f9
20151020 ???

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 66:14df82661dfa 5 //回転機構wait PI/4 追加で待つ
DeguNaoto 76:f9bcb7ac438f 6 //射出をスキップする機能を追加
DeguNaoto 71:4ebc1c0fcb4e 7 //スタートゾーンにマシンを戻した後に暴走しないようにする
DeguNaoto 73:86b7b82ba997 8 //手動時の操作性の改善
DeguNaoto 73:86b7b82ba997 9 //一度手動モードになったらスタートゾーンに戻るまで再び自動モードにならないようにする
DeguNaoto 83:e1638c58e1f1 10 //flagfで前進・後退切り替え
DeguNaoto 83:e1638c58e1f1 11 //大和田射角
DeguNaoto 83:e1638c58e1f1 12 //午後に一定回転制御の実験
DeguNaoto 85:dd18a2d79956 13 //モード切替時に時間を待つ
DeguNaoto 103:ffd3ca4a7a71 14 //赤チーム側を青チーム側のプログラムに合わせて実装
DeguNaoto 107:579bb1ab67d9 15 //横幅調整済み
DeguNaoto 108:7eb434cfcbd7 16 //相手妨害モードいらない new!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
DeguNaoto 110:2ecb2f8aff81 17 //赤チームモード、相手チーム側の射出位置調整
DeguNaoto 34:f9ef622f4376 18
DeguNaoto 57:3fbd487e055e 19 /***コース選択***/
DeguNaoto 105:0be5d5d64782 20 //#define BLUE
DeguNaoto 105:0be5d5d64782 21 #define RED
DeguNaoto 43:f9a75ecbe44e 22
DeguNaoto 57:3fbd487e055e 23 /***コントローラ選択***/
DeguNaoto 86:5f0b065cb4d3 24 #define IM920
DeguNaoto 86:5f0b065cb4d3 25 //#define PS3
DeguNaoto 83:e1638c58e1f1 26
DeguNaoto 83:e1638c58e1f1 27 /***回転機構測定***/
DeguNaoto 83:e1638c58e1f1 28 //#define MESURE
DeguNaoto 57:3fbd487e055e 29
DeguNaoto 57:3fbd487e055e 30 #if defined(IM920) && defined(PS3)
DeguNaoto 57:3fbd487e055e 31 #error Caution, You should define either IM920 or PS3
DeguNaoto 57:3fbd487e055e 32 #endif
DeguNaoto 57:3fbd487e055e 33
DeguNaoto 45:e11ec4f6d37e 34 #if defined(BLUE) && defined(RED)
DeguNaoto 45:e11ec4f6d37e 35 #error Caution, You should define either BLUE or RED
DeguNaoto 45:e11ec4f6d37e 36 #endif
DeguNaoto 45:e11ec4f6d37e 37
DeguNaoto 0:bd4719e15f7e 38 #include "machine_ps3.h"
DeguNaoto 0:bd4719e15f7e 39
DeguNaoto 106:20dab118bd5b 40 #define deff 70.0
DeguNaoto 106:20dab118bd5b 41
DeguNaoto 0:bd4719e15f7e 42 Serial pc(USBTX, USBRX);
DeguNaoto 83:e1638c58e1f1 43 #ifdef MESURE
DeguNaoto 83:e1638c58e1f1 44 LocalFileSystem local("local");
DeguNaoto 83:e1638c58e1f1 45 #endif
DeguNaoto 101:b67d33e56b66 46
DeguNaoto 77:7dd154b5ace3 47 Timeout MStop;
DeguNaoto 77:7dd154b5ace3 48 void Restart(){
DeguNaoto 77:7dd154b5ace3 49 if(step==10) step=11;
DeguNaoto 77:7dd154b5ace3 50 }
DeguNaoto 0:bd4719e15f7e 51
DeguNaoto 61:e018207ae860 52 int main() {
DeguNaoto 57:3fbd487e055e 53 #ifdef IM920
DeguNaoto 51:cb430192b28b 54 initializeIM920();
DeguNaoto 57:3fbd487e055e 55 #else
DeguNaoto 57:3fbd487e055e 56 initializeSBDBT();
DeguNaoto 57:3fbd487e055e 57 #endif
DeguNaoto 0:bd4719e15f7e 58 initializeMotors();
DeguNaoto 0:bd4719e15f7e 59 initializeControllers();
DeguNaoto 48:64d005c70df2 60 initializeSwing();
DeguNaoto 59:9d66edf3e734 61 initializeRS485();
DeguNaoto 43:f9a75ecbe44e 62 #ifdef BLUE
DeguNaoto 34:f9ef622f4376 63 sita=PI/4.0,targ_sita=PI/4.0;
DeguNaoto 55:db1797ac6cb1 64 IndicatorBLUE = 1;
DeguNaoto 43:f9a75ecbe44e 65 #else
DeguNaoto 43:f9a75ecbe44e 66 sita=-PI/4.0,targ_sita=-PI/4.0;
DeguNaoto 55:db1797ac6cb1 67 IndicatorRED = 1;
DeguNaoto 43:f9a75ecbe44e 68 #endif
DeguNaoto 21:79b94cb922f0 69 Indicator4=1;
DeguNaoto 26:8e6c736b6791 70 Enable=1;
DeguNaoto 83:e1638c58e1f1 71 #ifdef MESURE
DeguNaoto 83:e1638c58e1f1 72 FILE *fp_r = fopen("/local/velocity.dat", "w");
DeguNaoto 83:e1638c58e1f1 73 double time=0.0;
DeguNaoto 83:e1638c58e1f1 74 #endif
DeguNaoto 62:02a44f5bc51e 75 wait(0.3);
DeguNaoto 103:ffd3ca4a7a71 76 sendData(7,0);
DeguNaoto 0:bd4719e15f7e 77 while(1) {
DeguNaoto 21:79b94cb922f0 78 if(autoflag){
DeguNaoto 57:3fbd487e055e 79 #ifdef IM920
DeguNaoto 52:d9e1629da852 80 autoIM920(); /*IM920 button*/
DeguNaoto 57:3fbd487e055e 81 #else
DeguNaoto 57:3fbd487e055e 82 autoPS3(); /*PS3 button*/
DeguNaoto 57:3fbd487e055e 83 #endif
DeguNaoto 43:f9a75ecbe44e 84 #ifdef BLUE
DeguNaoto 60:4a75f3f3a934 85 //Blue
DeguNaoto 88:cbed08732872 86 if(spcount<speed){
DeguNaoto 89:048a72382b0a 87 spcount+=speed/100.0;
DeguNaoto 88:cbed08732872 88 targ_velocity=spcount;
DeguNaoto 88:cbed08732872 89 }
DeguNaoto 90:9fe39ebe721f 90 /********************************Nomal Mode*********************************/
DeguNaoto 92:3a12444b5192 91 if((step==0)&&((8650.0>x)&&(x>800.0))) {
DeguNaoto 101:b67d33e56b66 92 targ_sita=0.025;
DeguNaoto 83:e1638c58e1f1 93 step=1;
DeguNaoto 83:e1638c58e1f1 94 }
DeguNaoto 106:20dab118bd5b 95 if((step==1)&&(x>8600.0+deff)) {
DeguNaoto 83:e1638c58e1f1 96 targ_velocity=0.0;
DeguNaoto 79:7f86e18f40ef 97 step=2;
DeguNaoto 83:e1638c58e1f1 98 }
DeguNaoto 88:cbed08732872 99 if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 83:e1638c58e1f1 100 step=3;
DeguNaoto 89:048a72382b0a 101 spcount=0.0;
DeguNaoto 79:7f86e18f40ef 102 flagf=0;
DeguNaoto 101:b67d33e56b66 103 targ_sita=0.035;
DeguNaoto 79:7f86e18f40ef 104 }
DeguNaoto 86:5f0b065cb4d3 105 if((step==3)&&(x<2000.0)) {
DeguNaoto 66:14df82661dfa 106 targ_sita=PI/4;
DeguNaoto 83:e1638c58e1f1 107 step=4;
DeguNaoto 66:14df82661dfa 108 }
DeguNaoto 85:dd18a2d79956 109 if((step==4)&&(x<0.0)) {
DeguNaoto 73:86b7b82ba997 110 targ_velocity=0.0;
DeguNaoto 79:7f86e18f40ef 111 step=114;
DeguNaoto 70:5b8ed76a31c7 112 }
DeguNaoto 76:f9bcb7ac438f 113 /***Cylinder***/
DeguNaoto 107:579bb1ab67d9 114 if((x>3030.0+deff)&&(CStep==1)) {
DeguNaoto 72:021a14ee970f 115 if(!skip) sendData(1,1);
DeguNaoto 77:7dd154b5ace3 116 CStep=2;
DeguNaoto 72:021a14ee970f 117 }
DeguNaoto 108:7eb434cfcbd7 118 if((x>5900.0+deff)&&(CStep==2)) {
DeguNaoto 85:dd18a2d79956 119 if(!skip) sendData(1,3);
DeguNaoto 72:021a14ee970f 120 CStep=3;
DeguNaoto 72:021a14ee970f 121 }
DeguNaoto 107:579bb1ab67d9 122 if((x>7680.0+deff)&&(CStep==3)) {
DeguNaoto 85:dd18a2d79956 123 if(!skip) sendData(1,2);
DeguNaoto 72:021a14ee970f 124 CStep=4;
DeguNaoto 72:021a14ee970f 125 }
DeguNaoto 108:7eb434cfcbd7 126 if((x<6650.0+deff)&&(CStep==4)) {
DeguNaoto 72:021a14ee970f 127 if(!skip) sendData(1,5);
DeguNaoto 72:021a14ee970f 128 CStep=5;
DeguNaoto 72:021a14ee970f 129 }
DeguNaoto 107:579bb1ab67d9 130 if((x<6170.0+deff)&&(CStep==5)) {
DeguNaoto 72:021a14ee970f 131 if(!skip) sendData(1,4);
DeguNaoto 101:b67d33e56b66 132 CStep=6;
DeguNaoto 101:b67d33e56b66 133 }
DeguNaoto 101:b67d33e56b66 134 if((x<4000.0)&&(CStep==6)){
DeguNaoto 103:ffd3ca4a7a71 135 sendData(7,0);
DeguNaoto 83:e1638c58e1f1 136 CStep=114;
DeguNaoto 72:021a14ee970f 137 }
DeguNaoto 76:f9bcb7ac438f 138 /////////////////////////////////////////////////////////////////////////
DeguNaoto 76:f9bcb7ac438f 139 /******************************Middle Mode******************************/
DeguNaoto 83:e1638c58e1f1 140 if((step==5)&&((5700.0>x)&&(x>800.0))) {
DeguNaoto 101:b67d33e56b66 141 targ_sita=0.025;
DeguNaoto 76:f9bcb7ac438f 142 step=6;
DeguNaoto 83:e1638c58e1f1 143 }
DeguNaoto 83:e1638c58e1f1 144 if((step==6)&&(x>7000.0)){
DeguNaoto 83:e1638c58e1f1 145 targ_velocity=0.0;
DeguNaoto 83:e1638c58e1f1 146 step=7;
DeguNaoto 76:f9bcb7ac438f 147 }
DeguNaoto 88:cbed08732872 148 if((step==7)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 83:e1638c58e1f1 149 flagf=0;
DeguNaoto 89:048a72382b0a 150 spcount=0.0;
DeguNaoto 101:b67d33e56b66 151 targ_sita=0.035;
DeguNaoto 83:e1638c58e1f1 152 step=8;
DeguNaoto 83:e1638c58e1f1 153 }
DeguNaoto 83:e1638c58e1f1 154 if((step==8)&&(x<1900.0)) {
DeguNaoto 76:f9bcb7ac438f 155 targ_sita=PI/4;
DeguNaoto 83:e1638c58e1f1 156 step=9;
DeguNaoto 76:f9bcb7ac438f 157 }
DeguNaoto 85:dd18a2d79956 158 if((step==9)&&(x<-10.0)) {
DeguNaoto 76:f9bcb7ac438f 159 targ_velocity=0.0;
DeguNaoto 76:f9bcb7ac438f 160 step=114;
DeguNaoto 76:f9bcb7ac438f 161 }
DeguNaoto 83:e1638c58e1f1 162
DeguNaoto 107:579bb1ab67d9 163 if((x>4620.0+deff)&&(CStep==7)){
DeguNaoto 83:e1638c58e1f1 164 sendData(1,5);
DeguNaoto 101:b67d33e56b66 165 CStep=8;
DeguNaoto 83:e1638c58e1f1 166 }
DeguNaoto 107:579bb1ab67d9 167 if((x>5720.0+deff)&&(CStep==8)){
DeguNaoto 83:e1638c58e1f1 168 sendData(1,4);
DeguNaoto 101:b67d33e56b66 169 CStep=9;
DeguNaoto 101:b67d33e56b66 170 }
DeguNaoto 101:b67d33e56b66 171 if((x<4000.0)&&(CStep==9)){
DeguNaoto 103:ffd3ca4a7a71 172 sendData(7,0);
DeguNaoto 83:e1638c58e1f1 173 CStep=114;
DeguNaoto 83:e1638c58e1f1 174 }
DeguNaoto 76:f9bcb7ac438f 175 /////////////////////////////////////////////////////////////////////////
DeguNaoto 77:7dd154b5ace3 176 /*****************************Opponents Mode****************************/
DeguNaoto 101:b67d33e56b66 177 if((step==15)&&((5700.0>x)&&(x>50.0))) {
DeguNaoto 101:b67d33e56b66 178 targ_sita=0.025;
DeguNaoto 83:e1638c58e1f1 179 step=16;
DeguNaoto 83:e1638c58e1f1 180 }
DeguNaoto 107:579bb1ab67d9 181 if((step==16)&&(x>2850.0+deff)){
DeguNaoto 79:7f86e18f40ef 182 targ_velocity=0.0;
DeguNaoto 84:919a335ac81e 183 step=17;
DeguNaoto 84:919a335ac81e 184 }
DeguNaoto 101:b67d33e56b66 185 if((step==17)&&((velocity<5000.0)&&(velocity>-5000.0))){
DeguNaoto 84:919a335ac81e 186 flagf=0;
DeguNaoto 89:048a72382b0a 187 spcount=0.0;
DeguNaoto 101:b67d33e56b66 188 targ_sita=0.035;
DeguNaoto 84:919a335ac81e 189 step=18;
DeguNaoto 84:919a335ac81e 190 }
DeguNaoto 84:919a335ac81e 191 if((step==18)&&(x<-100.0)){
DeguNaoto 84:919a335ac81e 192 targ_velocity=0.0;
DeguNaoto 89:048a72382b0a 193 targ_sita=PI/4.0;
DeguNaoto 79:7f86e18f40ef 194 step=114;
DeguNaoto 84:919a335ac81e 195 }
DeguNaoto 84:919a335ac81e 196
DeguNaoto 108:7eb434cfcbd7 197 // if((x>1200.0+deff)&&(CStep==15)){
DeguNaoto 108:7eb434cfcbd7 198 if((x>1000.0+deff)&&(CStep==15)){
DeguNaoto 84:919a335ac81e 199 sendData(1,5);
DeguNaoto 85:dd18a2d79956 200 CStep=16;
DeguNaoto 85:dd18a2d79956 201 }
DeguNaoto 108:7eb434cfcbd7 202 // if((x>2655.0+deff)&&(CStep==16)){
DeguNaoto 108:7eb434cfcbd7 203 if((x>2455.0+deff)&&(CStep==16)){
DeguNaoto 108:7eb434cfcbd7 204 // if((x>2630.0)&&(CStep==16)){
DeguNaoto 85:dd18a2d79956 205 sendData(1,4);
DeguNaoto 101:b67d33e56b66 206 CStep=17;
DeguNaoto 101:b67d33e56b66 207 }
DeguNaoto 107:579bb1ab67d9 208 if((x<2400.0+deff)&&(CStep==17)){
DeguNaoto 103:ffd3ca4a7a71 209 sendData(7,0);
DeguNaoto 84:919a335ac81e 210 CStep=114;
DeguNaoto 103:ffd3ca4a7a71 211 }
DeguNaoto 77:7dd154b5ace3 212 /////////////////////////////////////////////////////////////////////////
DeguNaoto 107:579bb1ab67d9 213 /*****************************Disturb Mode****************************/
DeguNaoto 107:579bb1ab67d9 214 if((step==50)&&((8650.0>x)&&(x>800.0))) {
DeguNaoto 107:579bb1ab67d9 215 targ_sita=0.025;
DeguNaoto 107:579bb1ab67d9 216 step=51;
DeguNaoto 107:579bb1ab67d9 217 }
DeguNaoto 108:7eb434cfcbd7 218 if((step==51)&&(x>1500.0+deff)) {
DeguNaoto 107:579bb1ab67d9 219 targ_velocity=0.0;
DeguNaoto 107:579bb1ab67d9 220 velocity_controller.reset();
DeguNaoto 107:579bb1ab67d9 221 direction_controller.reset();
DeguNaoto 107:579bb1ab67d9 222 Move_l(0.0);
DeguNaoto 107:579bb1ab67d9 223 Move_r(0.0);
DeguNaoto 107:579bb1ab67d9 224 targ_sita=PI/2.0;
DeguNaoto 107:579bb1ab67d9 225 step=52;
DeguNaoto 107:579bb1ab67d9 226 }
DeguNaoto 107:579bb1ab67d9 227 if((step==52)&&((sita<PI/2.0+0.5)&&(sita>PI/2.0-0.5))){
DeguNaoto 107:579bb1ab67d9 228 step=53;
DeguNaoto 107:579bb1ab67d9 229 autoflag=0;
DeguNaoto 108:7eb434cfcbd7 230 flaga=0;
DeguNaoto 107:579bb1ab67d9 231 }
DeguNaoto 107:579bb1ab67d9 232 /////////////////////////////////////////////////////////////////////////
DeguNaoto 71:4ebc1c0fcb4e 233 #else
DeguNaoto 43:f9a75ecbe44e 234 //Red
DeguNaoto 92:3a12444b5192 235 if(spcount<speed){
DeguNaoto 92:3a12444b5192 236 spcount+=speed/100.0;
DeguNaoto 92:3a12444b5192 237 targ_velocity=spcount;
DeguNaoto 92:3a12444b5192 238 }
DeguNaoto 76:f9bcb7ac438f 239 /********************************Nomal Mode*********************************/
DeguNaoto 101:b67d33e56b66 240 if((step==0)&&((8650.0>x)&&(x>800.0))) {
DeguNaoto 105:0be5d5d64782 241 targ_sita=-0.025;
DeguNaoto 105:0be5d5d64782 242 step=1;
DeguNaoto 83:e1638c58e1f1 243 }
DeguNaoto 106:20dab118bd5b 244 if((step==1)&&(x>8600.0+deff)) {
DeguNaoto 83:e1638c58e1f1 245 targ_velocity=0.0;
DeguNaoto 79:7f86e18f40ef 246 step=2;
DeguNaoto 79:7f86e18f40ef 247 }
DeguNaoto 92:3a12444b5192 248 if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 71:4ebc1c0fcb4e 249 step=3;
DeguNaoto 92:3a12444b5192 250 spcount=0.0;
DeguNaoto 83:e1638c58e1f1 251 flagf=0;
DeguNaoto 105:0be5d5d64782 252 targ_sita=-0.035;
DeguNaoto 71:4ebc1c0fcb4e 253 }
DeguNaoto 105:0be5d5d64782 254 if((step==3)&&(x<2000.0)) {
DeguNaoto 83:e1638c58e1f1 255 targ_sita=-PI/4;
DeguNaoto 83:e1638c58e1f1 256 step=4;
DeguNaoto 83:e1638c58e1f1 257 }
DeguNaoto 92:3a12444b5192 258 if((step==4)&&(x<0.0)) {
DeguNaoto 73:86b7b82ba997 259 targ_velocity=0.0;
DeguNaoto 83:e1638c58e1f1 260 step=114;
DeguNaoto 73:86b7b82ba997 261 }
DeguNaoto 105:0be5d5d64782 262 /***Cylinder***/
DeguNaoto 106:20dab118bd5b 263 if((x>3030.0+deff)&&(CStep==1)) {
DeguNaoto 75:4cdcd50ee666 264 if(!skip) sendData(1,1);
DeguNaoto 105:0be5d5d64782 265 CStep=2;
DeguNaoto 75:4cdcd50ee666 266 }
DeguNaoto 106:20dab118bd5b 267 if((x>5940.0+deff)&&(CStep==2)) {
DeguNaoto 85:dd18a2d79956 268 if(!skip) sendData(1,2);
DeguNaoto 75:4cdcd50ee666 269 CStep=3;
DeguNaoto 75:4cdcd50ee666 270 }
DeguNaoto 106:20dab118bd5b 271 if((x>7680.0+deff)&&(CStep==3)) {
DeguNaoto 85:dd18a2d79956 272 if(!skip) sendData(1,3);
DeguNaoto 105:0be5d5d64782 273 CStep=4;
DeguNaoto 75:4cdcd50ee666 274 }
DeguNaoto 106:20dab118bd5b 275 if((x<6730.0+deff)&&(CStep==4)) {
DeguNaoto 75:4cdcd50ee666 276 if(!skip) sendData(1,4);
DeguNaoto 75:4cdcd50ee666 277 CStep=5;
DeguNaoto 75:4cdcd50ee666 278 }
DeguNaoto 106:20dab118bd5b 279 if((x<6170.0+deff)&&(CStep==5)) {
DeguNaoto 75:4cdcd50ee666 280 if(!skip) sendData(1,5);
DeguNaoto 105:0be5d5d64782 281 CStep=6;
DeguNaoto 105:0be5d5d64782 282 }
DeguNaoto 105:0be5d5d64782 283 if((x<4000.0)&&(CStep==6)){
DeguNaoto 105:0be5d5d64782 284 sendData(7,0);
DeguNaoto 83:e1638c58e1f1 285 CStep=114;
DeguNaoto 75:4cdcd50ee666 286 }
DeguNaoto 76:f9bcb7ac438f 287 /////////////////////////////////////////////////////////////////////////
DeguNaoto 76:f9bcb7ac438f 288 /******************************Middle Mode******************************/
DeguNaoto 92:3a12444b5192 289 if((step==5)&&((5700.0>x)&&(x>800.0))) {
DeguNaoto 105:0be5d5d64782 290 targ_sita=-0.025;
DeguNaoto 92:3a12444b5192 291 step=6;
DeguNaoto 83:e1638c58e1f1 292 }
DeguNaoto 83:e1638c58e1f1 293 if((step==6)&&(x>7000.0)){
DeguNaoto 83:e1638c58e1f1 294 targ_velocity=0.0;
DeguNaoto 83:e1638c58e1f1 295 step=7;
DeguNaoto 77:7dd154b5ace3 296 }
DeguNaoto 92:3a12444b5192 297 if((step==7)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 76:f9bcb7ac438f 298 flagf=0;
DeguNaoto 92:3a12444b5192 299 spcount=0.0;
DeguNaoto 105:0be5d5d64782 300 targ_sita=-0.035;
DeguNaoto 83:e1638c58e1f1 301 step=8;
DeguNaoto 76:f9bcb7ac438f 302 }
DeguNaoto 109:e009296125db 303 if((step==8)&&(x<1820.0)) {
DeguNaoto 76:f9bcb7ac438f 304 targ_sita=-PI/4;
DeguNaoto 83:e1638c58e1f1 305 step=9;
DeguNaoto 76:f9bcb7ac438f 306 }
DeguNaoto 92:3a12444b5192 307 if((step==9)&&(x<-10.0)) {
DeguNaoto 76:f9bcb7ac438f 308 targ_velocity=0.0;
DeguNaoto 84:919a335ac81e 309 step=114;
DeguNaoto 76:f9bcb7ac438f 310 }
DeguNaoto 83:e1638c58e1f1 311
DeguNaoto 109:e009296125db 312 if((x>4690.0+deff)&&(CStep==7)){
DeguNaoto 83:e1638c58e1f1 313 sendData(1,4);
DeguNaoto 105:0be5d5d64782 314 CStep=8;
DeguNaoto 83:e1638c58e1f1 315 }
DeguNaoto 109:e009296125db 316 if((x>5790.0+deff)&&(CStep==8)){
DeguNaoto 92:3a12444b5192 317 sendData(1,5);
DeguNaoto 105:0be5d5d64782 318 CStep=9;
DeguNaoto 105:0be5d5d64782 319 }
DeguNaoto 105:0be5d5d64782 320 if((x<4000.0)&&(CStep==9)){
DeguNaoto 105:0be5d5d64782 321 sendData(7,0);
DeguNaoto 92:3a12444b5192 322 CStep=114;
DeguNaoto 92:3a12444b5192 323 }
DeguNaoto 84:919a335ac81e 324 /////////////////////////////////////////////////////////////////////////
DeguNaoto 84:919a335ac81e 325 /*****************************Opponents Mode****************************/
DeguNaoto 104:3c77a48da95c 326 if((step==15)&&((5700.0>x)&&(x>50.0))) {
DeguNaoto 104:3c77a48da95c 327 targ_sita=-0.025;
DeguNaoto 84:919a335ac81e 328 step=16;
DeguNaoto 84:919a335ac81e 329 }
DeguNaoto 109:e009296125db 330 if((step==16)&&(x>2800.0+deff)){
DeguNaoto 84:919a335ac81e 331 targ_velocity=0.0;
DeguNaoto 84:919a335ac81e 332 step=17;
DeguNaoto 84:919a335ac81e 333 }
DeguNaoto 104:3c77a48da95c 334 if((step==17)&&((velocity<5000.0)&&(velocity>-5000.0))){
DeguNaoto 84:919a335ac81e 335 flagf=0;
DeguNaoto 92:3a12444b5192 336 spcount=0.0;
DeguNaoto 105:0be5d5d64782 337 targ_sita=-0.035;
DeguNaoto 84:919a335ac81e 338 step=18;
DeguNaoto 84:919a335ac81e 339 }
DeguNaoto 84:919a335ac81e 340 if((step==18)&&(x<-100.0)){
DeguNaoto 83:e1638c58e1f1 341 targ_velocity=0.0;
DeguNaoto 105:0be5d5d64782 342 targ_sita=-PI/4.0;
DeguNaoto 83:e1638c58e1f1 343 step=114;
DeguNaoto 84:919a335ac81e 344 }
DeguNaoto 84:919a335ac81e 345
DeguNaoto 106:20dab118bd5b 346 if((x>1450.0+deff)&&(CStep==15)){
DeguNaoto 84:919a335ac81e 347 sendData(1,4);
DeguNaoto 97:5e6f513b26f2 348 CStep=16;
DeguNaoto 97:5e6f513b26f2 349 }
DeguNaoto 106:20dab118bd5b 350 if((x>2850.0+deff)&&(CStep==16)){
DeguNaoto 97:5e6f513b26f2 351 sendData(1,5);
DeguNaoto 104:3c77a48da95c 352 CStep=17;
DeguNaoto 104:3c77a48da95c 353 }
DeguNaoto 107:579bb1ab67d9 354 if((x<2400.0+deff)&&(CStep==17)){
DeguNaoto 104:3c77a48da95c 355 sendData(7,0);
DeguNaoto 84:919a335ac81e 356 CStep=114;
DeguNaoto 83:e1638c58e1f1 357 }
DeguNaoto 108:7eb434cfcbd7 358 /////////////////////////////////////////////////////////////////////////
DeguNaoto 108:7eb434cfcbd7 359 /*****************************Disturb Mode****************************/
DeguNaoto 108:7eb434cfcbd7 360 if((step==50)&&((8650.0>x)&&(x>800.0))) {
DeguNaoto 108:7eb434cfcbd7 361 targ_sita=-0.025;
DeguNaoto 108:7eb434cfcbd7 362 step=51;
DeguNaoto 108:7eb434cfcbd7 363 }
DeguNaoto 108:7eb434cfcbd7 364 if((step==51)&&(x>1000.0+deff)) {
DeguNaoto 108:7eb434cfcbd7 365 targ_velocity=0.0;
DeguNaoto 108:7eb434cfcbd7 366 Move_l(0.0);
DeguNaoto 108:7eb434cfcbd7 367 Move_r(0.0);
DeguNaoto 108:7eb434cfcbd7 368 velocity_controller.reset();
DeguNaoto 108:7eb434cfcbd7 369 direction_controller.reset();
DeguNaoto 108:7eb434cfcbd7 370 targ_sita=-PI/2.0;
DeguNaoto 108:7eb434cfcbd7 371 step=52;
DeguNaoto 108:7eb434cfcbd7 372 }
DeguNaoto 108:7eb434cfcbd7 373 if((step==52)&&((sita<PI/2.0+0.5)&&(sita>PI/2.0-0.5))){
DeguNaoto 108:7eb434cfcbd7 374 step=53;
DeguNaoto 108:7eb434cfcbd7 375 autoflag=0;
DeguNaoto 108:7eb434cfcbd7 376 flaga=0;
DeguNaoto 108:7eb434cfcbd7 377 }
DeguNaoto 76:f9bcb7ac438f 378 /////////////////////////////////////////////////////////////////////////
DeguNaoto 66:14df82661dfa 379 #endif
DeguNaoto 73:86b7b82ba997 380 mesure_state(); /*位置測定*/
DeguNaoto 76:f9bcb7ac438f 381 move_following(); /*移動制御*/
DeguNaoto 4:51d87d2b698c 382 }
DeguNaoto 21:79b94cb922f0 383 else if(!autoflag) {
DeguNaoto 57:3fbd487e055e 384 #ifdef IM920
DeguNaoto 52:d9e1629da852 385 manualMoveIM920(); /*analogStick*/
DeguNaoto 52:d9e1629da852 386 manualIM920(); /*IM920 button*/
DeguNaoto 57:3fbd487e055e 387 #else
DeguNaoto 57:3fbd487e055e 388 manualMovePS3(); /*analogStick*/
DeguNaoto 57:3fbd487e055e 389 manualPS3(); /*PS3 button*/
DeguNaoto 57:3fbd487e055e 390 #endif
DeguNaoto 48:64d005c70df2 391 //Swing
unicore32 91:83c2e0381384 392 swingFollowing();
DeguNaoto 89:048a72382b0a 393 flaga=0;
DeguNaoto 83:e1638c58e1f1 394 #ifdef MESURE
DeguNaoto 83:e1638c58e1f1 395 if(down){
DeguNaoto 50:8ea4714316ce 396 IndicatorAuto=0;
DeguNaoto 50:8ea4714316ce 397 fclose(fp_r);
DeguNaoto 50:8ea4714316ce 398 }
DeguNaoto 50:8ea4714316ce 399 fprintf(fp_r, "time:%1.3f, %f[rad/s], pwm:%f\r\n",time,swingRadVelocity,cont);
DeguNaoto 83:e1638c58e1f1 400 time+=0.01;
DeguNaoto 83:e1638c58e1f1 401 #endif
DeguNaoto 4:51d87d2b698c 402 }
DeguNaoto 55:db1797ac6cb1 403 /***update state***/
DeguNaoto 57:3fbd487e055e 404 #ifdef IM920
DeguNaoto 51:cb430192b28b 405 readIM920();
DeguNaoto 57:3fbd487e055e 406 #endif
DeguNaoto 48:64d005c70df2 407 mesureSwing();
DeguNaoto 0:bd4719e15f7e 408 wait(RATE);
DeguNaoto 108:7eb434cfcbd7 409 // pc.printf("%f\r\n",(float)((((2.0 * PI) / swingRadVelocity) / 4.0)));
DeguNaoto 56:ac01d6b46291 410 // 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 411 // pc.printf("A2 = %d, X = %d, Y = %d, B = %d, dead = %d\r\n", a2, X, Y, b, deadflag);
DeguNaoto 100:5769dc0e7478 412 // pc.printf("%f %f %f\r\n",cont,swingRadVelocity, (double)SwingSens.getPulses());
DeguNaoto 0:bd4719e15f7e 413 }
DeguNaoto 0:bd4719e15f7e 414 }