ロボステ6期 / Mbed 2 deprecated NHK2020-arm-sub4

12/20 mainmode3

Dependencies:   mbed Encoder_

main.cpp@3:9cef6e58a462, 2019-12-20 (annotated)

Committer:
aoikoizumi
Date:
Fri Dec 20 10:21:47 2019 +0000
Revision:
3:9cef6e58a462
Parent:
2:90b53995cb1f 
testmode_3 12/20

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki0701 0:6e2abd0956f1 1 #include "mbed.h"
yuki0701 0:6e2abd0956f1 2 #include "EC.h" //Encoderライブラリをインクルード
yuki0701 0:6e2abd0956f1 3 #include "SpeedController.h"
yuki0701 0:6e2abd0956f1 4 #define RESOLUTION 2048//分解能
yuki0701 0:6e2abd0956f1 5
yuki0701 0:6e2abd0956f1 6 //#define NHK2020_TEST_MODE
yuki0701 0:6e2abd0956f1 7 #define NHK2020_MAIN_MODE
aoikoizumi 3:9cef6e58a462 8 //#define NHK2020_MAIN_MODE2
yuki0701 0:6e2abd0956f1 9 CAN can1(p30,p29);
yuki0701 0:6e2abd0956f1 10
yuki0701 0:6e2abd0956f1 11 Ticker can_ticker; //can用ticker
yuki0701 0:6e2abd0956f1 12
yuki0701 0:6e2abd0956f1 13 Ec4multi EC_backdrop(p15,p16,RESOLUTION);
yuki0701 0:6e2abd0956f1 14 SpeedControl backdrop(p21,p22,50,EC_backdrop);
yuki0701 0:6e2abd0956f1 15 Serial pc(USBTX,USBRX);
yuki0701 0:6e2abd0956f1 16 DigitalOut snatch(p8);
yuki0701 0:6e2abd0956f1 17 DigitalOut pass1(p27);
yuki0701 0:6e2abd0956f1 18 DigitalOut pass2(p28);
aoikoizumi 3:9cef6e58a462 19 DigitalOut pakapaka1(p14);
yuki0701 0:6e2abd0956f1 20 //Ticker motor_tick; //角速度計算用ticker
yuki0701 0:6e2abd0956f1 21
yuki0701 0:6e2abd0956f1 22 char t2[1]= {0}; //動作番号（送信する値（char型））
yuki0701 0:6e2abd0956f1 23 int t2_r=0, T2=0; //動作番号（受け取った値、送信する値（int型））
yuki0701 0:6e2abd0956f1 24
yuki0701 0:6e2abd0956f1 25 double a=0;//now
yuki0701 0:6e2abd0956f1 26 double b=0;//target
yuki0701 0:6e2abd0956f1 27 double turn=0;
yuki0701 0:6e2abd0956f1 28 int k = 0;
yuki0701 0:6e2abd0956f1 29 void tsukami()
yuki0701 0:6e2abd0956f1 30 {
yuki0701 0:6e2abd0956f1 31 b=1.3;
yuki0701 0:6e2abd0956f1 32 }
yuki0701 0:6e2abd0956f1 33 void put()
yuki0701 0:6e2abd0956f1 34 {
yuki0701 0:6e2abd0956f1 35 b=-0.7;
yuki0701 0:6e2abd0956f1 36 }
yuki0701 0:6e2abd0956f1 37 void top()
yuki0701 0:6e2abd0956f1 38 {
yuki0701 0:6e2abd0956f1 39 b=0;
yuki0701 0:6e2abd0956f1 40 }
aoikoizumi 1:bc34fdc4e16b 41 void move_motor()
yuki0701 0:6e2abd0956f1 42 {
aoikoizumi 1:bc34fdc4e16b 43 while(1) {
aoikoizumi 1:bc34fdc4e16b 44 double old_turn=turn;
aoikoizumi 1:bc34fdc4e16b 45 a=EC_backdrop.getRad();
aoikoizumi 1:bc34fdc4e16b 46 if(a-b>=0.1) {
aoikoizumi 1:bc34fdc4e16b 47 turn=0.1;
aoikoizumi 1:bc34fdc4e16b 48 pc.printf("F");
aoikoizumi 1:bc34fdc4e16b 49 } else if (a-b>=0.05) {
aoikoizumi 1:bc34fdc4e16b 50 turn=10*(a-b)*(a-b);
aoikoizumi 1:bc34fdc4e16b 51 pc.printf("f");
aoikoizumi 1:bc34fdc4e16b 52 } else if (b-a>=0.1) {
aoikoizumi 1:bc34fdc4e16b 53 turn=-0.1;
aoikoizumi 1:bc34fdc4e16b 54 pc.printf("B");
aoikoizumi 1:bc34fdc4e16b 55 } else if (b-a>=0.05) {
aoikoizumi 1:bc34fdc4e16b 56 turn=-10*(a-b)*(a-b);
aoikoizumi 1:bc34fdc4e16b 57 pc.printf("b");
aoikoizumi 1:bc34fdc4e16b 58 } else {
aoikoizumi 1:bc34fdc4e16b 59 backdrop.stop();
aoikoizumi 1:bc34fdc4e16b 60 backdrop.turn(0);
aoikoizumi 1:bc34fdc4e16b 61 turn=0;
aoikoizumi 1:bc34fdc4e16b 62 pc.printf("s");
aoikoizumi 1:bc34fdc4e16b 63 }
aoikoizumi 1:bc34fdc4e16b 64 if(turn*old_turn<0)turn=0;
aoikoizumi 1:bc34fdc4e16b 65 backdrop.turn(turn);
aoikoizumi 1:bc34fdc4e16b 66 pc.printf("%lf",EC_backdrop.getRad());
aoikoizumi 1:bc34fdc4e16b 67 if(b-a<0.05&&a-b<0.05)break;
yuki0701 0:6e2abd0956f1 68 }
yuki0701 0:6e2abd0956f1 69 }
yuki0701 0:6e2abd0956f1 70
yuki0701 0:6e2abd0956f1 71 int q = 0;
yuki0701 0:6e2abd0956f1 72
yuki0701 0:6e2abd0956f1 73 void can_read()
yuki0701 0:6e2abd0956f1 74 {
yuki0701 0:6e2abd0956f1 75
yuki0701 0:6e2abd0956f1 76
yuki0701 0:6e2abd0956f1 77 CANMessage msg;
yuki0701 0:6e2abd0956f1 78
yuki0701 0:6e2abd0956f1 79 if(can1.read(msg)) {
yuki0701 0:6e2abd0956f1 80
yuki0701 0:6e2abd0956f1 81 if(msg.id == 1) {
yuki0701 0:6e2abd0956f1 82 t2_r = msg.data[0];
yuki0701 0:6e2abd0956f1 83 //printf("arm T2 = %d t2_r = %d\n\r",T2,t2_r);
yuki0701 0:6e2abd0956f1 84 } else {
yuki0701 0:6e2abd0956f1 85 if(q > 100) {
yuki0701 0:6e2abd0956f1 86 //printf("id1 fale\n\r");
yuki0701 0:6e2abd0956f1 87 q = 0;
yuki0701 0:6e2abd0956f1 88 }
yuki0701 0:6e2abd0956f1 89 q++;
yuki0701 0:6e2abd0956f1 90 }
yuki0701 0:6e2abd0956f1 91
yuki0701 0:6e2abd0956f1 92 } else {
yuki0701 0:6e2abd0956f1 93 if(k > 100) {
yuki0701 0:6e2abd0956f1 94 //printf("arm fale\n\r");
yuki0701 0:6e2abd0956f1 95 k = 0;
yuki0701 0:6e2abd0956f1 96 }
yuki0701 0:6e2abd0956f1 97 k++;
yuki0701 0:6e2abd0956f1 98 }
yuki0701 0:6e2abd0956f1 99
yuki0701 0:6e2abd0956f1 100 t2[0] = T2;
yuki0701 0:6e2abd0956f1 101
yuki0701 0:6e2abd0956f1 102 if(can1.write(CANMessage(2,t2,1))) {
yuki0701 0:6e2abd0956f1 103 }
yuki0701 0:6e2abd0956f1 104
yuki0701 0:6e2abd0956f1 105 if(t2_r > T2) {
yuki0701 0:6e2abd0956f1 106 T2 = t2_r;
yuki0701 0:6e2abd0956f1 107 }
yuki0701 0:6e2abd0956f1 108
yuki0701 0:6e2abd0956f1 109
yuki0701 0:6e2abd0956f1 110 }
yuki0701 0:6e2abd0956f1 111
yuki0701 0:6e2abd0956f1 112
yuki0701 0:6e2abd0956f1 113 int main()
yuki0701 0:6e2abd0956f1 114 {
yuki0701 0:6e2abd0956f1 115 pc.printf("setting please");
aoikoizumi 1:bc34fdc4e16b 116 move_motor();
yuki0701 0:6e2abd0956f1 117 while(1) {
yuki0701 0:6e2abd0956f1 118
yuki0701 0:6e2abd0956f1 119
yuki0701 0:6e2abd0956f1 120
yuki0701 0:6e2abd0956f1 121
yuki0701 0:6e2abd0956f1 122 #ifdef NHK2020_TEST_MODE
aoikoizumi 2:90b53995cb1f 123 printf("T2 = %d\n\r",T2);
yuki0701 0:6e2abd0956f1 124 pc.printf("%lf",EC_backdrop.getRad());
yuki0701 0:6e2abd0956f1 125 if(pc.readable()) {
yuki0701 0:6e2abd0956f1 126 char sel=pc.getc();
yuki0701 0:6e2abd0956f1 127 if(sel=='z') {
yuki0701 0:6e2abd0956f1 128 pc.printf("z\r\n");
yuki0701 0:6e2abd0956f1 129 tsukami();
yuki0701 0:6e2abd0956f1 130 } else if(sel=='x') {
yuki0701 0:6e2abd0956f1 131 pc.printf("x\r\n");
yuki0701 0:6e2abd0956f1 132 put();
yuki0701 0:6e2abd0956f1 133 } else if(sel=='c') {
yuki0701 0:6e2abd0956f1 134 pc.printf("c\r\n");
yuki0701 0:6e2abd0956f1 135 top();
yuki0701 0:6e2abd0956f1 136 }
yuki0701 0:6e2abd0956f1 137 /* if(sel=='q') {
yuki0701 0:6e2abd0956f1 138 printf("\r\n");
yuki0701 0:6e2abd0956f1 139 if(denjiben==0)denjiben=1;
yuki0701 0:6e2abd0956f1 140 else denjiben=0;
yuki0701 0:6e2abd0956f1 141 }*/
yuki0701 0:6e2abd0956f1 142 if(sel=='1') {
yuki0701 0:6e2abd0956f1 143 snatch=0;
yuki0701 0:6e2abd0956f1 144 printf("snatch_off\r\n");
yuki0701 0:6e2abd0956f1 145 }
yuki0701 0:6e2abd0956f1 146 if(sel=='2') {
yuki0701 0:6e2abd0956f1 147 snatch=1;
yuki0701 0:6e2abd0956f1 148 printf("snatch_on\r\n");
yuki0701 0:6e2abd0956f1 149 }
yuki0701 0:6e2abd0956f1 150 if(sel=='3') {
yuki0701 0:6e2abd0956f1 151 pass1=0;
yuki0701 0:6e2abd0956f1 152 printf("pass1_off\r\n");
yuki0701 0:6e2abd0956f1 153 }
yuki0701 0:6e2abd0956f1 154 if(sel=='4') {
yuki0701 0:6e2abd0956f1 155 pass1=1;
yuki0701 0:6e2abd0956f1 156 printf("pass1_on\r\n");
yuki0701 0:6e2abd0956f1 157 }
yuki0701 0:6e2abd0956f1 158 if(sel=='5') {
yuki0701 0:6e2abd0956f1 159 pass2=0;
yuki0701 0:6e2abd0956f1 160 printf("pass2_off\r\n");
yuki0701 0:6e2abd0956f1 161 }
yuki0701 0:6e2abd0956f1 162 if(sel=='6') {
yuki0701 0:6e2abd0956f1 163 pass2=1;
yuki0701 0:6e2abd0956f1 164 printf("pass2_on\r\n");
yuki0701 0:6e2abd0956f1 165 }
yuki0701 0:6e2abd0956f1 166 // if(sel=='a') {
yuki0701 0:6e2abd0956f1 167 // //pc.printf("x\r\n");
yuki0701 0:6e2abd0956f1 168 // //put();
yuki0701 0:6e2abd0956f1 169 // }
yuki0701 0:6e2abd0956f1 170 }
yuki0701 0:6e2abd0956f1 171 }
yuki0701 0:6e2abd0956f1 172 #endif
yuki0701 0:6e2abd0956f1 173 #ifdef NHK2020_MAIN_MODE
yuki0701 0:6e2abd0956f1 174 can1.frequency(1000000);
yuki0701 0:6e2abd0956f1 175 can_ticker.attach(&can_read,0.01);
yuki0701 0:6e2abd0956f1 176 printf("wait_mode\r\n");
aoikoizumi 2:90b53995cb1f 177 //printf("T2 = %d\n\r",T2);
aoikoizumi 2:90b53995cb1f 178 if(T2 == 0) {
aoikoizumi 2:90b53995cb1f 179 while(1) {
aoikoizumi 2:90b53995cb1f 180 char sel=pc.getc();
aoikoizumi 2:90b53995cb1f 181 if(sel=='1') {
aoikoizumi 2:90b53995cb1f 182 snatch=0;
aoikoizumi 2:90b53995cb1f 183 printf("snatch_off\r\n");
aoikoizumi 2:90b53995cb1f 184 }
aoikoizumi 2:90b53995cb1f 185 if(sel=='2') {
aoikoizumi 2:90b53995cb1f 186 snatch=1;
aoikoizumi 2:90b53995cb1f 187 printf("snatch_on\r\n");
aoikoizumi 2:90b53995cb1f 188 }
aoikoizumi 2:90b53995cb1f 189 if(sel=='3') {
aoikoizumi 2:90b53995cb1f 190 pass1=0;
aoikoizumi 2:90b53995cb1f 191 printf("pass1_off\r\n");
aoikoizumi 2:90b53995cb1f 192 }
aoikoizumi 2:90b53995cb1f 193 if(sel=='4') {
aoikoizumi 2:90b53995cb1f 194 pass1=1;
aoikoizumi 2:90b53995cb1f 195 printf("pass1_on\r\n");
aoikoizumi 2:90b53995cb1f 196 }
aoikoizumi 2:90b53995cb1f 197 if(sel=='5') {
aoikoizumi 2:90b53995cb1f 198 pass2=0;
aoikoizumi 2:90b53995cb1f 199 printf("pass2_off\r\n");
aoikoizumi 2:90b53995cb1f 200 }
aoikoizumi 2:90b53995cb1f 201 if(sel=='6') {
aoikoizumi 2:90b53995cb1f 202 pass2=1;
aoikoizumi 2:90b53995cb1f 203 printf("pass2_on\r\n");
aoikoizumi 2:90b53995cb1f 204 }
aoikoizumi 2:90b53995cb1f 205 if(sel=='q') {
aoikoizumi 2:90b53995cb1f 206 printf("end_wait_mode\r\n");
aoikoizumi 2:90b53995cb1f 207 T2=1;
aoikoizumi 2:90b53995cb1f 208 break;
yuki0701 0:6e2abd0956f1 209
aoikoizumi 2:90b53995cb1f 210 }
yuki0701 0:6e2abd0956f1 211 }
yuki0701 0:6e2abd0956f1 212 }
aoikoizumi 2:90b53995cb1f 213 if(T2 == 1) { //スタート位置からボール前まで移動（アーム待機）
aoikoizumi 2:90b53995cb1f 214 //T2=1;
yuki0701 0:6e2abd0956f1 215 while(1) {
aoikoizumi 2:90b53995cb1f 216 printf("a-T = 0 T2 = %d t2_r = %d\n\r",T2,t2_r);
yuki0701 0:6e2abd0956f1 217 wait(0.1);
aoikoizumi 2:90b53995cb1f 218 if(T2 == 2) {
yuki0701 0:6e2abd0956f1 219 break;
yuki0701 0:6e2abd0956f1 220 }
yuki0701 0:6e2abd0956f1 221 }
yuki0701 0:6e2abd0956f1 222 }
yuki0701 0:6e2abd0956f1 223
aoikoizumi 2:90b53995cb1f 224 if(T2 == 2) { //ボール掴んで投げる
yuki0701 0:6e2abd0956f1 225 //printf("a-T = 1 T2 = %d t2_r = %d\n\r",T2,t2_r);
aoikoizumi 1:bc34fdc4e16b 226 snatch=1;
yuki0701 0:6e2abd0956f1 227 tsukami();
aoikoizumi 1:bc34fdc4e16b 228 printf("tsukami");
aoikoizumi 1:bc34fdc4e16b 229 move_motor();
yuki0701 0:6e2abd0956f1 230 wait(3);
yuki0701 0:6e2abd0956f1 231 snatch=0;
yuki0701 0:6e2abd0956f1 232 wait(1);
yuki0701 0:6e2abd0956f1 233 put();
aoikoizumi 1:bc34fdc4e16b 234 printf("put");
aoikoizumi 1:bc34fdc4e16b 235 move_motor();
yuki0701 0:6e2abd0956f1 236 wait(3);
yuki0701 0:6e2abd0956f1 237 snatch=1;
yuki0701 0:6e2abd0956f1 238 wait(1);
yuki0701 0:6e2abd0956f1 239 top();
aoikoizumi 1:bc34fdc4e16b 240 printf("top");
aoikoizumi 1:bc34fdc4e16b 241 move_motor();
yuki0701 0:6e2abd0956f1 242 wait(5);
yuki0701 0:6e2abd0956f1 243 pass1=0;
yuki0701 0:6e2abd0956f1 244 wait(5);
yuki0701 0:6e2abd0956f1 245 //wait(5);
yuki0701 0:6e2abd0956f1 246
yuki0701 0:6e2abd0956f1 247 T2++;
yuki0701 0:6e2abd0956f1 248 }
aoikoizumi 2:90b53995cb1f 249 if(T2 == 3) { //スタートゾーンに戻る（アーム待機）
yuki0701 0:6e2abd0956f1 250 while(1) {
aoikoizumi 2:90b53995cb1f 251 printf("a-T = 2 T2 = %d t2_r = %d\n\r",T2,t2_r);
yuki0701 0:6e2abd0956f1 252 wait(0.1);
aoikoizumi 2:90b53995cb1f 253 if(T2 == 4) {
yuki0701 0:6e2abd0956f1 254 break;
yuki0701 0:6e2abd0956f1 255 }
yuki0701 0:6e2abd0956f1 256 }
yuki0701 0:6e2abd0956f1 257 }
yuki0701 0:6e2abd0956f1 258 printf("end\n\r");
yuki0701 0:6e2abd0956f1 259 }
yuki0701 0:6e2abd0956f1 260 #endif
aoikoizumi 3:9cef6e58a462 261 #ifdef NHK2020_MAIN_MODE2
aoikoizumi 3:9cef6e58a462 262 can1.frequency(1000000);
aoikoizumi 3:9cef6e58a462 263 can_ticker.attach(&can_read,0.01);
aoikoizumi 3:9cef6e58a462 264 //printf("T2 = %d\n\r",T2);
aoikoizumi 3:9cef6e58a462 265 if(T2 == 0)
aoikoizumi 3:9cef6e58a462 266 {
aoikoizumi 3:9cef6e58a462 267 //T2=1;
aoikoizumi 3:9cef6e58a462 268 while(1) {
aoikoizumi 3:9cef6e58a462 269 if(T2 == 1) {
aoikoizumi 3:9cef6e58a462 270 break;
aoikoizumi 3:9cef6e58a462 271 }
aoikoizumi 3:9cef6e58a462 272 }
aoikoizumi 3:9cef6e58a462 273 }
aoikoizumi 3:9cef6e58a462 274 if(T2 == 1) //スタート位置からボール前まで移動（アーム待機）
aoikoizumi 3:9cef6e58a462 275 {
aoikoizumi 3:9cef6e58a462 276 //T2=2;
aoikoizumi 3:9cef6e58a462 277 while(1) {
aoikoizumi 3:9cef6e58a462 278 if(T2 == 2) {
aoikoizumi 3:9cef6e58a462 279 break;
aoikoizumi 3:9cef6e58a462 280 }
aoikoizumi 3:9cef6e58a462 281 }
aoikoizumi 3:9cef6e58a462 282 }
aoikoizumi 3:9cef6e58a462 283
aoikoizumi 3:9cef6e58a462 284 if(T2 == 2) //ボール掴んで投げる
aoikoizumi 3:9cef6e58a462 285 {
aoikoizumi 3:9cef6e58a462 286 pakapaka=1;
aoikoizumi 3:9cef6e58a462 287 //T2=3;
aoikoizumi 3:9cef6e58a462 288 while(1) {
aoikoizumi 3:9cef6e58a462 289 if(T2 == 3) {
aoikoizumi 3:9cef6e58a462 290 break;
aoikoizumi 3:9cef6e58a462 291 }
aoikoizumi 3:9cef6e58a462 292 }
aoikoizumi 3:9cef6e58a462 293 }
aoikoizumi 3:9cef6e58a462 294 if(T2 == 3) //スタートゾーンに戻る（アーム待機）
aoikoizumi 3:9cef6e58a462 295 {
aoikoizumi 3:9cef6e58a462 296 //T2=4;
aoikoizumi 3:9cef6e58a462 297 while(1) {
aoikoizumi 3:9cef6e58a462 298 if(T2 == 4) {
aoikoizumi 3:9cef6e58a462 299 break;
aoikoizumi 3:9cef6e58a462 300 }
aoikoizumi 3:9cef6e58a462 301 }
aoikoizumi 3:9cef6e58a462 302 }
aoikoizumi 3:9cef6e58a462 303 if(T2 == 4) //スタート位置からボール前まで移動（アーム待機）
aoikoizumi 3:9cef6e58a462 304 {
aoikoizumi 3:9cef6e58a462 305 pakapaka=0;
aoikoizumi 3:9cef6e58a462 306 //T2=5;
aoikoizumi 3:9cef6e58a462 307 while(1) {
aoikoizumi 3:9cef6e58a462 308 if(T2 == 5) {
aoikoizumi 3:9cef6e58a462 309 break;
aoikoizumi 3:9cef6e58a462 310 }
aoikoizumi 3:9cef6e58a462 311 }
aoikoizumi 3:9cef6e58a462 312 }
aoikoizumi 3:9cef6e58a462 313
aoikoizumi 3:9cef6e58a462 314 if(T2 == 5) //ボール掴んで投げる
aoikoizumi 3:9cef6e58a462 315 {
aoikoizumi 3:9cef6e58a462 316 //T2=6;
aoikoizumi 3:9cef6e58a462 317 while(1) {
aoikoizumi 3:9cef6e58a462 318 if(T2 == 6) {
aoikoizumi 3:9cef6e58a462 319 break;
aoikoizumi 3:9cef6e58a462 320 }
aoikoizumi 3:9cef6e58a462 321 }
aoikoizumi 3:9cef6e58a462 322 }
aoikoizumi 3:9cef6e58a462 323 if(T2 == 6) //スタートゾーンに戻る（アーム待機）
aoikoizumi 3:9cef6e58a462 324 {
aoikoizumi 3:9cef6e58a462 325 //T2=7;
aoikoizumi 3:9cef6e58a462 326 while(1) {
aoikoizumi 3:9cef6e58a462 327 if(T2 == 7) {
aoikoizumi 3:9cef6e58a462 328 break;
aoikoizumi 3:9cef6e58a462 329 }
aoikoizumi 3:9cef6e58a462 330 }
aoikoizumi 3:9cef6e58a462 331 }
aoikoizumi 3:9cef6e58a462 332 if(T2 == 7) //スタート位置からボール前まで移動（アーム待機）
aoikoizumi 3:9cef6e58a462 333 {
aoikoizumi 3:9cef6e58a462 334 //T2=8;
aoikoizumi 3:9cef6e58a462 335 while(1) {
aoikoizumi 3:9cef6e58a462 336 if(T2 == 8) {
aoikoizumi 3:9cef6e58a462 337 break;
aoikoizumi 3:9cef6e58a462 338 }
aoikoizumi 3:9cef6e58a462 339 }
aoikoizumi 3:9cef6e58a462 340 }
aoikoizumi 3:9cef6e58a462 341
aoikoizumi 3:9cef6e58a462 342 if(T2 == 8) //ボール掴んで投げる
aoikoizumi 3:9cef6e58a462 343 {
aoikoizumi 3:9cef6e58a462 344 //T2=9;
aoikoizumi 3:9cef6e58a462 345 while(1) {
aoikoizumi 3:9cef6e58a462 346 if(T2 == 9) {
aoikoizumi 3:9cef6e58a462 347 break;
aoikoizumi 3:9cef6e58a462 348 }
aoikoizumi 3:9cef6e58a462 349 }
aoikoizumi 3:9cef6e58a462 350 }
aoikoizumi 3:9cef6e58a462 351 if(T2 == 9) //スタートゾーンに戻る（アーム待機）
aoikoizumi 3:9cef6e58a462 352 {
aoikoizumi 3:9cef6e58a462 353 //T2=10;
aoikoizumi 3:9cef6e58a462 354 while(1) {
aoikoizumi 3:9cef6e58a462 355 if(T2 == 10) {
aoikoizumi 3:9cef6e58a462 356 break;
aoikoizumi 3:9cef6e58a462 357 }
aoikoizumi 3:9cef6e58a462 358 }
aoikoizumi 3:9cef6e58a462 359 }
aoikoizumi 3:9cef6e58a462 360
aoikoizumi 3:9cef6e58a462 361 #endif
aoikoizumi 3:9cef6e58a462 362 #ifdef NHK2020_MAIN_MODE3
aoikoizumi 3:9cef6e58a462 363 can1.frequency(1000000);
aoikoizumi 3:9cef6e58a462 364 can_ticker.attach(&can_read,0.01);
aoikoizumi 3:9cef6e58a462 365 //printf("T2 = %d\n\r",T2);
aoikoizumi 3:9cef6e58a462 366 if(T2 == 0)
aoikoizumi 3:9cef6e58a462 367 {
aoikoizumi 3:9cef6e58a462 368 //T2=1;
aoikoizumi 3:9cef6e58a462 369 while(1) {
aoikoizumi 3:9cef6e58a462 370 if(T2 == 1) {
aoikoizumi 3:9cef6e58a462 371 break;
aoikoizumi 3:9cef6e58a462 372 }
aoikoizumi 3:9cef6e58a462 373 }
aoikoizumi 3:9cef6e58a462 374 }
aoikoizumi 3:9cef6e58a462 375 if(T2 == 1) //スタート位置からボール前まで移動（アーム待機）
aoikoizumi 3:9cef6e58a462 376 {
aoikoizumi 3:9cef6e58a462 377 //T2=2;
aoikoizumi 3:9cef6e58a462 378 while(1) {
aoikoizumi 3:9cef6e58a462 379 if(T2 == 2) {
aoikoizumi 3:9cef6e58a462 380 break;
aoikoizumi 3:9cef6e58a462 381 }
aoikoizumi 3:9cef6e58a462 382 }
aoikoizumi 3:9cef6e58a462 383 }
aoikoizumi 3:9cef6e58a462 384
aoikoizumi 3:9cef6e58a462 385 if(T2 == 2) //ボール掴んで投げる
aoikoizumi 3:9cef6e58a462 386 {
aoikoizumi 3:9cef6e58a462 387 pakapaka=1;
aoikoizumi 3:9cef6e58a462 388 //T2=3;
aoikoizumi 3:9cef6e58a462 389 while(1) {
aoikoizumi 3:9cef6e58a462 390 if(T2 == 3) {
aoikoizumi 3:9cef6e58a462 391 break;
aoikoizumi 3:9cef6e58a462 392 }
aoikoizumi 3:9cef6e58a462 393 }
aoikoizumi 3:9cef6e58a462 394 }
aoikoizumi 3:9cef6e58a462 395 if(T2 == 3) //スタートゾーンに戻る（アーム待機）
aoikoizumi 3:9cef6e58a462 396 {
aoikoizumi 3:9cef6e58a462 397 //T2=4;
aoikoizumi 3:9cef6e58a462 398 while(1) {
aoikoizumi 3:9cef6e58a462 399 if(T2 == 4) {
aoikoizumi 3:9cef6e58a462 400 break;
aoikoizumi 3:9cef6e58a462 401 }
aoikoizumi 3:9cef6e58a462 402 }
aoikoizumi 3:9cef6e58a462 403 }
aoikoizumi 3:9cef6e58a462 404 if(T2 == 4) //スタート位置からボール前まで移動（アーム待機）
aoikoizumi 3:9cef6e58a462 405 {
aoikoizumi 3:9cef6e58a462 406 pakapaka=0;
aoikoizumi 3:9cef6e58a462 407 //T2=5;
aoikoizumi 3:9cef6e58a462 408 while(1) {
aoikoizumi 3:9cef6e58a462 409 if(T2 == 5) {
aoikoizumi 3:9cef6e58a462 410 break;
aoikoizumi 3:9cef6e58a462 411 }
aoikoizumi 3:9cef6e58a462 412 }
aoikoizumi 3:9cef6e58a462 413 }
aoikoizumi 3:9cef6e58a462 414
aoikoizumi 3:9cef6e58a462 415
aoikoizumi 3:9cef6e58a462 416 #endif
aoikoizumi 3:9cef6e58a462 417
aoikoizumi 3:9cef6e58a462 418
aoikoizumi 3:9cef6e58a462 419
aoikoizumi 3:9cef6e58a462 420
yuki0701 0:6e2abd0956f1 421 }