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

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:   mbed KondoServoLibrary ros_lib_kinetic

main.cpp@4:f1f638a6b6c5, 2019-12-24 (annotated)

Committer:
yuki0701
Date:
Tue Dec 24 10:24:35 2019 +0000
Revision:
4:f1f638a6b6c5
Parent:
3:9cef6e58a462
Child:
5:f9dd9346bfd4
a

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 4:f1f638a6b6c5 7 //#define NHK2020_MAIN_MODE
yuki0701 4:f1f638a6b6c5 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 4:f1f638a6b6c5 14 SpeedControl backdrop(p22,p21,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 4:f1f638a6b6c5 117
yuki0701 0:6e2abd0956f1 118
yuki0701 0:6e2abd0956f1 119
yuki0701 0:6e2abd0956f1 120
yuki0701 0:6e2abd0956f1 121
yuki0701 0:6e2abd0956f1 122 #ifdef NHK2020_TEST_MODE
yuki0701 4:f1f638a6b6c5 123 while(1) {
aoikoizumi 2:90b53995cb1f 124 printf("T2 = %d\n\r",T2);
yuki0701 0:6e2abd0956f1 125 pc.printf("%lf",EC_backdrop.getRad());
yuki0701 0:6e2abd0956f1 126 if(pc.readable()) {
yuki0701 0:6e2abd0956f1 127 char sel=pc.getc();
yuki0701 0:6e2abd0956f1 128 if(sel=='z') {
yuki0701 0:6e2abd0956f1 129 pc.printf("z\r\n");
yuki0701 0:6e2abd0956f1 130 tsukami();
yuki0701 0:6e2abd0956f1 131 } else if(sel=='x') {
yuki0701 0:6e2abd0956f1 132 pc.printf("x\r\n");
yuki0701 0:6e2abd0956f1 133 put();
yuki0701 0:6e2abd0956f1 134 } else if(sel=='c') {
yuki0701 0:6e2abd0956f1 135 pc.printf("c\r\n");
yuki0701 0:6e2abd0956f1 136 top();
yuki0701 0:6e2abd0956f1 137 }
yuki0701 0:6e2abd0956f1 138 /* if(sel=='q') {
yuki0701 0:6e2abd0956f1 139 printf("\r\n");
yuki0701 0:6e2abd0956f1 140 if(denjiben==0)denjiben=1;
yuki0701 0:6e2abd0956f1 141 else denjiben=0;
yuki0701 0:6e2abd0956f1 142 }*/
yuki0701 0:6e2abd0956f1 143 if(sel=='1') {
yuki0701 0:6e2abd0956f1 144 snatch=0;
yuki0701 0:6e2abd0956f1 145 printf("snatch_off\r\n");
yuki0701 0:6e2abd0956f1 146 }
yuki0701 0:6e2abd0956f1 147 if(sel=='2') {
yuki0701 0:6e2abd0956f1 148 snatch=1;
yuki0701 0:6e2abd0956f1 149 printf("snatch_on\r\n");
yuki0701 0:6e2abd0956f1 150 }
yuki0701 0:6e2abd0956f1 151 if(sel=='3') {
yuki0701 0:6e2abd0956f1 152 pass1=0;
yuki0701 0:6e2abd0956f1 153 printf("pass1_off\r\n");
yuki0701 0:6e2abd0956f1 154 }
yuki0701 0:6e2abd0956f1 155 if(sel=='4') {
yuki0701 0:6e2abd0956f1 156 pass1=1;
yuki0701 0:6e2abd0956f1 157 printf("pass1_on\r\n");
yuki0701 0:6e2abd0956f1 158 }
yuki0701 0:6e2abd0956f1 159 if(sel=='5') {
yuki0701 0:6e2abd0956f1 160 pass2=0;
yuki0701 0:6e2abd0956f1 161 printf("pass2_off\r\n");
yuki0701 0:6e2abd0956f1 162 }
yuki0701 0:6e2abd0956f1 163 if(sel=='6') {
yuki0701 0:6e2abd0956f1 164 pass2=1;
yuki0701 0:6e2abd0956f1 165 printf("pass2_on\r\n");
yuki0701 0:6e2abd0956f1 166 }
yuki0701 0:6e2abd0956f1 167 // if(sel=='a') {
yuki0701 0:6e2abd0956f1 168 // //pc.printf("x\r\n");
yuki0701 0:6e2abd0956f1 169 // //put();
yuki0701 0:6e2abd0956f1 170 // }
yuki0701 0:6e2abd0956f1 171 }
yuki0701 0:6e2abd0956f1 172 }
yuki0701 0:6e2abd0956f1 173 #endif
yuki0701 0:6e2abd0956f1 174 #ifdef NHK2020_MAIN_MODE
yuki0701 0:6e2abd0956f1 175 can1.frequency(1000000);
yuki0701 0:6e2abd0956f1 176 can_ticker.attach(&can_read,0.01);
yuki0701 0:6e2abd0956f1 177 printf("wait_mode\r\n");
aoikoizumi 2:90b53995cb1f 178 //printf("T2 = %d\n\r",T2);
aoikoizumi 2:90b53995cb1f 179 if(T2 == 0) {
aoikoizumi 2:90b53995cb1f 180 while(1) {
aoikoizumi 2:90b53995cb1f 181 char sel=pc.getc();
aoikoizumi 2:90b53995cb1f 182 if(sel=='1') {
aoikoizumi 2:90b53995cb1f 183 snatch=0;
aoikoizumi 2:90b53995cb1f 184 printf("snatch_off\r\n");
aoikoizumi 2:90b53995cb1f 185 }
aoikoizumi 2:90b53995cb1f 186 if(sel=='2') {
aoikoizumi 2:90b53995cb1f 187 snatch=1;
aoikoizumi 2:90b53995cb1f 188 printf("snatch_on\r\n");
aoikoizumi 2:90b53995cb1f 189 }
aoikoizumi 2:90b53995cb1f 190 if(sel=='3') {
aoikoizumi 2:90b53995cb1f 191 pass1=0;
aoikoizumi 2:90b53995cb1f 192 printf("pass1_off\r\n");
aoikoizumi 2:90b53995cb1f 193 }
aoikoizumi 2:90b53995cb1f 194 if(sel=='4') {
aoikoizumi 2:90b53995cb1f 195 pass1=1;
aoikoizumi 2:90b53995cb1f 196 printf("pass1_on\r\n");
aoikoizumi 2:90b53995cb1f 197 }
aoikoizumi 2:90b53995cb1f 198 if(sel=='5') {
aoikoizumi 2:90b53995cb1f 199 pass2=0;
aoikoizumi 2:90b53995cb1f 200 printf("pass2_off\r\n");
aoikoizumi 2:90b53995cb1f 201 }
aoikoizumi 2:90b53995cb1f 202 if(sel=='6') {
aoikoizumi 2:90b53995cb1f 203 pass2=1;
aoikoizumi 2:90b53995cb1f 204 printf("pass2_on\r\n");
aoikoizumi 2:90b53995cb1f 205 }
aoikoizumi 2:90b53995cb1f 206 if(sel=='q') {
aoikoizumi 2:90b53995cb1f 207 printf("end_wait_mode\r\n");
aoikoizumi 2:90b53995cb1f 208 T2=1;
aoikoizumi 2:90b53995cb1f 209 break;
yuki0701 0:6e2abd0956f1 210
aoikoizumi 2:90b53995cb1f 211 }
yuki0701 0:6e2abd0956f1 212 }
yuki0701 0:6e2abd0956f1 213 }
aoikoizumi 2:90b53995cb1f 214 if(T2 == 1) { //スタート位置からボール前まで移動（アーム待機）
aoikoizumi 2:90b53995cb1f 215 //T2=1;
yuki0701 4:f1f638a6b6c5 216 pass2=0;
yuki0701 0:6e2abd0956f1 217 while(1) {
aoikoizumi 2:90b53995cb1f 218 printf("a-T = 0 T2 = %d t2_r = %d\n\r",T2,t2_r);
yuki0701 0:6e2abd0956f1 219 wait(0.1);
aoikoizumi 2:90b53995cb1f 220 if(T2 == 2) {
yuki0701 0:6e2abd0956f1 221 break;
yuki0701 0:6e2abd0956f1 222 }
yuki0701 0:6e2abd0956f1 223 }
yuki0701 0:6e2abd0956f1 224 }
yuki0701 0:6e2abd0956f1 225
aoikoizumi 2:90b53995cb1f 226 if(T2 == 2) { //ボール掴んで投げる
yuki0701 0:6e2abd0956f1 227 //printf("a-T = 1 T2 = %d t2_r = %d\n\r",T2,t2_r);
aoikoizumi 1:bc34fdc4e16b 228 snatch=1;
yuki0701 0:6e2abd0956f1 229 tsukami();
aoikoizumi 1:bc34fdc4e16b 230 printf("tsukami");
aoikoizumi 1:bc34fdc4e16b 231 move_motor();
yuki0701 0:6e2abd0956f1 232 wait(3);
yuki0701 0:6e2abd0956f1 233 snatch=0;
yuki0701 0:6e2abd0956f1 234 wait(1);
yuki0701 0:6e2abd0956f1 235 put();
aoikoizumi 1:bc34fdc4e16b 236 printf("put");
aoikoizumi 1:bc34fdc4e16b 237 move_motor();
yuki0701 0:6e2abd0956f1 238 wait(3);
yuki0701 0:6e2abd0956f1 239 snatch=1;
yuki0701 0:6e2abd0956f1 240 wait(1);
yuki0701 0:6e2abd0956f1 241 top();
aoikoizumi 1:bc34fdc4e16b 242 printf("top");
aoikoizumi 1:bc34fdc4e16b 243 move_motor();
yuki0701 0:6e2abd0956f1 244 wait(5);
yuki0701 0:6e2abd0956f1 245 pass1=0;
yuki0701 0:6e2abd0956f1 246 wait(5);
yuki0701 0:6e2abd0956f1 247 //wait(5);
yuki0701 0:6e2abd0956f1 248
yuki0701 0:6e2abd0956f1 249 T2++;
yuki0701 0:6e2abd0956f1 250 }
aoikoizumi 2:90b53995cb1f 251 if(T2 == 3) { //スタートゾーンに戻る（アーム待機）
yuki0701 0:6e2abd0956f1 252 while(1) {
aoikoizumi 2:90b53995cb1f 253 printf("a-T = 2 T2 = %d t2_r = %d\n\r",T2,t2_r);
yuki0701 0:6e2abd0956f1 254 wait(0.1);
aoikoizumi 2:90b53995cb1f 255 if(T2 == 4) {
yuki0701 0:6e2abd0956f1 256 break;
yuki0701 0:6e2abd0956f1 257 }
yuki0701 0:6e2abd0956f1 258 }
yuki0701 0:6e2abd0956f1 259 }
yuki0701 0:6e2abd0956f1 260 printf("end\n\r");
yuki0701 4:f1f638a6b6c5 261
yuki0701 0:6e2abd0956f1 262 #endif
aoikoizumi 3:9cef6e58a462 263 #ifdef NHK2020_MAIN_MODE2
yuki0701 4:f1f638a6b6c5 264 can1.frequency(1000000);
yuki0701 4:f1f638a6b6c5 265 can_ticker.attach(&can_read,0.01);
aoikoizumi 3:9cef6e58a462 266 //printf("T2 = %d\n\r",T2);
yuki0701 4:f1f638a6b6c5 267 if(T2 == 0) {
yuki0701 4:f1f638a6b6c5 268 //T2=1;
yuki0701 4:f1f638a6b6c5 269 while(1) {
yuki0701 4:f1f638a6b6c5 270 char sel=pc.getc();
yuki0701 4:f1f638a6b6c5 271 if(sel=='1') {
yuki0701 4:f1f638a6b6c5 272 snatch=0;
yuki0701 4:f1f638a6b6c5 273 printf("snatch_off\r\n");
yuki0701 4:f1f638a6b6c5 274 }
yuki0701 4:f1f638a6b6c5 275 if(sel=='2') {
yuki0701 4:f1f638a6b6c5 276 snatch=1;
yuki0701 4:f1f638a6b6c5 277 printf("snatch_on\r\n");
yuki0701 4:f1f638a6b6c5 278 }
yuki0701 4:f1f638a6b6c5 279 if(sel=='3') {
yuki0701 4:f1f638a6b6c5 280 pass1=0;
yuki0701 4:f1f638a6b6c5 281 printf("pass1_off\r\n");
yuki0701 4:f1f638a6b6c5 282 }
yuki0701 4:f1f638a6b6c5 283 if(sel=='4') {
yuki0701 4:f1f638a6b6c5 284 pass1=1;
yuki0701 4:f1f638a6b6c5 285 printf("pass1_on\r\n");
yuki0701 4:f1f638a6b6c5 286 }
yuki0701 4:f1f638a6b6c5 287 if(sel=='5') {
yuki0701 4:f1f638a6b6c5 288 pass2=0;
yuki0701 4:f1f638a6b6c5 289 printf("pass2_off\r\n");
yuki0701 4:f1f638a6b6c5 290 }
yuki0701 4:f1f638a6b6c5 291 if(sel=='6') {
yuki0701 4:f1f638a6b6c5 292 pass2=1;
yuki0701 4:f1f638a6b6c5 293 printf("pass2_on\r\n");
yuki0701 4:f1f638a6b6c5 294 }
yuki0701 4:f1f638a6b6c5 295 if(sel=='q') {
yuki0701 4:f1f638a6b6c5 296 printf("end_wait_mode\r\n");
yuki0701 4:f1f638a6b6c5 297 T2=1;
yuki0701 4:f1f638a6b6c5 298 break;
yuki0701 4:f1f638a6b6c5 299
yuki0701 4:f1f638a6b6c5 300 }
aoikoizumi 3:9cef6e58a462 301 }
aoikoizumi 3:9cef6e58a462 302 }
yuki0701 4:f1f638a6b6c5 303 if(T2 == 1) { //スタート位置からボール前まで移動（アーム待機）
yuki0701 4:f1f638a6b6c5 304 //T2=2;
yuki0701 4:f1f638a6b6c5 305 pass2=0;
yuki0701 4:f1f638a6b6c5 306 while(1) {
yuki0701 4:f1f638a6b6c5 307 if(T2 == 2) {
yuki0701 4:f1f638a6b6c5 308 break;
yuki0701 4:f1f638a6b6c5 309 }
aoikoizumi 3:9cef6e58a462 310 }
aoikoizumi 3:9cef6e58a462 311 }
yuki0701 4:f1f638a6b6c5 312
yuki0701 4:f1f638a6b6c5 313 if(T2 == 2) { //ボール掴んで投げる
yuki0701 4:f1f638a6b6c5 314 //pakapaka=1;
yuki0701 4:f1f638a6b6c5 315 //T2=3;
aoikoizumi 3:9cef6e58a462 316
yuki0701 4:f1f638a6b6c5 317 while(1) {
yuki0701 4:f1f638a6b6c5 318 if(T2 == 3) {
yuki0701 4:f1f638a6b6c5 319 break;
yuki0701 4:f1f638a6b6c5 320 }
aoikoizumi 3:9cef6e58a462 321 }
aoikoizumi 3:9cef6e58a462 322 }
yuki0701 4:f1f638a6b6c5 323 if(T2 == 3) { //スタートゾーンに戻る（アーム待機）
yuki0701 4:f1f638a6b6c5 324 //T2=4;
yuki0701 4:f1f638a6b6c5 325 while(1) {
yuki0701 4:f1f638a6b6c5 326 if(T2 == 4) {
yuki0701 4:f1f638a6b6c5 327 break;
yuki0701 4:f1f638a6b6c5 328 }
yuki0701 4:f1f638a6b6c5 329 }
yuki0701 4:f1f638a6b6c5 330 }
yuki0701 4:f1f638a6b6c5 331 if(T2 == 4) { //kikk
yuki0701 4:f1f638a6b6c5 332 //T2=5;
yuki0701 4:f1f638a6b6c5 333 T2++;
yuki0701 4:f1f638a6b6c5 334 while(1) {
yuki0701 4:f1f638a6b6c5 335 if(T2 == 5) {
yuki0701 4:f1f638a6b6c5 336 break;
yuki0701 4:f1f638a6b6c5 337 }
yuki0701 4:f1f638a6b6c5 338 }
yuki0701 4:f1f638a6b6c5 339 }
yuki0701 4:f1f638a6b6c5 340
yuki0701 4:f1f638a6b6c5 341
yuki0701 4:f1f638a6b6c5 342 if(T2 == 5) { //ボール掴んで投げる
yuki0701 4:f1f638a6b6c5 343 while(1) {
yuki0701 4:f1f638a6b6c5 344 if(T2 == 6) {
yuki0701 4:f1f638a6b6c5 345 break;
yuki0701 4:f1f638a6b6c5 346 }
aoikoizumi 3:9cef6e58a462 347 }
aoikoizumi 3:9cef6e58a462 348 }
yuki0701 4:f1f638a6b6c5 349 if(T2 == 6) { //スタートゾーンに戻る（アーム待機）
yuki0701 4:f1f638a6b6c5 350 snatch=1;
yuki0701 4:f1f638a6b6c5 351 tsukami();
yuki0701 4:f1f638a6b6c5 352 printf("tsukami");
yuki0701 4:f1f638a6b6c5 353 move_motor();
yuki0701 4:f1f638a6b6c5 354 wait(3);
yuki0701 4:f1f638a6b6c5 355 snatch=0;
yuki0701 4:f1f638a6b6c5 356 wait(1);
yuki0701 4:f1f638a6b6c5 357 put();
yuki0701 4:f1f638a6b6c5 358 printf("put");
yuki0701 4:f1f638a6b6c5 359 move_motor();
yuki0701 4:f1f638a6b6c5 360 wait(3);
yuki0701 4:f1f638a6b6c5 361 snatch=1;
yuki0701 4:f1f638a6b6c5 362 wait(1);
yuki0701 4:f1f638a6b6c5 363 top();
yuki0701 4:f1f638a6b6c5 364 printf("top");
yuki0701 4:f1f638a6b6c5 365 move_motor();
yuki0701 4:f1f638a6b6c5 366 wait(5);
yuki0701 4:f1f638a6b6c5 367 pass1=0;
yuki0701 4:f1f638a6b6c5 368 wait(5);
yuki0701 4:f1f638a6b6c5 369
yuki0701 4:f1f638a6b6c5 370 pass1=1;
yuki0701 4:f1f638a6b6c5 371 pass2=1;
yuki0701 4:f1f638a6b6c5 372 //wait(5);
yuki0701 4:f1f638a6b6c5 373 T2++;
yuki0701 4:f1f638a6b6c5 374 //T2=7;
yuki0701 4:f1f638a6b6c5 375 while(1) {
yuki0701 4:f1f638a6b6c5 376 if(T2 == 7) {
yuki0701 4:f1f638a6b6c5 377 break;
yuki0701 4:f1f638a6b6c5 378 }
aoikoizumi 3:9cef6e58a462 379 }
aoikoizumi 3:9cef6e58a462 380 }
yuki0701 4:f1f638a6b6c5 381 if(T2 == 7) { //スタート位置からボール前まで移動（アーム待機）
yuki0701 4:f1f638a6b6c5 382 //T2=8;
aoikoizumi 3:9cef6e58a462 383
yuki0701 4:f1f638a6b6c5 384 pass2=0;
yuki0701 4:f1f638a6b6c5 385 while(1) {
yuki0701 4:f1f638a6b6c5 386 if(T2 == 8) {
yuki0701 4:f1f638a6b6c5 387 break;
yuki0701 4:f1f638a6b6c5 388 }
aoikoizumi 3:9cef6e58a462 389 }
aoikoizumi 3:9cef6e58a462 390 }
yuki0701 4:f1f638a6b6c5 391
yuki0701 4:f1f638a6b6c5 392 if(T2 == 8) { //ボール掴んで投げる
yuki0701 4:f1f638a6b6c5 393 snatch=1;
yuki0701 4:f1f638a6b6c5 394 tsukami();
yuki0701 4:f1f638a6b6c5 395 printf("tsukami");
yuki0701 4:f1f638a6b6c5 396 move_motor();
yuki0701 4:f1f638a6b6c5 397 wait(3);
yuki0701 4:f1f638a6b6c5 398 snatch=0;
yuki0701 4:f1f638a6b6c5 399 wait(1);
yuki0701 4:f1f638a6b6c5 400 put();
yuki0701 4:f1f638a6b6c5 401 printf("put");
yuki0701 4:f1f638a6b6c5 402 move_motor();
yuki0701 4:f1f638a6b6c5 403 wait(3);
yuki0701 4:f1f638a6b6c5 404 snatch=1;
yuki0701 4:f1f638a6b6c5 405 wait(1);
yuki0701 4:f1f638a6b6c5 406 top();
yuki0701 4:f1f638a6b6c5 407 printf("top");
yuki0701 4:f1f638a6b6c5 408 move_motor();
yuki0701 4:f1f638a6b6c5 409 wait(5);
yuki0701 4:f1f638a6b6c5 410 pass1=0;
yuki0701 4:f1f638a6b6c5 411 wait(5);
yuki0701 4:f1f638a6b6c5 412 //wait(5);
yuki0701 4:f1f638a6b6c5 413
yuki0701 4:f1f638a6b6c5 414 pass1=1;
yuki0701 4:f1f638a6b6c5 415 pass2=1;
yuki0701 4:f1f638a6b6c5 416
yuki0701 4:f1f638a6b6c5 417 T2++;
yuki0701 4:f1f638a6b6c5 418 //T2=9;
yuki0701 4:f1f638a6b6c5 419 while(1) {
yuki0701 4:f1f638a6b6c5 420 if(T2 == 9) {
yuki0701 4:f1f638a6b6c5 421 break;
yuki0701 4:f1f638a6b6c5 422 }
aoikoizumi 3:9cef6e58a462 423 }
aoikoizumi 3:9cef6e58a462 424 }
yuki0701 4:f1f638a6b6c5 425 if(T2 == 9) { //スタートゾーンに戻る（アーム待機）
yuki0701 4:f1f638a6b6c5 426 pass2=0;
yuki0701 4:f1f638a6b6c5 427 //T2=10;
yuki0701 4:f1f638a6b6c5 428 while(1) {
yuki0701 4:f1f638a6b6c5 429 if(T2 == 10) {
yuki0701 4:f1f638a6b6c5 430 break;
yuki0701 4:f1f638a6b6c5 431 }
aoikoizumi 3:9cef6e58a462 432 }
aoikoizumi 3:9cef6e58a462 433 }
yuki0701 4:f1f638a6b6c5 434 if(T2 == 10) { //ボール掴んで投げる
yuki0701 4:f1f638a6b6c5 435 snatch=1;
yuki0701 4:f1f638a6b6c5 436 tsukami();
yuki0701 4:f1f638a6b6c5 437 printf("tsukami");
yuki0701 4:f1f638a6b6c5 438 move_motor();
yuki0701 4:f1f638a6b6c5 439 wait(3);
yuki0701 4:f1f638a6b6c5 440 snatch=0;
yuki0701 4:f1f638a6b6c5 441 wait(1);
yuki0701 4:f1f638a6b6c5 442 put();
yuki0701 4:f1f638a6b6c5 443 printf("put");
yuki0701 4:f1f638a6b6c5 444 move_motor();
yuki0701 4:f1f638a6b6c5 445 wait(3);
yuki0701 4:f1f638a6b6c5 446 snatch=1;
yuki0701 4:f1f638a6b6c5 447 wait(1);
yuki0701 4:f1f638a6b6c5 448 top();
yuki0701 4:f1f638a6b6c5 449 printf("top");
yuki0701 4:f1f638a6b6c5 450 move_motor();
yuki0701 4:f1f638a6b6c5 451 wait(5);
yuki0701 4:f1f638a6b6c5 452 pass1=0;
yuki0701 4:f1f638a6b6c5 453 wait(5);
yuki0701 4:f1f638a6b6c5 454 //wait(5);
yuki0701 4:f1f638a6b6c5 455
yuki0701 4:f1f638a6b6c5 456 T2++;
yuki0701 4:f1f638a6b6c5 457 //T2=9;
yuki0701 4:f1f638a6b6c5 458 while(1) {
yuki0701 4:f1f638a6b6c5 459 if(T2 == 11) {
yuki0701 4:f1f638a6b6c5 460 break;
yuki0701 4:f1f638a6b6c5 461 }
aoikoizumi 3:9cef6e58a462 462 }
aoikoizumi 3:9cef6e58a462 463 }
yuki0701 4:f1f638a6b6c5 464 if(T2 == 11) { //スタートゾーンに戻る（アーム待機）
yuki0701 4:f1f638a6b6c5 465 //T2=10;
yuki0701 4:f1f638a6b6c5 466 while(1) {
yuki0701 4:f1f638a6b6c5 467 if(T2 == 12) {
yuki0701 4:f1f638a6b6c5 468 break;
yuki0701 4:f1f638a6b6c5 469 }
yuki0701 4:f1f638a6b6c5 470 }
yuki0701 4:f1f638a6b6c5 471 }
aoikoizumi 3:9cef6e58a462 472 #endif
aoikoizumi 3:9cef6e58a462 473 #ifdef NHK2020_MAIN_MODE3
yuki0701 4:f1f638a6b6c5 474 can1.frequency(1000000);
yuki0701 4:f1f638a6b6c5 475 can_ticker.attach(&can_read,0.01);
aoikoizumi 3:9cef6e58a462 476 //printf("T2 = %d\n\r",T2);
yuki0701 4:f1f638a6b6c5 477 if(T2 == 0) {
yuki0701 4:f1f638a6b6c5 478 //T2=1;
yuki0701 4:f1f638a6b6c5 479 while(1) {
yuki0701 4:f1f638a6b6c5 480 if(T2 == 1) {
yuki0701 4:f1f638a6b6c5 481 break;
yuki0701 4:f1f638a6b6c5 482 }
aoikoizumi 3:9cef6e58a462 483 }
aoikoizumi 3:9cef6e58a462 484 }
yuki0701 4:f1f638a6b6c5 485 if(T2 == 1) { //スタート位置からボール前まで移動（アーム待機）
yuki0701 4:f1f638a6b6c5 486 //T2=2;
yuki0701 4:f1f638a6b6c5 487 while(1) {
yuki0701 4:f1f638a6b6c5 488 if(T2 == 2) {
yuki0701 4:f1f638a6b6c5 489 break;
yuki0701 4:f1f638a6b6c5 490 }
aoikoizumi 3:9cef6e58a462 491 }
aoikoizumi 3:9cef6e58a462 492 }
aoikoizumi 3:9cef6e58a462 493
yuki0701 4:f1f638a6b6c5 494 if(T2 == 2) { //ボール掴んで投げる
yuki0701 4:f1f638a6b6c5 495 pakapaka=1;
yuki0701 4:f1f638a6b6c5 496 //T2=3;
yuki0701 4:f1f638a6b6c5 497 while(1) {
yuki0701 4:f1f638a6b6c5 498 if(T2 == 3) {
yuki0701 4:f1f638a6b6c5 499 break;
yuki0701 4:f1f638a6b6c5 500 }
aoikoizumi 3:9cef6e58a462 501 }
aoikoizumi 3:9cef6e58a462 502 }
yuki0701 4:f1f638a6b6c5 503 if(T2 == 3) { //スタートゾーンに戻る（アーム待機）
yuki0701 4:f1f638a6b6c5 504 //T2=4;
yuki0701 4:f1f638a6b6c5 505 while(1) {
yuki0701 4:f1f638a6b6c5 506 if(T2 == 4) {
yuki0701 4:f1f638a6b6c5 507 break;
yuki0701 4:f1f638a6b6c5 508 }
aoikoizumi 3:9cef6e58a462 509 }
aoikoizumi 3:9cef6e58a462 510 }
yuki0701 4:f1f638a6b6c5 511 if(T2 == 4) { //スタート位置からボール前まで移動（アーム待機）
yuki0701 4:f1f638a6b6c5 512 pakapaka=0;
yuki0701 4:f1f638a6b6c5 513 //T2=5;
yuki0701 4:f1f638a6b6c5 514 while(1) {
yuki0701 4:f1f638a6b6c5 515 if(T2 == 5) {
yuki0701 4:f1f638a6b6c5 516 break;
yuki0701 4:f1f638a6b6c5 517 }
aoikoizumi 3:9cef6e58a462 518 }
aoikoizumi 3:9cef6e58a462 519 }
aoikoizumi 3:9cef6e58a462 520
aoikoizumi 3:9cef6e58a462 521
aoikoizumi 3:9cef6e58a462 522 #endif
aoikoizumi 3:9cef6e58a462 523
aoikoizumi 3:9cef6e58a462 524
aoikoizumi 3:9cef6e58a462 525
aoikoizumi 3:9cef6e58a462 526
yuki0701 0:6e2abd0956f1 527 }