ロボステ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@5:f9dd9346bfd4, 2020-03-04 (annotated)

Committer:
yuki0701
Date:
Wed Mar 04 04:05:04 2020 +0000
Revision:
5:f9dd9346bfd4
Parent:
4:f1f638a6b6c5 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki0701 0:6e2abd0956f1 1 #include "mbed.h"
yuki0701 5:f9dd9346bfd4 2 #include "KondoServo.h"
yuki0701 5:f9dd9346bfd4 3
yuki0701 5:f9dd9346bfd4 4 #include <ros.h>
yuki0701 5:f9dd9346bfd4 5 #include <geometry_msgs/Point.h>
yuki0701 5:f9dd9346bfd4 6 #include <geometry_msgs/Pose.h>
yuki0701 5:f9dd9346bfd4 7 #include <std_msgs/String.h>
yuki0701 5:f9dd9346bfd4 8
yuki0701 5:f9dd9346bfd4 9 ros::NodeHandle nh;
yuki0701 5:f9dd9346bfd4 10 geometry_msgs::Point arm_num;
yuki0701 5:f9dd9346bfd4 11
yuki0701 5:f9dd9346bfd4 12 ros::Publisher pub("/arm_rt",&arm_num);
yuki0701 5:f9dd9346bfd4 13
yuki0701 5:f9dd9346bfd4 14
yuki0701 0:6e2abd0956f1 15 #define RESOLUTION 2048//分解能
yuki0701 0:6e2abd0956f1 16
yuki0701 0:6e2abd0956f1 17 //#define NHK2020_TEST_MODE
yuki0701 4:f1f638a6b6c5 18 //#define NHK2020_MAIN_MODE
yuki0701 4:f1f638a6b6c5 19 #define NHK2020_MAIN_MODE2
yuki0701 5:f9dd9346bfd4 20 //#define NHK2020_MAIN_MODE3
yuki0701 5:f9dd9346bfd4 21 //CAN can1(p30,p29);
yuki0701 0:6e2abd0956f1 22
yuki0701 0:6e2abd0956f1 23 Ticker can_ticker; //can用ticker
yuki0701 0:6e2abd0956f1 24
yuki0701 5:f9dd9346bfd4 25 //Ec4multi EC_backdrop(p15,p16,RESOLUTION);
yuki0701 5:f9dd9346bfd4 26 //SpeedControl backdrop(p22,p21,50,EC_backdrop);
yuki0701 5:f9dd9346bfd4 27 ///////////Serial pc(USBTX,USBRX);
yuki0701 5:f9dd9346bfd4 28 DigitalOut snatch(p8);//sponge
yuki0701 0:6e2abd0956f1 29 DigitalOut pass1(p27);
yuki0701 0:6e2abd0956f1 30 DigitalOut pass2(p28);
yuki0701 5:f9dd9346bfd4 31 DigitalOut tkeep(p11);
yuki0701 5:f9dd9346bfd4 32 DigitalOut kick(p12);
yuki0701 5:f9dd9346bfd4 33 KondoServo servo(p13, p14, 1, 115200); //ピン宣言 (TX,RX,不明,通信の周期？)
yuki0701 5:f9dd9346bfd4 34
yuki0701 5:f9dd9346bfd4 35
yuki0701 5:f9dd9346bfd4 36
yuki0701 5:f9dd9346bfd4 37 int ID=0; //ICSマネージャーを使って変更可能（このプログラムそのまま使って動かなかったらIDが違う説濃厚）
yuki0701 0:6e2abd0956f1 38 //Ticker motor_tick; //角速度計算用ticker
yuki0701 0:6e2abd0956f1 39
yuki0701 0:6e2abd0956f1 40 char t2[1]= {0}; //動作番号（送信する値（char型））
yuki0701 0:6e2abd0956f1 41 int t2_r=0, T2=0; //動作番号（受け取った値、送信する値（int型））
yuki0701 0:6e2abd0956f1 42
yuki0701 0:6e2abd0956f1 43 double turn=0;
yuki0701 0:6e2abd0956f1 44 int k = 0;
yuki0701 5:f9dd9346bfd4 45 int kkk = 0;
yuki0701 5:f9dd9346bfd4 46
yuki0701 5:f9dd9346bfd4 47 double gb1, gb2, gb3;
yuki0701 5:f9dd9346bfd4 48 void messageCallback(const geometry_msgs::Quaternion &arm_get)
yuki0701 5:f9dd9346bfd4 49 {
yuki0701 5:f9dd9346bfd4 50 t2_r = (int)arm_get.x;
yuki0701 5:f9dd9346bfd4 51 gb1 = (double)arm_get.y;
yuki0701 5:f9dd9346bfd4 52 gb2 = (double)arm_get.z;
yuki0701 5:f9dd9346bfd4 53 gb3 = (double)arm_get.w;
yuki0701 5:f9dd9346bfd4 54 }
yuki0701 5:f9dd9346bfd4 55
yuki0701 5:f9dd9346bfd4 56 ros::Subscriber<geometry_msgs::Quaternion> sub("/mbed_arm", &messageCallback);
yuki0701 5:f9dd9346bfd4 57
yuki0701 0:6e2abd0956f1 58 void tsukami()
yuki0701 0:6e2abd0956f1 59 {
yuki0701 5:f9dd9346bfd4 60 servo.set_degree(ID, 80); //サーボを動かす(ID,動かしたい角度)
yuki0701 0:6e2abd0956f1 61 }
yuki0701 0:6e2abd0956f1 62 void put()
yuki0701 0:6e2abd0956f1 63 {
yuki0701 5:f9dd9346bfd4 64 servo.set_degree(ID, 100); //サーボを動かす(ID,動かしたい角度)
yuki0701 0:6e2abd0956f1 65 }
yuki0701 0:6e2abd0956f1 66 void top()
yuki0701 0:6e2abd0956f1 67 {
yuki0701 5:f9dd9346bfd4 68 servo.set_degree(ID, 90); //サーボを動かす(ID,動かしたい角度)
yuki0701 0:6e2abd0956f1 69 }
yuki0701 5:f9dd9346bfd4 70
yuki0701 0:6e2abd0956f1 71
yuki0701 0:6e2abd0956f1 72 int q = 0;
yuki0701 0:6e2abd0956f1 73
yuki0701 0:6e2abd0956f1 74 void can_read()
yuki0701 0:6e2abd0956f1 75 {
yuki0701 0:6e2abd0956f1 76
yuki0701 0:6e2abd0956f1 77
yuki0701 5:f9dd9346bfd4 78 //CANMessage msg;
yuki0701 0:6e2abd0956f1 79
yuki0701 5:f9dd9346bfd4 80 arm_num.x = T2;
yuki0701 5:f9dd9346bfd4 81 arm_num.y = (float)kkk;
yuki0701 5:f9dd9346bfd4 82 arm_num.z = 12;
yuki0701 0:6e2abd0956f1 83
yuki0701 5:f9dd9346bfd4 84 pub.publish(&arm_num);
yuki0701 0:6e2abd0956f1 85
yuki0701 0:6e2abd0956f1 86 if(t2_r > T2) {
yuki0701 0:6e2abd0956f1 87 T2 = t2_r;
yuki0701 0:6e2abd0956f1 88 }
yuki0701 0:6e2abd0956f1 89
yuki0701 0:6e2abd0956f1 90
yuki0701 0:6e2abd0956f1 91 }
yuki0701 0:6e2abd0956f1 92
yuki0701 0:6e2abd0956f1 93
yuki0701 0:6e2abd0956f1 94 int main()
yuki0701 0:6e2abd0956f1 95 {
yuki0701 5:f9dd9346bfd4 96 //pc.printf("setting please");
yuki0701 4:f1f638a6b6c5 97
yuki0701 5:f9dd9346bfd4 98 nh.getHardware()->setBaud(115200);
yuki0701 5:f9dd9346bfd4 99 nh.initNode();
yuki0701 5:f9dd9346bfd4 100 nh.advertise(pub);
yuki0701 5:f9dd9346bfd4 101 nh.subscribe(sub);
yuki0701 5:f9dd9346bfd4 102 // snatch=0,pass1=0,pass2=0,ttsukami=0,kick=0;
yuki0701 5:f9dd9346bfd4 103 // servo.init(); //servo初期化？ 決まり文句
yuki0701 0:6e2abd0956f1 104
yuki0701 0:6e2abd0956f1 105 #ifdef NHK2020_TEST_MODE
yuki0701 4:f1f638a6b6c5 106 while(1) {
aoikoizumi 2:90b53995cb1f 107 printf("T2 = %d\n\r",T2);
yuki0701 5:f9dd9346bfd4 108 //printf("%lf",EC_backdrop.getRad());
yuki0701 5:f9dd9346bfd4 109 /* if(sel=='z') {
yuki0701 5:f9dd9346bfd4 110 pc.printf("z\r\n");
yuki0701 5:f9dd9346bfd4 111 tsukami();
yuki0701 5:f9dd9346bfd4 112 } else if(sel=='x') {
yuki0701 5:f9dd9346bfd4 113 pc.printf("x\r\n");
yuki0701 5:f9dd9346bfd4 114 put();
yuki0701 5:f9dd9346bfd4 115 } else if(sel=='c') {
yuki0701 5:f9dd9346bfd4 116 pc.printf("c\r\n");
yuki0701 5:f9dd9346bfd4 117 top();
yuki0701 5:f9dd9346bfd4 118 }
yuki0701 5:f9dd9346bfd4 119 if(sel=='q') {
yuki0701 5:f9dd9346bfd4 120 printf("\r\n");
yuki0701 5:f9dd9346bfd4 121 if(denjiben==0)denjiben=1;
yuki0701 5:f9dd9346bfd4 122 else denjiben=0;
yuki0701 5:f9dd9346bfd4 123 }
yuki0701 5:f9dd9346bfd4 124 if(sel=='1') {
yuki0701 5:f9dd9346bfd4 125 snatch=0;
yuki0701 5:f9dd9346bfd4 126 printf("snatch_off\r\n");
yuki0701 5:f9dd9346bfd4 127 }
yuki0701 5:f9dd9346bfd4 128 if(sel=='2') {
yuki0701 5:f9dd9346bfd4 129 snatch=1;
yuki0701 5:f9dd9346bfd4 130 printf("snatch_on\r\n");
yuki0701 5:f9dd9346bfd4 131 }
yuki0701 5:f9dd9346bfd4 132 if(sel=='3') {
yuki0701 5:f9dd9346bfd4 133 pass1=0;
yuki0701 5:f9dd9346bfd4 134 printf("pass1_off\r\n");
yuki0701 5:f9dd9346bfd4 135 }
yuki0701 5:f9dd9346bfd4 136 if(sel=='4') {
yuki0701 5:f9dd9346bfd4 137 pass1=1;
yuki0701 5:f9dd9346bfd4 138 printf("pass1_on\r\n");
yuki0701 5:f9dd9346bfd4 139 }
yuki0701 5:f9dd9346bfd4 140 if(sel=='5') {
yuki0701 5:f9dd9346bfd4 141 pass2=0;
yuki0701 5:f9dd9346bfd4 142 printf("pass2_off\r\n");
yuki0701 5:f9dd9346bfd4 143 }
yuki0701 5:f9dd9346bfd4 144 if(sel=='6') {
yuki0701 5:f9dd9346bfd4 145 pass2=1;
yuki0701 5:f9dd9346bfd4 146 printf("pass2_on\r\n");
yuki0701 5:f9dd9346bfd4 147 }*/
yuki0701 5:f9dd9346bfd4 148
yuki0701 5:f9dd9346bfd4 149
yuki0701 0:6e2abd0956f1 150 }
yuki0701 0:6e2abd0956f1 151 #endif
yuki0701 0:6e2abd0956f1 152 #ifdef NHK2020_MAIN_MODE
yuki0701 0:6e2abd0956f1 153 can_ticker.attach(&can_read,0.01);
yuki0701 0:6e2abd0956f1 154 printf("wait_mode\r\n");
yuki0701 5:f9dd9346bfd4 155
yuki0701 5:f9dd9346bfd4 156
yuki0701 5:f9dd9346bfd4 157
yuki0701 5:f9dd9346bfd4 158 //while(1){
yuki0701 5:f9dd9346bfd4 159 // if()brake;
yuki0701 5:f9dd9346bfd4 160 // }
yuki0701 5:f9dd9346bfd4 161 //ボタン操作待ち(セミオートセット
yuki0701 5:f9dd9346bfd4 162
yuki0701 0:6e2abd0956f1 163
yuki0701 5:f9dd9346bfd4 164 //snatch=1;
yuki0701 5:f9dd9346bfd4 165 pass1=1;
yuki0701 5:f9dd9346bfd4 166 wait(1);
yuki0701 5:f9dd9346bfd4 167 pass2=1;
yuki0701 5:f9dd9346bfd4 168 wait(5);
yuki0701 5:f9dd9346bfd4 169 while(1) {
yuki0701 5:f9dd9346bfd4 170 printf("a-T = 0 T2 = %d t2_r = %d\n\r",T2,t2_r);
yuki0701 5:f9dd9346bfd4 171 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 172 wait(0.01);
yuki0701 5:f9dd9346bfd4 173 if(T2 == 1) {
yuki0701 5:f9dd9346bfd4 174 break;
yuki0701 0:6e2abd0956f1 175 }
yuki0701 0:6e2abd0956f1 176 }
yuki0701 5:f9dd9346bfd4 177 //T2 = 1;
aoikoizumi 2:90b53995cb1f 178 if(T2 == 1) { //スタート位置からボール前まで移動（アーム待機）
aoikoizumi 2:90b53995cb1f 179 //T2=1;
yuki0701 5:f9dd9346bfd4 180 wait(1);
yuki0701 5:f9dd9346bfd4 181 pass2=0;//動き出した瞬間に展開始めるやつ
yuki0701 0:6e2abd0956f1 182 while(1) {
aoikoizumi 2:90b53995cb1f 183 printf("a-T = 0 T2 = %d t2_r = %d\n\r",T2,t2_r);
yuki0701 5:f9dd9346bfd4 184 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 185 wait(0.01);
aoikoizumi 2:90b53995cb1f 186 if(T2 == 2) {
yuki0701 0:6e2abd0956f1 187 break;
yuki0701 0:6e2abd0956f1 188 }
yuki0701 0:6e2abd0956f1 189 }
yuki0701 0:6e2abd0956f1 190 }
yuki0701 0:6e2abd0956f1 191
aoikoizumi 2:90b53995cb1f 192 if(T2 == 2) { //ボール掴んで投げる
yuki0701 0:6e2abd0956f1 193 //printf("a-T = 1 T2 = %d t2_r = %d\n\r",T2,t2_r);
yuki0701 5:f9dd9346bfd4 194 tsukami();
yuki0701 5:f9dd9346bfd4 195 wait(3);
aoikoizumi 1:bc34fdc4e16b 196 snatch=1;
yuki0701 5:f9dd9346bfd4 197 wait(1);
yuki0701 5:f9dd9346bfd4 198 put();
yuki0701 0:6e2abd0956f1 199 wait(3);
yuki0701 0:6e2abd0956f1 200 snatch=0;
yuki0701 0:6e2abd0956f1 201 wait(1);
yuki0701 0:6e2abd0956f1 202 top();
yuki0701 0:6e2abd0956f1 203 wait(5);
yuki0701 0:6e2abd0956f1 204 pass1=0;
yuki0701 0:6e2abd0956f1 205 wait(5);
yuki0701 0:6e2abd0956f1 206 //wait(5);
yuki0701 0:6e2abd0956f1 207
yuki0701 0:6e2abd0956f1 208 T2++;
yuki0701 0:6e2abd0956f1 209 }
aoikoizumi 2:90b53995cb1f 210 if(T2 == 3) { //スタートゾーンに戻る（アーム待機）
yuki0701 0:6e2abd0956f1 211 while(1) {
aoikoizumi 2:90b53995cb1f 212 printf("a-T = 2 T2 = %d t2_r = %d\n\r",T2,t2_r);
yuki0701 5:f9dd9346bfd4 213 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 214 wait(0.01);
aoikoizumi 2:90b53995cb1f 215 if(T2 == 4) {
yuki0701 0:6e2abd0956f1 216 break;
yuki0701 0:6e2abd0956f1 217 }
yuki0701 0:6e2abd0956f1 218 }
yuki0701 0:6e2abd0956f1 219 }
yuki0701 0:6e2abd0956f1 220 printf("end\n\r");
yuki0701 4:f1f638a6b6c5 221
yuki0701 0:6e2abd0956f1 222 #endif
aoikoizumi 3:9cef6e58a462 223 #ifdef NHK2020_MAIN_MODE2
yuki0701 5:f9dd9346bfd4 224 //can1.frequency(1000000);
yuki0701 4:f1f638a6b6c5 225 can_ticker.attach(&can_read,0.01);
aoikoizumi 3:9cef6e58a462 226 //printf("T2 = %d\n\r",T2);
yuki0701 4:f1f638a6b6c5 227 if(T2 == 0) {
yuki0701 4:f1f638a6b6c5 228 //T2=1;
yuki0701 5:f9dd9346bfd4 229 /* while(1) {
yuki0701 5:f9dd9346bfd4 230 char sel=pc.getc();
yuki0701 5:f9dd9346bfd4 231 if(sel=='1') {
yuki0701 5:f9dd9346bfd4 232 snatch=0;
yuki0701 5:f9dd9346bfd4 233 printf("snatch_off\r\n");
yuki0701 5:f9dd9346bfd4 234 }
yuki0701 5:f9dd9346bfd4 235 if(sel=='2') {
yuki0701 5:f9dd9346bfd4 236 snatch=1;
yuki0701 5:f9dd9346bfd4 237 printf("snatch_on\r\n");
yuki0701 5:f9dd9346bfd4 238 }
yuki0701 5:f9dd9346bfd4 239 if(sel=='3') {
yuki0701 5:f9dd9346bfd4 240 pass1=0;
yuki0701 5:f9dd9346bfd4 241 printf("pass1_off\r\n");
yuki0701 5:f9dd9346bfd4 242 }
yuki0701 5:f9dd9346bfd4 243 if(sel=='4') {
yuki0701 5:f9dd9346bfd4 244 pass1=1;
yuki0701 5:f9dd9346bfd4 245 printf("pass1_on\r\n");
yuki0701 5:f9dd9346bfd4 246 }
yuki0701 5:f9dd9346bfd4 247 if(sel=='5') {
yuki0701 5:f9dd9346bfd4 248 pass2=0;
yuki0701 5:f9dd9346bfd4 249 printf("pass2_off\r\n");
yuki0701 5:f9dd9346bfd4 250 }
yuki0701 5:f9dd9346bfd4 251 if(sel=='6') {
yuki0701 5:f9dd9346bfd4 252 pass2=1;
yuki0701 5:f9dd9346bfd4 253 printf("pass2_on\r\n");
yuki0701 5:f9dd9346bfd4 254 }
yuki0701 5:f9dd9346bfd4 255 if(sel=='q') {
yuki0701 5:f9dd9346bfd4 256 printf("end_wait_mode\r\n");
yuki0701 5:f9dd9346bfd4 257 T2=1;
yuki0701 5:f9dd9346bfd4 258 break;
yuki0701 5:f9dd9346bfd4 259
yuki0701 5:f9dd9346bfd4 260 }*/
yuki0701 5:f9dd9346bfd4 261
yuki0701 5:f9dd9346bfd4 262 snatch=1;
yuki0701 5:f9dd9346bfd4 263 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 264 wait(5);
yuki0701 5:f9dd9346bfd4 265 //T2=1;
yuki0701 4:f1f638a6b6c5 266 while(1) {
yuki0701 5:f9dd9346bfd4 267 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 268 wait(0.01);
yuki0701 5:f9dd9346bfd4 269 printf("t2=1");
yuki0701 5:f9dd9346bfd4 270 if(T2 == 1) {
yuki0701 4:f1f638a6b6c5 271 break;
yuki0701 4:f1f638a6b6c5 272 }
aoikoizumi 3:9cef6e58a462 273 }
aoikoizumi 3:9cef6e58a462 274 }
yuki0701 5:f9dd9346bfd4 275 if(T2 == 1) { //idou
yuki0701 4:f1f638a6b6c5 276 //T2=2;
yuki0701 4:f1f638a6b6c5 277 pass2=0;
yuki0701 5:f9dd9346bfd4 278 printf("t2=1");
yuki0701 4:f1f638a6b6c5 279 while(1) {
yuki0701 5:f9dd9346bfd4 280 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 281 wait(0.01);
yuki0701 5:f9dd9346bfd4 282 printf("t2=1");
yuki0701 4:f1f638a6b6c5 283 if(T2 == 2) {
yuki0701 4:f1f638a6b6c5 284 break;
yuki0701 4:f1f638a6b6c5 285 }
aoikoizumi 3:9cef6e58a462 286 }
aoikoizumi 3:9cef6e58a462 287 }
yuki0701 4:f1f638a6b6c5 288
yuki0701 5:f9dd9346bfd4 289 if(T2 == 2) { //idou
yuki0701 4:f1f638a6b6c5 290 //T2=3;
aoikoizumi 3:9cef6e58a462 291
yuki0701 4:f1f638a6b6c5 292 while(1) {
yuki0701 5:f9dd9346bfd4 293 kkk++;
yuki0701 5:f9dd9346bfd4 294 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 295 wait(0.01);
yuki0701 5:f9dd9346bfd4 296 printf("t2=2");
yuki0701 5:f9dd9346bfd4 297 wait(5);
yuki0701 5:f9dd9346bfd4 298 T2 = 3;
yuki0701 4:f1f638a6b6c5 299 if(T2 == 3) {
yuki0701 4:f1f638a6b6c5 300 break;
yuki0701 4:f1f638a6b6c5 301 }
yuki0701 5:f9dd9346bfd4 302
aoikoizumi 3:9cef6e58a462 303 }
aoikoizumi 3:9cef6e58a462 304 }
yuki0701 5:f9dd9346bfd4 305 if(T2 == 3) { //idou
yuki0701 4:f1f638a6b6c5 306 //T2=4;
yuki0701 4:f1f638a6b6c5 307 while(1) {
yuki0701 5:f9dd9346bfd4 308 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 309 wait(0.01);
yuki0701 5:f9dd9346bfd4 310 printf("t2=3");
yuki0701 4:f1f638a6b6c5 311 if(T2 == 4) {
yuki0701 4:f1f638a6b6c5 312 break;
yuki0701 4:f1f638a6b6c5 313 }
yuki0701 4:f1f638a6b6c5 314 }
yuki0701 4:f1f638a6b6c5 315 }
yuki0701 4:f1f638a6b6c5 316 if(T2 == 4) { //kikk
yuki0701 4:f1f638a6b6c5 317 //T2=5;
yuki0701 5:f9dd9346bfd4 318 wait(1);
yuki0701 5:f9dd9346bfd4 319 tkeep=1;
yuki0701 5:f9dd9346bfd4 320 wait(1);
yuki0701 5:f9dd9346bfd4 321 snatch=0;
yuki0701 5:f9dd9346bfd4 322 wait(1);
yuki0701 5:f9dd9346bfd4 323 kick=1;
yuki0701 4:f1f638a6b6c5 324 T2++;
yuki0701 5:f9dd9346bfd4 325
yuki0701 4:f1f638a6b6c5 326 while(1) {
yuki0701 5:f9dd9346bfd4 327 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 328 wait(0.01);
yuki0701 5:f9dd9346bfd4 329 printf("t2=4");
yuki0701 4:f1f638a6b6c5 330 if(T2 == 5) {
yuki0701 4:f1f638a6b6c5 331 break;
yuki0701 4:f1f638a6b6c5 332 }
yuki0701 4:f1f638a6b6c5 333 }
yuki0701 4:f1f638a6b6c5 334 }
yuki0701 5:f9dd9346bfd4 335
yuki0701 4:f1f638a6b6c5 336
yuki0701 5:f9dd9346bfd4 337 if(T2 == 5) { //idou
yuki0701 5:f9dd9346bfd4 338 pass1=1;
yuki0701 5:f9dd9346bfd4 339 pass2=1;
yuki0701 5:f9dd9346bfd4 340 wait(2);
yuki0701 5:f9dd9346bfd4 341 pass2=0;
yuki0701 4:f1f638a6b6c5 342 while(1) {
yuki0701 5:f9dd9346bfd4 343 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 344 wait(0.01);
yuki0701 5:f9dd9346bfd4 345 printf("t2=5");
yuki0701 4:f1f638a6b6c5 346 if(T2 == 6) {
yuki0701 4:f1f638a6b6c5 347 break;
yuki0701 4:f1f638a6b6c5 348 }
aoikoizumi 3:9cef6e58a462 349 }
aoikoizumi 3:9cef6e58a462 350 }
yuki0701 5:f9dd9346bfd4 351 if(T2 == 6) { //ボール掴んで投げる
yuki0701 4:f1f638a6b6c5 352 snatch=1;
yuki0701 4:f1f638a6b6c5 353 tsukami();
yuki0701 4:f1f638a6b6c5 354 printf("tsukami");
yuki0701 4:f1f638a6b6c5 355 wait(3);
yuki0701 4:f1f638a6b6c5 356 snatch=0;
yuki0701 4:f1f638a6b6c5 357 wait(1);
yuki0701 4:f1f638a6b6c5 358 put();
yuki0701 4:f1f638a6b6c5 359 printf("put");
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 wait(5);
yuki0701 4:f1f638a6b6c5 366 pass1=0;
yuki0701 4:f1f638a6b6c5 367 wait(5);
yuki0701 4:f1f638a6b6c5 368
yuki0701 4:f1f638a6b6c5 369 pass1=1;
yuki0701 4:f1f638a6b6c5 370 pass2=1;
yuki0701 5:f9dd9346bfd4 371 wait(5);
yuki0701 4:f1f638a6b6c5 372 T2++;
yuki0701 4:f1f638a6b6c5 373 //T2=7;
yuki0701 5:f9dd9346bfd4 374 //while(1) {
yuki0701 5:f9dd9346bfd4 375 // printf("t2=6");
yuki0701 5:f9dd9346bfd4 376 // if(T2 == 7) {
yuki0701 5:f9dd9346bfd4 377 // break;
yuki0701 5:f9dd9346bfd4 378 // }
yuki0701 5:f9dd9346bfd4 379 // }
aoikoizumi 3:9cef6e58a462 380 }
yuki0701 5:f9dd9346bfd4 381 if(T2 == 7) { //移動
yuki0701 4:f1f638a6b6c5 382 //T2=8;
aoikoizumi 3:9cef6e58a462 383
yuki0701 4:f1f638a6b6c5 384 pass2=0;
yuki0701 5:f9dd9346bfd4 385 wait(1);
yuki0701 4:f1f638a6b6c5 386 while(1) {
yuki0701 5:f9dd9346bfd4 387 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 388 wait(0.01);
yuki0701 5:f9dd9346bfd4 389 printf("t2=7");
yuki0701 4:f1f638a6b6c5 390 if(T2 == 8) {
yuki0701 4:f1f638a6b6c5 391 break;
yuki0701 4:f1f638a6b6c5 392 }
aoikoizumi 3:9cef6e58a462 393 }
aoikoizumi 3:9cef6e58a462 394 }
yuki0701 4:f1f638a6b6c5 395
yuki0701 4:f1f638a6b6c5 396 if(T2 == 8) { //ボール掴んで投げる
yuki0701 4:f1f638a6b6c5 397 snatch=1;
yuki0701 4:f1f638a6b6c5 398 tsukami();
yuki0701 4:f1f638a6b6c5 399 printf("tsukami");
yuki0701 4:f1f638a6b6c5 400 wait(3);
yuki0701 4:f1f638a6b6c5 401 snatch=0;
yuki0701 4:f1f638a6b6c5 402 wait(1);
yuki0701 4:f1f638a6b6c5 403 put();
yuki0701 4:f1f638a6b6c5 404 printf("put");
yuki0701 4:f1f638a6b6c5 405 wait(3);
yuki0701 4:f1f638a6b6c5 406 snatch=1;
yuki0701 4:f1f638a6b6c5 407 wait(1);
yuki0701 4:f1f638a6b6c5 408 top();
yuki0701 4:f1f638a6b6c5 409 printf("top");
yuki0701 4:f1f638a6b6c5 410 wait(5);
yuki0701 4:f1f638a6b6c5 411 pass1=0;
yuki0701 4:f1f638a6b6c5 412 wait(5);
yuki0701 4:f1f638a6b6c5 413 //wait(5);
yuki0701 4:f1f638a6b6c5 414
yuki0701 4:f1f638a6b6c5 415 pass1=1;
yuki0701 4:f1f638a6b6c5 416 pass2=1;
yuki0701 5:f9dd9346bfd4 417 wait(5);
yuki0701 4:f1f638a6b6c5 418
yuki0701 4:f1f638a6b6c5 419 T2++;
yuki0701 4:f1f638a6b6c5 420 //T2=9;
yuki0701 5:f9dd9346bfd4 421 //while(1) {
yuki0701 5:f9dd9346bfd4 422 // printf("t2=8");
yuki0701 5:f9dd9346bfd4 423 // if(T2 == 9) {
yuki0701 5:f9dd9346bfd4 424 // break;
yuki0701 5:f9dd9346bfd4 425 // }
yuki0701 5:f9dd9346bfd4 426 // }
aoikoizumi 3:9cef6e58a462 427 }
yuki0701 5:f9dd9346bfd4 428 if(T2 == 9) { //idou
yuki0701 4:f1f638a6b6c5 429 pass2=0;
yuki0701 5:f9dd9346bfd4 430 wait(1);
yuki0701 4:f1f638a6b6c5 431 //T2=10;
yuki0701 4:f1f638a6b6c5 432 while(1) {
yuki0701 5:f9dd9346bfd4 433 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 434 wait(0.01);
yuki0701 5:f9dd9346bfd4 435 printf("t2=9");
yuki0701 4:f1f638a6b6c5 436 if(T2 == 10) {
yuki0701 4:f1f638a6b6c5 437 break;
yuki0701 4:f1f638a6b6c5 438 }
aoikoizumi 3:9cef6e58a462 439 }
aoikoizumi 3:9cef6e58a462 440 }
yuki0701 4:f1f638a6b6c5 441 if(T2 == 10) { //ボール掴んで投げる
yuki0701 4:f1f638a6b6c5 442 snatch=1;
yuki0701 4:f1f638a6b6c5 443 tsukami();
yuki0701 4:f1f638a6b6c5 444 printf("tsukami");
yuki0701 4:f1f638a6b6c5 445 wait(3);
yuki0701 4:f1f638a6b6c5 446 snatch=0;
yuki0701 4:f1f638a6b6c5 447 wait(1);
yuki0701 4:f1f638a6b6c5 448 put();
yuki0701 4:f1f638a6b6c5 449 printf("put");
yuki0701 4:f1f638a6b6c5 450 wait(3);
yuki0701 4:f1f638a6b6c5 451 snatch=1;
yuki0701 4:f1f638a6b6c5 452 wait(1);
yuki0701 4:f1f638a6b6c5 453 top();
yuki0701 4:f1f638a6b6c5 454 printf("top");
yuki0701 4:f1f638a6b6c5 455 wait(5);
yuki0701 4:f1f638a6b6c5 456 pass1=0;
yuki0701 4:f1f638a6b6c5 457 wait(5);
yuki0701 4:f1f638a6b6c5 458 //wait(5);
yuki0701 4:f1f638a6b6c5 459
yuki0701 4:f1f638a6b6c5 460 T2++;
yuki0701 4:f1f638a6b6c5 461 //T2=9;
yuki0701 5:f9dd9346bfd4 462 //while(1) {
yuki0701 5:f9dd9346bfd4 463 // printf("t2=10");
yuki0701 5:f9dd9346bfd4 464 // if(T2 == 11) {
yuki0701 5:f9dd9346bfd4 465 // break;
yuki0701 5:f9dd9346bfd4 466 // }
yuki0701 5:f9dd9346bfd4 467 // }
aoikoizumi 3:9cef6e58a462 468 }
yuki0701 4:f1f638a6b6c5 469 if(T2 == 11) { //スタートゾーンに戻る（アーム待機）
yuki0701 4:f1f638a6b6c5 470 //T2=10;
yuki0701 4:f1f638a6b6c5 471 while(1) {
yuki0701 5:f9dd9346bfd4 472 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 473 wait(0.01);
yuki0701 5:f9dd9346bfd4 474 printf("t2=11");
yuki0701 4:f1f638a6b6c5 475 if(T2 == 12) {
yuki0701 4:f1f638a6b6c5 476 break;
yuki0701 4:f1f638a6b6c5 477 }
yuki0701 4:f1f638a6b6c5 478 }
yuki0701 4:f1f638a6b6c5 479 }
aoikoizumi 3:9cef6e58a462 480 #endif
aoikoizumi 3:9cef6e58a462 481 #ifdef NHK2020_MAIN_MODE3
yuki0701 4:f1f638a6b6c5 482 can1.frequency(1000000);
yuki0701 4:f1f638a6b6c5 483 can_ticker.attach(&can_read,0.01);
aoikoizumi 3:9cef6e58a462 484 //printf("T2 = %d\n\r",T2);
yuki0701 4:f1f638a6b6c5 485 if(T2 == 0) {
yuki0701 4:f1f638a6b6c5 486 //T2=1;
yuki0701 5:f9dd9346bfd4 487 snatch=1;
yuki0701 4:f1f638a6b6c5 488 while(1) {
yuki0701 5:f9dd9346bfd4 489 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 490 wait(0.01);
yuki0701 4:f1f638a6b6c5 491 if(T2 == 1) {
yuki0701 4:f1f638a6b6c5 492 break;
yuki0701 4:f1f638a6b6c5 493 }
aoikoizumi 3:9cef6e58a462 494 }
aoikoizumi 3:9cef6e58a462 495 }
yuki0701 5:f9dd9346bfd4 496 if(T2 == 1) { //スタート位置からkick前まで移動（アーム待機）
yuki0701 4:f1f638a6b6c5 497 //T2=2;
yuki0701 4:f1f638a6b6c5 498 while(1) {
yuki0701 5:f9dd9346bfd4 499 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 500 wait(0.01);
yuki0701 4:f1f638a6b6c5 501 if(T2 == 2) {
yuki0701 4:f1f638a6b6c5 502 break;
yuki0701 4:f1f638a6b6c5 503 }
aoikoizumi 3:9cef6e58a462 504 }
aoikoizumi 3:9cef6e58a462 505 }
aoikoizumi 3:9cef6e58a462 506
yuki0701 5:f9dd9346bfd4 507 if(T2 == 2) { //idou
yuki0701 4:f1f638a6b6c5 508 //T2=3;
yuki0701 4:f1f638a6b6c5 509 while(1) {
yuki0701 5:f9dd9346bfd4 510 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 511 wait(0.01);
yuki0701 4:f1f638a6b6c5 512 if(T2 == 3) {
yuki0701 4:f1f638a6b6c5 513 break;
yuki0701 4:f1f638a6b6c5 514 }
aoikoizumi 3:9cef6e58a462 515 }
aoikoizumi 3:9cef6e58a462 516 }
yuki0701 5:f9dd9346bfd4 517 if(T2 == 3) { //kick
yuki0701 4:f1f638a6b6c5 518 //T2=4;
yuki0701 5:f9dd9346bfd4 519 wait(1);
yuki0701 5:f9dd9346bfd4 520 tkeep=1;
yuki0701 5:f9dd9346bfd4 521 wait(1);
yuki0701 5:f9dd9346bfd4 522 snatch=0;
yuki0701 5:f9dd9346bfd4 523 wait(1);
yuki0701 5:f9dd9346bfd4 524 kick=1;
yuki0701 4:f1f638a6b6c5 525 while(1) {
yuki0701 5:f9dd9346bfd4 526 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 527 wait(0.01);
yuki0701 4:f1f638a6b6c5 528 if(T2 == 4) {
yuki0701 4:f1f638a6b6c5 529 break;
yuki0701 4:f1f638a6b6c5 530 }
aoikoizumi 3:9cef6e58a462 531 }
aoikoizumi 3:9cef6e58a462 532 }
yuki0701 5:f9dd9346bfd4 533 if(T2 == 4) { //kitaku
yuki0701 4:f1f638a6b6c5 534 //T2=5;
yuki0701 4:f1f638a6b6c5 535 while(1) {
yuki0701 5:f9dd9346bfd4 536 nh.spinOnce();
yuki0701 5:f9dd9346bfd4 537 wait(0.01);
yuki0701 4:f1f638a6b6c5 538 if(T2 == 5) {
yuki0701 4:f1f638a6b6c5 539 break;
yuki0701 4:f1f638a6b6c5 540 }
aoikoizumi 3:9cef6e58a462 541 }
aoikoizumi 3:9cef6e58a462 542 }
aoikoizumi 3:9cef6e58a462 543
aoikoizumi 3:9cef6e58a462 544
aoikoizumi 3:9cef6e58a462 545 #endif
aoikoizumi 3:9cef6e58a462 546
aoikoizumi 3:9cef6e58a462 547
aoikoizumi 3:9cef6e58a462 548
aoikoizumi 3:9cef6e58a462 549
yuki0701 0:6e2abd0956f1 550 }