春ロボ　ロケット団 / Mbed 2 deprecated Right_F3RC_mar4_2

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 Encoder CruizCore_R1370P

main.cpp@0:9c9994747ea7, 2019-10-04 (annotated)

Committer:
maxnagazumi
Date:
Fri Oct 04 12:43:40 2019 +0000
Revision:
0:9c9994747ea7
Child:
1:1cb20fa989c2
yeah

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maxnagazumi 0:9c9994747ea7 1 #include "mbed.h"
maxnagazumi 0:9c9994747ea7 2 #include "math.h"
maxnagazumi 0:9c9994747ea7 3 #include "R1370P.h"
maxnagazumi 0:9c9994747ea7 4 #include "EC.h"
maxnagazumi 0:9c9994747ea7 5 #define RESOLUTION 500
maxnagazumi 0:9c9994747ea7 6 PwmOut TB(PB_8);//Top motor Forward
maxnagazumi 0:9c9994747ea7 7 PwmOut TF(PC_9);//Top motor Backward
maxnagazumi 0:9c9994747ea7 8 PwmOut RF(PC_8);//Right motor is okay
maxnagazumi 0:9c9994747ea7 9 PwmOut RB(PC_6);
maxnagazumi 0:9c9994747ea7 10 PwmOut LF(PB_13);//Left motor ONLY F is okay
maxnagazumi 0:9c9994747ea7 11 PwmOut LB(PB_15);
maxnagazumi 0:9c9994747ea7 12
maxnagazumi 0:9c9994747ea7 13 PwmOut servo(PA_15);
maxnagazumi 0:9c9994747ea7 14
maxnagazumi 0:9c9994747ea7 15 DigitalOut led1(LED1);
maxnagazumi 0:9c9994747ea7 16 DigitalIn button(USER_BUTTON);
maxnagazumi 0:9c9994747ea7 17 DigitalOut out1(PB_9);
maxnagazumi 0:9c9994747ea7 18 DigitalIn RandL(PC_12);
maxnagazumi 0:9c9994747ea7 19
maxnagazumi 0:9c9994747ea7 20 Serial pc(USBTX, USBRX); // tx, rx
maxnagazumi 0:9c9994747ea7 21 R1370P gyro(PA_11,PA_12); // tx, rx
maxnagazumi 0:9c9994747ea7 22 Ticker ticker;
maxnagazumi 0:9c9994747ea7 23
maxnagazumi 0:9c9994747ea7 24 //Serial pc(USBTX,USBRX);
maxnagazumi 0:9c9994747ea7 25 Ec1multi EC(PA_8,PA_9,RESOLUTION); //1逓倍用class
maxnagazumi 0:9c9994747ea7 26 Ec1multi EC2(PB_3,PB_5,RESOLUTION);
maxnagazumi 0:9c9994747ea7 27
maxnagazumi 0:9c9994747ea7 28 void rotation(double angle)
maxnagazumi 0:9c9994747ea7 29 {
maxnagazumi 0:9c9994747ea7 30 double a,b;
maxnagazumi 0:9c9994747ea7 31 if( angle > 1) {
maxnagazumi 0:9c9994747ea7 32 a = 0.04;
maxnagazumi 0:9c9994747ea7 33 b = 0;
maxnagazumi 0:9c9994747ea7 34 RF = a;
maxnagazumi 0:9c9994747ea7 35 RB = b;
maxnagazumi 0:9c9994747ea7 36 LF = a;
maxnagazumi 0:9c9994747ea7 37 LB = b;
maxnagazumi 0:9c9994747ea7 38 TF = a;
maxnagazumi 0:9c9994747ea7 39 TB = b;
maxnagazumi 0:9c9994747ea7 40 } else if(angle < -1) {
maxnagazumi 0:9c9994747ea7 41 a = 0;
maxnagazumi 0:9c9994747ea7 42 b = 0.04;
maxnagazumi 0:9c9994747ea7 43 RF = a;
maxnagazumi 0:9c9994747ea7 44 RB = b;
maxnagazumi 0:9c9994747ea7 45 LF = a;
maxnagazumi 0:9c9994747ea7 46 LB = b;
maxnagazumi 0:9c9994747ea7 47 TF = a;
maxnagazumi 0:9c9994747ea7 48 TB = b;
maxnagazumi 0:9c9994747ea7 49 }
maxnagazumi 0:9c9994747ea7 50
maxnagazumi 0:9c9994747ea7 51 }
maxnagazumi 0:9c9994747ea7 52
maxnagazumi 0:9c9994747ea7 53 void go_S(double a)
maxnagazumi 0:9c9994747ea7 54 {
maxnagazumi 0:9c9994747ea7 55 RF = 0;
maxnagazumi 0:9c9994747ea7 56 RB = a;
maxnagazumi 0:9c9994747ea7 57 LF = a;
maxnagazumi 0:9c9994747ea7 58 LB = 0;
maxnagazumi 0:9c9994747ea7 59 TF = 0;
maxnagazumi 0:9c9994747ea7 60 TB = 0;
maxnagazumi 0:9c9994747ea7 61 }
maxnagazumi 0:9c9994747ea7 62
maxnagazumi 0:9c9994747ea7 63 void go_B(double a)
maxnagazumi 0:9c9994747ea7 64 {
maxnagazumi 0:9c9994747ea7 65 RF = a;
maxnagazumi 0:9c9994747ea7 66 RB = 0;
maxnagazumi 0:9c9994747ea7 67 LF = 0;
maxnagazumi 0:9c9994747ea7 68 LB = a;
maxnagazumi 0:9c9994747ea7 69 TF = 0;
maxnagazumi 0:9c9994747ea7 70 TB = 0;
maxnagazumi 0:9c9994747ea7 71 }
maxnagazumi 0:9c9994747ea7 72
maxnagazumi 0:9c9994747ea7 73 void go_R(double a)
maxnagazumi 0:9c9994747ea7 74 {
maxnagazumi 0:9c9994747ea7 75 TB = a;
maxnagazumi 0:9c9994747ea7 76 TF = 0;
maxnagazumi 0:9c9994747ea7 77 RF = a/2;
maxnagazumi 0:9c9994747ea7 78 RB = 0;
maxnagazumi 0:9c9994747ea7 79 LF = a/2;
maxnagazumi 0:9c9994747ea7 80 LB = 0;
maxnagazumi 0:9c9994747ea7 81 }
maxnagazumi 0:9c9994747ea7 82
maxnagazumi 0:9c9994747ea7 83 void go_L(double a)
maxnagazumi 0:9c9994747ea7 84 {
maxnagazumi 0:9c9994747ea7 85 TB = 0;
maxnagazumi 0:9c9994747ea7 86 TF = a;
maxnagazumi 0:9c9994747ea7 87 RF = 0;
maxnagazumi 0:9c9994747ea7 88 RB = a/2;
maxnagazumi 0:9c9994747ea7 89 LF = 0;
maxnagazumi 0:9c9994747ea7 90 LB = a/2;
maxnagazumi 0:9c9994747ea7 91 }
maxnagazumi 0:9c9994747ea7 92
maxnagazumi 0:9c9994747ea7 93 int main()
maxnagazumi 0:9c9994747ea7 94 {
maxnagazumi 0:9c9994747ea7 95 TF.period_ms(1);
maxnagazumi 0:9c9994747ea7 96 TB.period_ms(1);
maxnagazumi 0:9c9994747ea7 97 RF.period_ms(1);
maxnagazumi 0:9c9994747ea7 98 RB.period_ms(1);
maxnagazumi 0:9c9994747ea7 99 LF.period_ms(1);
maxnagazumi 0:9c9994747ea7 100 LB.period_ms(1);
maxnagazumi 0:9c9994747ea7 101 out1 = 0;
maxnagazumi 0:9c9994747ea7 102 servo.period_ms(20);
maxnagazumi 0:9c9994747ea7 103 servo.pulsewidth_us(550);
maxnagazumi 0:9c9994747ea7 104 wait(1);
maxnagazumi 0:9c9994747ea7 105 int count=0,count2=0;
maxnagazumi 0:9c9994747ea7 106 double angle;
maxnagazumi 0:9c9994747ea7 107 //ticker.attach(&calcOmega,0.5);
maxnagazumi 0:9c9994747ea7 108 gyro.initialize(); //main関数の最初に一度だけ実行
maxnagazumi 0:9c9994747ea7 109 gyro.acc_offset(); //やってもやらなくてもいい
maxnagazumi 0:9c9994747ea7 110
maxnagazumi 0:9c9994747ea7 111 count=EC.getCount();
maxnagazumi 0:9c9994747ea7 112 count2=EC2.getCount();
maxnagazumi 0:9c9994747ea7 113 angle=gyro.getAngle(); //角度の値を受け取る
maxnagazumi 0:9c9994747ea7 114 pc.printf(" x=%d ",count);
maxnagazumi 0:9c9994747ea7 115 pc.printf(" y=%d ",count2);
maxnagazumi 0:9c9994747ea7 116 pc.printf(" angle=%8.4f ",angle);
maxnagazumi 0:9c9994747ea7 117 pc.printf("\r\n");
maxnagazumi 0:9c9994747ea7 118
maxnagazumi 0:9c9994747ea7 119 while(count < 200) {
maxnagazumi 0:9c9994747ea7 120 count=EC.getCount();
maxnagazumi 0:9c9994747ea7 121 count2=EC2.getCount();
maxnagazumi 0:9c9994747ea7 122 angle=gyro.getAngle(); //角度の値を受け取る
maxnagazumi 0:9c9994747ea7 123 pc.printf(" x=%d ",count);
maxnagazumi 0:9c9994747ea7 124 pc.printf(" y=%d ",count2);
maxnagazumi 0:9c9994747ea7 125 pc.printf(" angle=%8.4f ",angle);
maxnagazumi 0:9c9994747ea7 126 pc.printf("first");
maxnagazumi 0:9c9994747ea7 127 pc.printf("\r\n");
maxnagazumi 0:9c9994747ea7 128 go_R(0.1);
maxnagazumi 0:9c9994747ea7 129 rotation(angle);
maxnagazumi 0:9c9994747ea7 130 }
maxnagazumi 0:9c9994747ea7 131 rotation(angle);
maxnagazumi 0:9c9994747ea7 132 while(count2 < 10800) {
maxnagazumi 0:9c9994747ea7 133 count=EC.getCount();
maxnagazumi 0:9c9994747ea7 134 count2=EC2.getCount();
maxnagazumi 0:9c9994747ea7 135 angle=gyro.getAngle(); //角度の値を受け取る
maxnagazumi 0:9c9994747ea7 136 pc.printf(" x=%d ",count);
maxnagazumi 0:9c9994747ea7 137 pc.printf(" y=%d ",count2);
maxnagazumi 0:9c9994747ea7 138 pc.printf(" angle=%8.4f ",angle);
maxnagazumi 0:9c9994747ea7 139 pc.printf("\r\n");
maxnagazumi 0:9c9994747ea7 140 go_S(0.1);
maxnagazumi 0:9c9994747ea7 141 rotation(angle);
maxnagazumi 0:9c9994747ea7 142 }
maxnagazumi 0:9c9994747ea7 143 while(count < 195) {
maxnagazumi 0:9c9994747ea7 144 count=EC.getCount();
maxnagazumi 0:9c9994747ea7 145 count2=EC2.getCount();
maxnagazumi 0:9c9994747ea7 146 angle=gyro.getAngle(); //角度の値を受け取る
maxnagazumi 0:9c9994747ea7 147 pc.printf(" x=%d ",count);
maxnagazumi 0:9c9994747ea7 148 pc.printf(" y=%d ",count2);
maxnagazumi 0:9c9994747ea7 149 pc.printf(" angle=%8.4f",angle);
maxnagazumi 0:9c9994747ea7 150 pc.printf("\r\n");
maxnagazumi 0:9c9994747ea7 151 go_R(0.1);
maxnagazumi 0:9c9994747ea7 152 rotation(angle);
maxnagazumi 0:9c9994747ea7 153 }
maxnagazumi 0:9c9994747ea7 154 go_R(0);
maxnagazumi 0:9c9994747ea7 155 while(count > 205) {
maxnagazumi 0:9c9994747ea7 156 count=EC.getCount();
maxnagazumi 0:9c9994747ea7 157 count2=EC2.getCount();
maxnagazumi 0:9c9994747ea7 158 angle=gyro.getAngle(); //角度の値を受け取る
maxnagazumi 0:9c9994747ea7 159 pc.printf(" x=%d ",count);
maxnagazumi 0:9c9994747ea7 160 pc.printf(" y=%d ",count2);
maxnagazumi 0:9c9994747ea7 161 pc.printf(" angle=%8.4f",angle);
maxnagazumi 0:9c9994747ea7 162 pc.printf("\r\n");
maxnagazumi 0:9c9994747ea7 163 go_L(0.1);
maxnagazumi 0:9c9994747ea7 164 rotation(angle);
maxnagazumi 0:9c9994747ea7 165 }
maxnagazumi 0:9c9994747ea7 166 go_L(0);
maxnagazumi 0:9c9994747ea7 167 //get (-600,10000)
maxnagazumi 0:9c9994747ea7 168 servo.pulsewidth_us(600);
maxnagazumi 0:9c9994747ea7 169 wait(0.5);
maxnagazumi 0:9c9994747ea7 170 servo.pulsewidth_us(700);
maxnagazumi 0:9c9994747ea7 171 wait(0.5);
maxnagazumi 0:9c9994747ea7 172 servo.pulsewidth_us(750);
maxnagazumi 0:9c9994747ea7 173 wait(0.5);
maxnagazumi 0:9c9994747ea7 174 servo.pulsewidth_us(800);
maxnagazumi 0:9c9994747ea7 175 wait(0.5);
maxnagazumi 0:9c9994747ea7 176 servo.pulsewidth_us(850);
maxnagazumi 0:9c9994747ea7 177 wait(0.5);
maxnagazumi 0:9c9994747ea7 178 servo.pulsewidth_us(900);
maxnagazumi 0:9c9994747ea7 179 wait(0.5);
maxnagazumi 0:9c9994747ea7 180 servo.pulsewidth_us(950);
maxnagazumi 0:9c9994747ea7 181 wait(0.5);
maxnagazumi 0:9c9994747ea7 182 servo.pulsewidth_us(1000);
maxnagazumi 0:9c9994747ea7 183 wait(0.5);
maxnagazumi 0:9c9994747ea7 184 servo.pulsewidth_us(1050);
maxnagazumi 0:9c9994747ea7 185 wait(0.5);
maxnagazumi 0:9c9994747ea7 186 servo.pulsewidth_us(1070);
maxnagazumi 0:9c9994747ea7 187 wait(0.5);
maxnagazumi 0:9c9994747ea7 188 servo.pulsewidth_us(1100);
maxnagazumi 0:9c9994747ea7 189 wait(0.5);
maxnagazumi 0:9c9994747ea7 190 servo.pulsewidth_us(1180);
maxnagazumi 0:9c9994747ea7 191 wait(1);
maxnagazumi 0:9c9994747ea7 192 out1 = 1;//catch
maxnagazumi 0:9c9994747ea7 193 wait(2);
maxnagazumi 0:9c9994747ea7 194 servo.pulsewidth_us(1150);
maxnagazumi 0:9c9994747ea7 195 //have caught
maxnagazumi 0:9c9994747ea7 196 while(count2 > 200) {
maxnagazumi 0:9c9994747ea7 197 count=EC.getCount();
maxnagazumi 0:9c9994747ea7 198 count2=EC2.getCount();
maxnagazumi 0:9c9994747ea7 199 angle=gyro.getAngle(); //角度の値を受け取る
maxnagazumi 0:9c9994747ea7 200 pc.printf(" x=%d ",count);
maxnagazumi 0:9c9994747ea7 201 pc.printf(" y=%d ",count2);
maxnagazumi 0:9c9994747ea7 202 pc.printf(" angle=%8.4f ",angle);
maxnagazumi 0:9c9994747ea7 203 pc.printf("\r\n");
maxnagazumi 0:9c9994747ea7 204 go_B(0.1);
maxnagazumi 0:9c9994747ea7 205 rotation(angle);
maxnagazumi 0:9c9994747ea7 206 }
maxnagazumi 0:9c9994747ea7 207 while(count < 195) {
maxnagazumi 0:9c9994747ea7 208 count=EC.getCount();
maxnagazumi 0:9c9994747ea7 209 count2=EC2.getCount();
maxnagazumi 0:9c9994747ea7 210 angle=gyro.getAngle(); //角度の値を受け取る
maxnagazumi 0:9c9994747ea7 211 pc.printf(" x=%d ",count);
maxnagazumi 0:9c9994747ea7 212 pc.printf(" y=%d ",count2);
maxnagazumi 0:9c9994747ea7 213 pc.printf(" angle=%8.4f",angle);
maxnagazumi 0:9c9994747ea7 214 pc.printf("\r\n");
maxnagazumi 0:9c9994747ea7 215 go_R(0.1);
maxnagazumi 0:9c9994747ea7 216 rotation(angle);
maxnagazumi 0:9c9994747ea7 217 }
maxnagazumi 0:9c9994747ea7 218 while(count > 205) {
maxnagazumi 0:9c9994747ea7 219 count=EC.getCount();
maxnagazumi 0:9c9994747ea7 220 count2=EC2.getCount();
maxnagazumi 0:9c9994747ea7 221 angle=gyro.getAngle(); //角度の値を受け取る
maxnagazumi 0:9c9994747ea7 222 pc.printf(" x=%d ",count);
maxnagazumi 0:9c9994747ea7 223 pc.printf(" y=%d ",count2);
maxnagazumi 0:9c9994747ea7 224 pc.printf(" angle=%8.4f",angle);
maxnagazumi 0:9c9994747ea7 225 pc.printf("\r\n");
maxnagazumi 0:9c9994747ea7 226 go_L(0.1);
maxnagazumi 0:9c9994747ea7 227 rotation(angle);
maxnagazumi 0:9c9994747ea7 228 }
maxnagazumi 0:9c9994747ea7 229 while(angle > -85) {
maxnagazumi 0:9c9994747ea7 230 double a,b;
maxnagazumi 0:9c9994747ea7 231 a = 0.05;
maxnagazumi 0:9c9994747ea7 232 b = 0;
maxnagazumi 0:9c9994747ea7 233 RF = a;
maxnagazumi 0:9c9994747ea7 234 RB = b;
maxnagazumi 0:9c9994747ea7 235 LF = a;
maxnagazumi 0:9c9994747ea7 236 LB = b;
maxnagazumi 0:9c9994747ea7 237 TF = a;
maxnagazumi 0:9c9994747ea7 238 TB = b;
maxnagazumi 0:9c9994747ea7 239 angle=gyro.getAngle();
maxnagazumi 0:9c9994747ea7 240 }
maxnagazumi 0:9c9994747ea7 241 //now (-700,100)
maxnagazumi 0:9c9994747ea7 242 //EC reset
maxnagazumi 0:9c9994747ea7 243 pc.printf("1\r\n");
maxnagazumi 0:9c9994747ea7 244 while(count2 < 1200) {
maxnagazumi 0:9c9994747ea7 245 count=EC.getCount();
maxnagazumi 0:9c9994747ea7 246 count2=EC2.getCount();
maxnagazumi 0:9c9994747ea7 247 angle=gyro.getAngle(); //角度の値を受け取る
maxnagazumi 0:9c9994747ea7 248 pc.printf(" x=%d ",count);
maxnagazumi 0:9c9994747ea7 249 pc.printf(" y=%d ",count2);
maxnagazumi 0:9c9994747ea7 250 pc.printf(" angle=%8.4f ",angle);
maxnagazumi 0:9c9994747ea7 251 pc.printf("\r\n");
maxnagazumi 0:9c9994747ea7 252 go_S(0.1);
maxnagazumi 0:9c9994747ea7 253 }//(0,600)
maxnagazumi 0:9c9994747ea7 254 pc.printf("2\r\n");
maxnagazumi 0:9c9994747ea7 255 go_S(0);
maxnagazumi 0:9c9994747ea7 256 go_R(0);
maxnagazumi 0:9c9994747ea7 257 go_L(0);
maxnagazumi 0:9c9994747ea7 258 wait(15);
maxnagazumi 0:9c9994747ea7 259 pc.printf("3\r\n");
maxnagazumi 0:9c9994747ea7 260 while(count2 > 500) {
maxnagazumi 0:9c9994747ea7 261 count=EC.getCount();
maxnagazumi 0:9c9994747ea7 262 count2=EC2.getCount();
maxnagazumi 0:9c9994747ea7 263 angle=gyro.getAngle(); //角度の値を受け取る
maxnagazumi 0:9c9994747ea7 264 pc.printf(" x=%d ",count);
maxnagazumi 0:9c9994747ea7 265 pc.printf(" y=%d ",count2);
maxnagazumi 0:9c9994747ea7 266 pc.printf(" angle=%8.4f ",angle);
maxnagazumi 0:9c9994747ea7 267 pc.printf("\r\n");
maxnagazumi 0:9c9994747ea7 268 go_B(0.1);
maxnagazumi 0:9c9994747ea7 269 }
maxnagazumi 0:9c9994747ea7 270 go_B(0);
maxnagazumi 0:9c9994747ea7 271
maxnagazumi 0:9c9994747ea7 272 pc.printf("4\r\n");
maxnagazumi 0:9c9994747ea7 273 //now(0,0)
maxnagazumi 0:9c9994747ea7 274 while( angle > 1) {
maxnagazumi 0:9c9994747ea7 275 double a,b;
maxnagazumi 0:9c9994747ea7 276 angle=gyro.getAngle();
maxnagazumi 0:9c9994747ea7 277 a = 0.05;
maxnagazumi 0:9c9994747ea7 278 b = 0;
maxnagazumi 0:9c9994747ea7 279 RF = a;
maxnagazumi 0:9c9994747ea7 280 RB = b;
maxnagazumi 0:9c9994747ea7 281 LF = a;
maxnagazumi 0:9c9994747ea7 282 LB = b;
maxnagazumi 0:9c9994747ea7 283 TF = a;
maxnagazumi 0:9c9994747ea7 284 TB = b;
maxnagazumi 0:9c9994747ea7 285 }
maxnagazumi 0:9c9994747ea7 286 while(angle < -1) {
maxnagazumi 0:9c9994747ea7 287 double a,b;
maxnagazumi 0:9c9994747ea7 288 angle=gyro.getAngle();
maxnagazumi 0:9c9994747ea7 289 a = 0;
maxnagazumi 0:9c9994747ea7 290 b = 0.05;
maxnagazumi 0:9c9994747ea7 291 RF = a;
maxnagazumi 0:9c9994747ea7 292 RB = b;
maxnagazumi 0:9c9994747ea7 293 LF = a;
maxnagazumi 0:9c9994747ea7 294 LB = b;
maxnagazumi 0:9c9994747ea7 295 TF = a;
maxnagazumi 0:9c9994747ea7 296 TB = b;
maxnagazumi 0:9c9994747ea7 297 }
maxnagazumi 0:9c9994747ea7 298 //now (0,0)
maxnagazumi 0:9c9994747ea7 299 pc.printf("5\r\n");
maxnagazumi 0:9c9994747ea7 300 while(count2 < 11800) {
maxnagazumi 0:9c9994747ea7 301 count=EC.getCount();
maxnagazumi 0:9c9994747ea7 302 count2=EC2.getCount();
maxnagazumi 0:9c9994747ea7 303 angle=gyro.getAngle(); //角度の値を受け取る
maxnagazumi 0:9c9994747ea7 304 pc.printf(" x=%d ",count);
maxnagazumi 0:9c9994747ea7 305 pc.printf(" y=%d ",count2);
maxnagazumi 0:9c9994747ea7 306 pc.printf(" angle=%8.4f ",angle);
maxnagazumi 0:9c9994747ea7 307 pc.printf("\r\n");
maxnagazumi 0:9c9994747ea7 308 go_S(0.1);
maxnagazumi 0:9c9994747ea7 309 rotation(angle);
maxnagazumi 0:9c9994747ea7 310 }//now(0,10000)
maxnagazumi 0:9c9994747ea7 311 while(count < 2500) {
maxnagazumi 0:9c9994747ea7 312 count=EC.getCount();
maxnagazumi 0:9c9994747ea7 313 count2=EC2.getCount();
maxnagazumi 0:9c9994747ea7 314 angle=gyro.getAngle(); //角度の値を受け取る
maxnagazumi 0:9c9994747ea7 315 pc.printf(" x=%d ",count);
maxnagazumi 0:9c9994747ea7 316 pc.printf(" y=%d ",count2);
maxnagazumi 0:9c9994747ea7 317 pc.printf(" angle=%8.4f",angle);
maxnagazumi 0:9c9994747ea7 318 pc.printf("\r\n");
maxnagazumi 0:9c9994747ea7 319 go_R(0.1);
maxnagazumi 0:9c9994747ea7 320 rotation(angle);
maxnagazumi 0:9c9994747ea7 321 }//now(-700,11800)
maxnagazumi 0:9c9994747ea7 322 while(count2 < 13000) {
maxnagazumi 0:9c9994747ea7 323 count=EC.getCount();
maxnagazumi 0:9c9994747ea7 324 count2=EC2.getCount();
maxnagazumi 0:9c9994747ea7 325 angle=gyro.getAngle(); //角度の値を受け取る
maxnagazumi 0:9c9994747ea7 326 pc.printf(" x=%d ",count);
maxnagazumi 0:9c9994747ea7 327 pc.printf(" y=%d ",count2);
maxnagazumi 0:9c9994747ea7 328 pc.printf(" angle=%8.4f ",angle);
maxnagazumi 0:9c9994747ea7 329 pc.printf("\r\n");
maxnagazumi 0:9c9994747ea7 330 go_S(0.1);
maxnagazumi 0:9c9994747ea7 331 rotation(angle);
maxnagazumi 0:9c9994747ea7 332 }//now(-1700,13000)
maxnagazumi 0:9c9994747ea7 333 //remove battery
maxnagazumi 0:9c9994747ea7 334 wait(3);
maxnagazumi 0:9c9994747ea7 335 while(count > 800) {
maxnagazumi 0:9c9994747ea7 336 count=EC.getCount();
maxnagazumi 0:9c9994747ea7 337 count2=EC2.getCount();
maxnagazumi 0:9c9994747ea7 338 angle=gyro.getAngle(); //角度の値を受け取る
maxnagazumi 0:9c9994747ea7 339 pc.printf(" x=%d ",count);
maxnagazumi 0:9c9994747ea7 340 pc.printf(" y=%d ",count2);
maxnagazumi 0:9c9994747ea7 341 pc.printf(" angle=%8.4f",angle);
maxnagazumi 0:9c9994747ea7 342 pc.printf("\r\n");
maxnagazumi 0:9c9994747ea7 343 go_L(0.1);
maxnagazumi 0:9c9994747ea7 344 rotation(angle);
maxnagazumi 0:9c9994747ea7 345 }
maxnagazumi 0:9c9994747ea7 346 go_R(0);
maxnagazumi 0:9c9994747ea7 347 go_S(0);
maxnagazumi 0:9c9994747ea7 348 go_L(0);
maxnagazumi 0:9c9994747ea7 349 //now(0,13000)
maxnagazumi 0:9c9994747ea7 350 servo.pulsewidth_us(1100);
maxnagazumi 0:9c9994747ea7 351 wait(0.5);
maxnagazumi 0:9c9994747ea7 352 servo.pulsewidth_us(1050);
maxnagazumi 0:9c9994747ea7 353 wait(0.5);
maxnagazumi 0:9c9994747ea7 354 servo.pulsewidth_us(1000);
maxnagazumi 0:9c9994747ea7 355 wait(0.5);
maxnagazumi 0:9c9994747ea7 356 servo.pulsewidth_us(950);
maxnagazumi 0:9c9994747ea7 357 wait(0.5);
maxnagazumi 0:9c9994747ea7 358 servo.pulsewidth_us(900);
maxnagazumi 0:9c9994747ea7 359 wait(0.5);
maxnagazumi 0:9c9994747ea7 360 servo.pulsewidth_us(850);
maxnagazumi 0:9c9994747ea7 361 wait(0.5);
maxnagazumi 0:9c9994747ea7 362 servo.pulsewidth_us(800);
maxnagazumi 0:9c9994747ea7 363 wait(0.5);
maxnagazumi 0:9c9994747ea7 364 servo.pulsewidth_us(750);
maxnagazumi 0:9c9994747ea7 365 wait(0.5);
maxnagazumi 0:9c9994747ea7 366 servo.pulsewidth_us(700);
maxnagazumi 0:9c9994747ea7 367 wait(0.5);
maxnagazumi 0:9c9994747ea7 368 servo.pulsewidth_us(650);
maxnagazumi 0:9c9994747ea7 369 wait(1);
maxnagazumi 0:9c9994747ea7 370 servo.pulsewidth_us(600);
maxnagazumi 0:9c9994747ea7 371 wait(1);
maxnagazumi 0:9c9994747ea7 372 servo.pulsewidth_us(550);
maxnagazumi 0:9c9994747ea7 373 wait(1);
maxnagazumi 0:9c9994747ea7 374 }//now(-800,13000)