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@114:325e4c158141, 2015-10-27 (annotated)

Committer:
DeguNaoto
Date:
Tue Oct 27 07:49:43 2015 +0000
Revision:
114:325e4c158141
Parent:
112:717acc7c99f9 
??????????; ????ver

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