Tk A / Move2

aaa

Fork of Move by 涼太郎 中村

move.cpp@25:fa9b75c175fd, 2016-09-10 (annotated)

Committer:
sakanakuuun
Date:
Sat Sep 10 17:33:19 2016 +0000
Revision:
25:fa9b75c175fd
Parent:
24:d041fc34d846
Child:
26:0584d68e8f30
very little SYUSEI

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakanakuuun 0:d7ff86f25eaa 1 #include "mbed.h"
sakanakuuun 0:d7ff86f25eaa 2 #include "move.h"
sakanakuuun 0:d7ff86f25eaa 3 #include "locate.h"
sakanakuuun 2:f25a09c5e113 4 #include "stdlib.h"
sakanakuuun 18:4c812a3c7411 5 #include "math.h"
sakanakuuun 0:d7ff86f25eaa 6
sakanakuuun 24:d041fc34d846 7 /*************/
sakanakuuun 24:d041fc34d846 8 const int rightspeed = 70;
sakanakuuun 24:d041fc34d846 9 const int leftspeed = rightspeed + 4;
sakanakuuun 24:d041fc34d846 10 const int hosei_turnspeed = 13;
sakanakuuun 24:d041fc34d846 11 const int max_disorder = 3;
sakanakuuun 24:d041fc34d846 12 const float ratio = 1.0/8.5;
sakanakuuun 24:d041fc34d846 13 /*************/
sakanakuuun 24:d041fc34d846 14
sakanakuuun 0:d7ff86f25eaa 15 PwmOut M1cw(PA_11);
sakanakuuun 0:d7ff86f25eaa 16 PwmOut M1ccw(PB_15);
sakanakuuun 0:d7ff86f25eaa 17 PwmOut M2ccw(PB_14);
sakanakuuun 0:d7ff86f25eaa 18 PwmOut M2cw(PB_13);
sakanakuuun 7:7f1721542753 19
sakanakuuun 7:7f1721542753 20 DigitalOut green (PC_2);
sakanakuuun 7:7f1721542753 21 DigitalOut yellow(PC_3);
sakanakuuun 7:7f1721542753 22 DigitalOut red (PC_0);
sakanakuuun 7:7f1721542753 23
sakanakuuun 7:7f1721542753 24 Serial pc2(SERIAL_TX, SERIAL_RX); //pcと通信
sakanakuuun 2:f25a09c5e113 25
sakanakuuun 0:d7ff86f25eaa 26 void initmotor()
sakanakuuun 0:d7ff86f25eaa 27 {
sakanakuuun 0:d7ff86f25eaa 28 M1cw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 29 M1ccw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 30 M2cw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 31 M2ccw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 32
sakanakuuun 0:d7ff86f25eaa 33 }
sakanakuuun 0:d7ff86f25eaa 34
sakanakuuun 0:d7ff86f25eaa 35 void move(int left,int right)
sakanakuuun 0:d7ff86f25eaa 36 {
sakanakuuun 0:d7ff86f25eaa 37
sakanakuuun 0:d7ff86f25eaa 38 float rightduty,leftduty;
sakanakuuun 0:d7ff86f25eaa 39
sakanakuuun 0:d7ff86f25eaa 40 if(right>256) {
sakanakuuun 0:d7ff86f25eaa 41 right=256;
sakanakuuun 0:d7ff86f25eaa 42 }
sakanakuuun 0:d7ff86f25eaa 43 if(left>256) {
sakanakuuun 0:d7ff86f25eaa 44 left=256;
sakanakuuun 0:d7ff86f25eaa 45 }
sakanakuuun 0:d7ff86f25eaa 46 if(right<-256) {
sakanakuuun 0:d7ff86f25eaa 47 right=-256;
sakanakuuun 0:d7ff86f25eaa 48 }
sakanakuuun 0:d7ff86f25eaa 49 if(left<-256) {
sakanakuuun 0:d7ff86f25eaa 50 left=-256;
sakanakuuun 0:d7ff86f25eaa 51 }
sakanakuuun 0:d7ff86f25eaa 52
sakanakuuun 0:d7ff86f25eaa 53 rightduty=right/256.0;
sakanakuuun 0:d7ff86f25eaa 54 leftduty=left/256.0;
sakanakuuun 0:d7ff86f25eaa 55 if(right>0) {
sakanakuuun 0:d7ff86f25eaa 56 M1cw.write(1-rightduty);
sakanakuuun 0:d7ff86f25eaa 57 M1ccw.write(1);
sakanakuuun 0:d7ff86f25eaa 58 } else {
sakanakuuun 0:d7ff86f25eaa 59 M1cw.write(1);
sakanakuuun 0:d7ff86f25eaa 60 M1ccw.write(1+rightduty);
sakanakuuun 0:d7ff86f25eaa 61 }
sakanakuuun 0:d7ff86f25eaa 62
sakanakuuun 0:d7ff86f25eaa 63 if(left>0) {
sakanakuuun 0:d7ff86f25eaa 64 M2cw.write(1-leftduty);
sakanakuuun 0:d7ff86f25eaa 65 M2ccw.write(1);
sakanakuuun 0:d7ff86f25eaa 66 } else {
sakanakuuun 0:d7ff86f25eaa 67 M2cw.write(1);
sakanakuuun 0:d7ff86f25eaa 68 M2ccw.write(1+leftduty);
sakanakuuun 0:d7ff86f25eaa 69 }
sakanakuuun 0:d7ff86f25eaa 70 }
sakanakuuun 0:d7ff86f25eaa 71
sakanakuuun 0:d7ff86f25eaa 72 void hosei_turn(float pt, bool cw, float rad)
sakanakuuun 0:d7ff86f25eaa 73 {
sakanakuuun 0:d7ff86f25eaa 74 int np;
sakanakuuun 0:d7ff86f25eaa 75 if(cw) np = 1;
sakanakuuun 0:d7ff86f25eaa 76 else np = -1;
sakanakuuun 0:d7ff86f25eaa 77 while(1) {
sakanakuuun 14:db58d3b0ecde 78 update_np();
sakanakuuun 0:d7ff86f25eaa 79 if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
sakanakuuun 24:d041fc34d846 80 move(-hosei_turnspeed, hosei_turnspeed);
sakanakuuun 0:d7ff86f25eaa 81 } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
sakanakuuun 24:d041fc34d846 82 move(hosei_turnspeed, -hosei_turnspeed);
sakanakuuun 0:d7ff86f25eaa 83 } else {
sakanakuuun 0:d7ff86f25eaa 84 move(0,0);
sakanakuuun 0:d7ff86f25eaa 85 return;
sakanakuuun 0:d7ff86f25eaa 86 }
sakanakuuun 0:d7ff86f25eaa 87 }
sakanakuuun 2:f25a09c5e113 88
sakanakuuun 0:d7ff86f25eaa 89 }
sakanakuuun 0:d7ff86f25eaa 90
sakanakuuun 15:403e9c57c1a1 91 void turnrad(float rad)
sakanakuuun 0:d7ff86f25eaa 92 {
sakanakuuun 7:7f1721542753 93 green = 1;
sakanakuuun 7:7f1721542753 94
sakanakuuun 14:db58d3b0ecde 95 update_np();
sakanakuuun 0:d7ff86f25eaa 96
sakanakuuun 1:405e28b64fdb 97 int np;
sakanakuuun 1:405e28b64fdb 98 if(coordinateTheta() > rad) np = 1;
sakanakuuun 1:405e28b64fdb 99 else if(coordinateTheta() < rad) np = -1;
sakanakuuun 1:405e28b64fdb 100 else return;
sakanakuuun 1:405e28b64fdb 101
sakanakuuun 24:d041fc34d846 102 move((-np)*rightspeed, (np)*leftspeed);
sakanakuuun 0:d7ff86f25eaa 103
sakanakuuun 0:d7ff86f25eaa 104 while(1) {
sakanakuuun 14:db58d3b0ecde 105 update_np();
sakanakuuun 2:f25a09c5e113 106 if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
sakanakuuun 0:d7ff86f25eaa 107 move(0,0);
sakanakuuun 0:d7ff86f25eaa 108 break;
sakanakuuun 0:d7ff86f25eaa 109 }
sakanakuuun 0:d7ff86f25eaa 110 }
sakanakuuun 0:d7ff86f25eaa 111
sakanakuuun 2:f25a09c5e113 112 hosei_turn(0, false, rad);
sakanakuuun 2:f25a09c5e113 113 wait(0.5);
sakanakuuun 2:f25a09c5e113 114 hosei_turn(0, false, rad);
sakanakuuun 2:f25a09c5e113 115 wait(0.5);
sakanakuuun 7:7f1721542753 116 green = 0;
sakanakuuun 1:405e28b64fdb 117 }
sakanakuuun 1:405e28b64fdb 118
sakanakuuun 15:403e9c57c1a1 119
sakanakuuun 15:403e9c57c1a1 120 void turnrad_ccw(float rad)
sakanakuuun 15:403e9c57c1a1 121 {
sakanakuuun 15:403e9c57c1a1 122 green = 1;
sakanakuuun 15:403e9c57c1a1 123
sakanakuuun 15:403e9c57c1a1 124 update();
sakanakuuun 15:403e9c57c1a1 125
sakanakuuun 24:d041fc34d846 126 move(rightspeed, -leftspeed);
sakanakuuun 15:403e9c57c1a1 127
sakanakuuun 15:403e9c57c1a1 128 while(1) {
sakanakuuun 15:403e9c57c1a1 129 update();
sakanakuuun 15:403e9c57c1a1 130 if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
sakanakuuun 15:403e9c57c1a1 131 move(0,0);
sakanakuuun 15:403e9c57c1a1 132 break;
sakanakuuun 15:403e9c57c1a1 133 }
sakanakuuun 15:403e9c57c1a1 134 }
sakanakuuun 15:403e9c57c1a1 135
sakanakuuun 15:403e9c57c1a1 136 hosei_turn(0, false, rad);
sakanakuuun 15:403e9c57c1a1 137 wait(0.5);
sakanakuuun 15:403e9c57c1a1 138 hosei_turn(0, false, rad);
sakanakuuun 15:403e9c57c1a1 139 wait(0.5);
sakanakuuun 15:403e9c57c1a1 140 green = 0;
sakanakuuun 15:403e9c57c1a1 141 }
sakanakuuun 15:403e9c57c1a1 142
sakanakuuun 15:403e9c57c1a1 143 void turnrad_cw(float rad)
sakanakuuun 15:403e9c57c1a1 144 {
sakanakuuun 15:403e9c57c1a1 145 green = 1;
sakanakuuun 15:403e9c57c1a1 146
sakanakuuun 15:403e9c57c1a1 147 update();
sakanakuuun 15:403e9c57c1a1 148
sakanakuuun 24:d041fc34d846 149 move((-1)*rightspeed, leftspeed);
sakanakuuun 15:403e9c57c1a1 150
sakanakuuun 15:403e9c57c1a1 151 while(1) {
sakanakuuun 15:403e9c57c1a1 152 update();
sakanakuuun 15:403e9c57c1a1 153 if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
sakanakuuun 15:403e9c57c1a1 154 move(0,0);
sakanakuuun 15:403e9c57c1a1 155 break;
sakanakuuun 15:403e9c57c1a1 156 }
sakanakuuun 15:403e9c57c1a1 157 }
sakanakuuun 15:403e9c57c1a1 158
sakanakuuun 15:403e9c57c1a1 159 hosei_turn(0, false, rad);
sakanakuuun 18:4c812a3c7411 160 wait(0.2);
sakanakuuun 15:403e9c57c1a1 161 hosei_turn(0, false, rad);
sakanakuuun 18:4c812a3c7411 162 wait(0.2);
sakanakuuun 15:403e9c57c1a1 163 green = 0;
sakanakuuun 15:403e9c57c1a1 164 }
sakanakuuun 15:403e9c57c1a1 165
sakanakuuun 5:0e18cf25291a 166 void pmove(int x, int y)
sakanakuuun 7:7f1721542753 167 {
sakanakuuun 7:7f1721542753 168 yellow = 1;
sakanakuuun 7:7f1721542753 169
sakanakuuun 14:db58d3b0ecde 170 float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
sakanakuuun 14:db58d3b0ecde 171 int k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
sakanakuuun 7:7f1721542753 172
sakanakuuun 4:4c574be6325c 173 int length, dx, dy;
sakanakuuun 2:f25a09c5e113 174 int *d_length, *disorder;
sakanakuuun 5:0e18cf25291a 175 int absd_length;
sakanakuuun 4:4c574be6325c 176 float dtheta, ptheta;
sakanakuuun 2:f25a09c5e113 177 float daikei;
sakanakuuun 1:405e28b64fdb 178
sakanakuuun 7:7f1721542753 179 int direction;
sakanakuuun 18:4c812a3c7411 180
sakanakuuun 5:0e18cf25291a 181 if(abs(x - coordinateX()) > abs(y - coordinateY())) {
sakanakuuun 5:0e18cf25291a 182 y = coordinateY();
sakanakuuun 5:0e18cf25291a 183 direction = X_PLUS;
sakanakuuun 5:0e18cf25291a 184 length = abs(x - coordinateX());
sakanakuuun 5:0e18cf25291a 185 d_length = &dx;
sakanakuuun 5:0e18cf25291a 186 disorder = &dy;
sakanakuuun 7:7f1721542753 187 } else {
sakanakuuun 5:0e18cf25291a 188 x = coordinateX();
sakanakuuun 5:0e18cf25291a 189 direction = Y_PLUS;
sakanakuuun 5:0e18cf25291a 190 length = abs(y - coordinateY());
sakanakuuun 5:0e18cf25291a 191 d_length = &dy;
sakanakuuun 5:0e18cf25291a 192 disorder = &dx;
sakanakuuun 5:0e18cf25291a 193 }
sakanakuuun 18:4c812a3c7411 194
sakanakuuun 7:7f1721542753 195 update();
sakanakuuun 7:7f1721542753 196 dx = x - coordinateX();
sakanakuuun 7:7f1721542753 197 dy = y - coordinateY();
sakanakuuun 18:4c812a3c7411 198
sakanakuuun 7:7f1721542753 199 if(*d_length < 0) //x,y減少方向なら、*d_length<0
sakanakuuun 5:0e18cf25291a 200 direction *= -1;
sakanakuuun 1:405e28b64fdb 201
sakanakuuun 7:7f1721542753 202 pc2.printf("direction:%d", direction);
sakanakuuun 7:7f1721542753 203
sakanakuuun 2:f25a09c5e113 204 switch(direction) {
sakanakuuun 2:f25a09c5e113 205 case X_PLUS:
sakanakuuun 2:f25a09c5e113 206 ptheta = 0;
sakanakuuun 2:f25a09c5e113 207 break;
sakanakuuun 2:f25a09c5e113 208 case Y_PLUS:
sakanakuuun 2:f25a09c5e113 209 k *= -1;
sakanakuuun 2:f25a09c5e113 210 ptheta = PI/2;
sakanakuuun 2:f25a09c5e113 211 break;
sakanakuuun 2:f25a09c5e113 212 case X_MINUS:
sakanakuuun 2:f25a09c5e113 213 k *= -1;
sakanakuuun 2:f25a09c5e113 214 ptheta = PI;
sakanakuuun 2:f25a09c5e113 215 break;
sakanakuuun 2:f25a09c5e113 216 case Y_MINUS:
choutin 10:6d38d1b6cad5 217 ptheta = -PI/2;
sakanakuuun 2:f25a09c5e113 218 break;
sakanakuuun 2:f25a09c5e113 219 default:
sakanakuuun 2:f25a09c5e113 220 return;
sakanakuuun 2:f25a09c5e113 221 }
sakanakuuun 18:4c812a3c7411 222
sakanakuuun 13:176e543e6d64 223 ptheta += nearPi(coordinateTheta() - ptheta);
sakanakuuun 18:4c812a3c7411 224
sakanakuuun 15:403e9c57c1a1 225 turnrad(ptheta);
sakanakuuun 5:0e18cf25291a 226
sakanakuuun 2:f25a09c5e113 227 if(length == 0) return;
sakanakuuun 18:4c812a3c7411 228
sakanakuuun 14:db58d3b0ecde 229 int i = 0;
sakanakuuun 18:4c812a3c7411 230
sakanakuuun 1:405e28b64fdb 231 while(1) {
sakanakuuun 14:db58d3b0ecde 232 update_np();
sakanakuuun 5:0e18cf25291a 233 dx = x - coordinateX();
sakanakuuun 5:0e18cf25291a 234 dy = y - coordinateY();
sakanakuuun 2:f25a09c5e113 235 dtheta = coordinateTheta() - ptheta;
sakanakuuun 2:f25a09c5e113 236
sakanakuuun 12:f41918f71131 237 if(*disorder>max_disorder) {
sakanakuuun 12:f41918f71131 238 *disorder = max_disorder;
sakanakuuun 12:f41918f71131 239 } else if(*disorder<-max_disorder) {
sakanakuuun 12:f41918f71131 240 *disorder = -max_disorder;
sakanakuuun 1:405e28b64fdb 241 }
sakanakuuun 1:405e28b64fdb 242
sakanakuuun 5:0e18cf25291a 243 absd_length = abs(*d_length);
sakanakuuun 18:4c812a3c7411 244
sakanakuuun 18:4c812a3c7411 245
sakanakuuun 14:db58d3b0ecde 246 if(i++ < 5) {
sakanakuuun 14:db58d3b0ecde 247 daikei = i/5;
sakanakuuun 18:4c812a3c7411 248 } else if(absd_length < 300) {
sakanakuuun 13:176e543e6d64 249 daikei = absd_length / 300.0;
sakanakuuun 18:4c812a3c7411 250 }
sakanakuuun 14:db58d3b0ecde 251 else
sakanakuuun 4:4c574be6325c 252 daikei = 1;
sakanakuuun 4:4c574be6325c 253
sakanakuuun 24:d041fc34d846 254 move(daikei * (rightspeed*(1-ratio) + k*(*disorder) - k_theta*dtheta) + rightspeed*ratio,
sakanakuuun 24:d041fc34d846 255 daikei * (leftspeed *(1-ratio) - k*(*disorder) + k_theta*dtheta) + leftspeed *ratio);
sakanakuuun 4:4c574be6325c 256
choutin 9:7e99a1c80656 257 //pc2.printf("d_length:%d disorder:%d rs:%f ls:%f daikei:%f\n\r", *d_length, *disorder, k*(*disorder) - k_theta*dtheta, -k*(*disorder) + k_theta*dtheta, daikei);
sakanakuuun 5:0e18cf25291a 258 if((direction > 0 && *d_length <= 0) || (direction < 0 && *d_length >= 0)) {
sakanakuuun 2:f25a09c5e113 259 move(0, 0);
sakanakuuun 1:405e28b64fdb 260 break;
sakanakuuun 1:405e28b64fdb 261 }
sakanakuuun 0:d7ff86f25eaa 262
sakanakuuun 0:d7ff86f25eaa 263 }
sakanakuuun 0:d7ff86f25eaa 264
sakanakuuun 18:4c812a3c7411 265 wait(0.2);
sakanakuuun 7:7f1721542753 266
sakanakuuun 7:7f1721542753 267 yellow = 0;
sakanakuuun 0:d7ff86f25eaa 268 }
sakanakuuun 3:cecaa0154f92 269
sakanakuuun 18:4c812a3c7411 270 void pmove2(int x, int y)
sakanakuuun 18:4c812a3c7411 271 {
sakanakuuun 18:4c812a3c7411 272 yellow = 1;
sakanakuuun 18:4c812a3c7411 273 red=0;
sakanakuuun 18:4c812a3c7411 274 float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
sakanakuuun 18:4c812a3c7411 275 int k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
sakanakuuun 18:4c812a3c7411 276
sakanakuuun 18:4c812a3c7411 277 double length;
sakanakuuun 18:4c812a3c7411 278 int d_length, disorder;
sakanakuuun 18:4c812a3c7411 279 float dtheta, ptheta;
sakanakuuun 18:4c812a3c7411 280 float daikei;
sakanakuuun 18:4c812a3c7411 281
sakanakuuun 22:eed10c0b7fe0 282 length = sqrt(pow((double)(x - coordinateX()), 2) + pow((double)(y - coordinateY()), 2));
sakanakuuun 22:eed10c0b7fe0 283
sakanakuuun 18:4c812a3c7411 284 pc2.printf("length:%f", length);
sakanakuuun 22:eed10c0b7fe0 285
sakanakuuun 18:4c812a3c7411 286 if(length == 0) {
sakanakuuun 18:4c812a3c7411 287 red=1;
sakanakuuun 18:4c812a3c7411 288 return;
sakanakuuun 18:4c812a3c7411 289 }
sakanakuuun 18:4c812a3c7411 290
sakanakuuun 18:4c812a3c7411 291 ptheta = giveatan(x, y);
sakanakuuun 22:eed10c0b7fe0 292
sakanakuuun 19:0b0ed6b22de9 293 ptheta += nearPi(coordinateTheta() - ptheta);
sakanakuuun 22:eed10c0b7fe0 294
sakanakuuun 18:4c812a3c7411 295 turnrad(ptheta);
sakanakuuun 18:4c812a3c7411 296
sakanakuuun 18:4c812a3c7411 297 virtual_setup();
sakanakuuun 18:4c812a3c7411 298
sakanakuuun 18:4c812a3c7411 299 int i = 0;
sakanakuuun 18:4c812a3c7411 300
sakanakuuun 18:4c812a3c7411 301 while(1) {
sakanakuuun 18:4c812a3c7411 302 update_np();
sakanakuuun 18:4c812a3c7411 303 virtual_update();
sakanakuuun 18:4c812a3c7411 304
sakanakuuun 18:4c812a3c7411 305 d_length = length - virtual_coordinateX();
sakanakuuun 18:4c812a3c7411 306 disorder = virtual_coordinateY();
sakanakuuun 18:4c812a3c7411 307 dtheta = virtual_coordinateTheta();
sakanakuuun 18:4c812a3c7411 308
sakanakuuun 18:4c812a3c7411 309 if(disorder>max_disorder) {
sakanakuuun 18:4c812a3c7411 310 disorder = max_disorder;
sakanakuuun 18:4c812a3c7411 311 } else if(disorder<-max_disorder) {
sakanakuuun 18:4c812a3c7411 312 disorder = -max_disorder;
sakanakuuun 18:4c812a3c7411 313 }
sakanakuuun 18:4c812a3c7411 314
sakanakuuun 18:4c812a3c7411 315 if(i++ < 5) {
sakanakuuun 18:4c812a3c7411 316 daikei = i/5;
sakanakuuun 18:4c812a3c7411 317 } else if(d_length < 300) {
sakanakuuun 18:4c812a3c7411 318 daikei = d_length / 300.0;
sakanakuuun 22:eed10c0b7fe0 319 } else
sakanakuuun 18:4c812a3c7411 320 daikei = 1;
sakanakuuun 18:4c812a3c7411 321
sakanakuuun 24:d041fc34d846 322 move(daikei * (rightspeed*(1-ratio) - k*disorder - k_theta*dtheta) + rightspeed*ratio,
sakanakuuun 24:d041fc34d846 323 daikei * (leftspeed *(1-ratio) + k*disorder + k_theta*dtheta) + leftspeed *ratio);
sakanakuuun 18:4c812a3c7411 324
sakanakuuun 18:4c812a3c7411 325
sakanakuuun 25:fa9b75c175fd 326 //pc2.printf("length:%f, d_length:%d, vx:%d, vy:%d\n\r", length, d_length, virtual_coordinateX(), virtual_coordinateY());
sakanakuuun 22:eed10c0b7fe0 327 if(d_length <= 0) {
sakanakuuun 18:4c812a3c7411 328 move(0, 0);
sakanakuuun 18:4c812a3c7411 329 break;
sakanakuuun 18:4c812a3c7411 330 }
sakanakuuun 18:4c812a3c7411 331
sakanakuuun 18:4c812a3c7411 332 }
sakanakuuun 18:4c812a3c7411 333
sakanakuuun 18:4c812a3c7411 334 wait(0.2);
sakanakuuun 18:4c812a3c7411 335
sakanakuuun 18:4c812a3c7411 336 yellow = 0;
sakanakuuun 22:eed10c0b7fe0 337 red = 0;
sakanakuuun 18:4c812a3c7411 338 }
sakanakuuun 18:4c812a3c7411 339
sakanakuuun 6:0aa97a99c9cb 340 void back300()
sakanakuuun 7:7f1721542753 341 {
sakanakuuun 7:7f1721542753 342 red = 1;
sakanakuuun 7:7f1721542753 343
sakanakuuun 4:4c574be6325c 344 float k = 0.9;
sakanakuuun 6:0aa97a99c9cb 345 int length, px, py, dx, dy;
sakanakuuun 7:7f1721542753 346
sakanakuuun 6:0aa97a99c9cb 347 update();
sakanakuuun 7:7f1721542753 348
sakanakuuun 7:7f1721542753 349 px = coordinateX();
sakanakuuun 7:7f1721542753 350 py = coordinateY();
sakanakuuun 7:7f1721542753 351
sakanakuuun 6:0aa97a99c9cb 352 length = 300;
sakanakuuun 7:7f1721542753 353
sakanakuuun 15:403e9c57c1a1 354 turnrad(PI + nearPi(coordinateTheta() - PI));
sakanakuuun 7:7f1721542753 355
sakanakuuun 4:4c574be6325c 356 while(1) {
sakanakuuun 14:db58d3b0ecde 357 update_np();
sakanakuuun 6:0aa97a99c9cb 358 dx = coordinateX() - px;
sakanakuuun 6:0aa97a99c9cb 359 dy = coordinateY() - py;
sakanakuuun 4:4c574be6325c 360
sakanakuuun 12:f41918f71131 361 if(dy>max_disorder) {
sakanakuuun 12:f41918f71131 362 dy = max_disorder;
sakanakuuun 12:f41918f71131 363 } else if(dy<-max_disorder) {
sakanakuuun 12:f41918f71131 364 dy = -max_disorder;
sakanakuuun 6:0aa97a99c9cb 365 }
sakanakuuun 7:7f1721542753 366
sakanakuuun 8:3bf4addaaedd 367 move(-(30 + k*dy), -(32 - k*dy));
sakanakuuun 4:4c574be6325c 368
sakanakuuun 6:0aa97a99c9cb 369 if(dx>length) {
sakanakuuun 4:4c574be6325c 370 move(0, 0);
sakanakuuun 4:4c574be6325c 371 break;
sakanakuuun 4:4c574be6325c 372 }
sakanakuuun 4:4c574be6325c 373 }
sakanakuuun 4:4c574be6325c 374
sakanakuuun 18:4c812a3c7411 375 wait(0.2);
sakanakuuun 7:7f1721542753 376
sakanakuuun 7:7f1721542753 377 red = 0;
sakanakuuun 4:4c574be6325c 378 }
sakanakuuun 12:f41918f71131 379
sakanakuuun 16:140e758346ae 380
sakanakuuun 22:eed10c0b7fe0 381 void nxback300(int team)
sakanakuuun 16:140e758346ae 382 {
sakanakuuun 16:140e758346ae 383 red = 1;
sakanakuuun 16:140e758346ae 384
sakanakuuun 16:140e758346ae 385 float k = 0.9;
sakanakuuun 16:140e758346ae 386 int length, px, py, dx, dy;
sakanakuuun 16:140e758346ae 387
sakanakuuun 16:140e758346ae 388 update();
sakanakuuun 16:140e758346ae 389
sakanakuuun 16:140e758346ae 390 px = coordinateX();
sakanakuuun 16:140e758346ae 391 py = coordinateY();
sakanakuuun 16:140e758346ae 392
sakanakuuun 16:140e758346ae 393 length = 300;
sakanakuuun 16:140e758346ae 394
sakanakuuun 16:140e758346ae 395 turnrad(nearPi(coordinateTheta()));
sakanakuuun 16:140e758346ae 396
sakanakuuun 16:140e758346ae 397 while(1) {
sakanakuuun 16:140e758346ae 398 update_np();
sakanakuuun 16:140e758346ae 399 dx = coordinateX() - px;
sakanakuuun 16:140e758346ae 400 dy = coordinateY() - py;
sakanakuuun 16:140e758346ae 401
sakanakuuun 16:140e758346ae 402 if(dy>max_disorder) {
sakanakuuun 16:140e758346ae 403 dy = max_disorder;
sakanakuuun 16:140e758346ae 404 } else if(dy<-max_disorder) {
sakanakuuun 16:140e758346ae 405 dy = -max_disorder;
sakanakuuun 16:140e758346ae 406 }
sakanakuuun 16:140e758346ae 407
sakanakuuun 16:140e758346ae 408 move(-(30 - k*dy), -(32 + k*dy));
sakanakuuun 16:140e758346ae 409
sakanakuuun 16:140e758346ae 410 if(abs(dx)>length) {
sakanakuuun 16:140e758346ae 411 move(0, 0);
sakanakuuun 16:140e758346ae 412 break;
sakanakuuun 16:140e758346ae 413 }
sakanakuuun 16:140e758346ae 414 }
sakanakuuun 16:140e758346ae 415
sakanakuuun 18:4c812a3c7411 416 wait(0.2);
sakanakuuun 16:140e758346ae 417
sakanakuuun 16:140e758346ae 418 red = 0;
sakanakuuun 16:140e758346ae 419 }
sakanakuuun 16:140e758346ae 420
sakanakuuun 16:140e758346ae 421
sakanakuuun 22:eed10c0b7fe0 422 void pyback300(int team)
sakanakuuun 17:c167f4ed9070 423 {
sakanakuuun 22:eed10c0b7fe0 424 if(team == 0) {
sakanakuuun 22:eed10c0b7fe0 425 nyback300(1);
sakanakuuun 22:eed10c0b7fe0 426 return;
sakanakuuun 22:eed10c0b7fe0 427 }
sakanakuuun 22:eed10c0b7fe0 428
sakanakuuun 17:c167f4ed9070 429 red = 1;
sakanakuuun 17:c167f4ed9070 430
sakanakuuun 17:c167f4ed9070 431 float k = 0.9;
sakanakuuun 17:c167f4ed9070 432 int length, px, py, dx, dy;
sakanakuuun 17:c167f4ed9070 433
sakanakuuun 17:c167f4ed9070 434 update();
sakanakuuun 17:c167f4ed9070 435
sakanakuuun 17:c167f4ed9070 436 px = coordinateX();
sakanakuuun 17:c167f4ed9070 437 py = coordinateY();
sakanakuuun 17:c167f4ed9070 438
sakanakuuun 17:c167f4ed9070 439 length = 300;
sakanakuuun 17:c167f4ed9070 440
sakanakuuun 18:4c812a3c7411 441 turnrad(PI/2 + nearPi(coordinateTheta() - PI/2));
sakanakuuun 17:c167f4ed9070 442
sakanakuuun 17:c167f4ed9070 443 while(1) {
sakanakuuun 17:c167f4ed9070 444 update_np();
sakanakuuun 17:c167f4ed9070 445 dx = coordinateX() - px;
sakanakuuun 17:c167f4ed9070 446 dy = coordinateY() - py;
sakanakuuun 17:c167f4ed9070 447
sakanakuuun 17:c167f4ed9070 448 if(dx>max_disorder) {
sakanakuuun 17:c167f4ed9070 449 dx = max_disorder;
sakanakuuun 17:c167f4ed9070 450 } else if(dx<-max_disorder) {
sakanakuuun 17:c167f4ed9070 451 dx = -max_disorder;
sakanakuuun 17:c167f4ed9070 452 }
sakanakuuun 17:c167f4ed9070 453
sakanakuuun 17:c167f4ed9070 454 move(-(30 - k*dx), -(32 + k*dx));
sakanakuuun 17:c167f4ed9070 455
sakanakuuun 17:c167f4ed9070 456 if(dy>length) {
sakanakuuun 17:c167f4ed9070 457 move(0, 0);
sakanakuuun 17:c167f4ed9070 458 break;
sakanakuuun 17:c167f4ed9070 459 }
sakanakuuun 17:c167f4ed9070 460
sakanakuuun 17:c167f4ed9070 461 }
sakanakuuun 17:c167f4ed9070 462
sakanakuuun 18:4c812a3c7411 463 wait(0.2);
sakanakuuun 17:c167f4ed9070 464
sakanakuuun 24:d041fc34d846 465 red = 0;
sakanakuuun 17:c167f4ed9070 466 }
sakanakuuun 17:c167f4ed9070 467
sakanakuuun 22:eed10c0b7fe0 468 void nyback300(int team)
sakanakuuun 18:4c812a3c7411 469 {
sakanakuuun 24:d041fc34d846 470 if(team == 0) {
sakanakuuun 22:eed10c0b7fe0 471 pyback300(1);
sakanakuuun 22:eed10c0b7fe0 472 return;
sakanakuuun 22:eed10c0b7fe0 473 }
sakanakuuun 24:d041fc34d846 474
sakanakuuun 22:eed10c0b7fe0 475 red = 1;
sakanakuuun 18:4c812a3c7411 476
sakanakuuun 22:eed10c0b7fe0 477 float k = 0.9;
sakanakuuun 22:eed10c0b7fe0 478 int length, px, py, dx, dy;
sakanakuuun 18:4c812a3c7411 479
sakanakuuun 22:eed10c0b7fe0 480 update();
sakanakuuun 18:4c812a3c7411 481
sakanakuuun 22:eed10c0b7fe0 482 px = coordinateX();
sakanakuuun 22:eed10c0b7fe0 483 py = coordinateY();
sakanakuuun 18:4c812a3c7411 484
sakanakuuun 22:eed10c0b7fe0 485 length = 300;
sakanakuuun 18:4c812a3c7411 486
sakanakuuun 22:eed10c0b7fe0 487 turnrad(-PI/2 + nearPi(coordinateTheta() + PI/2));
sakanakuuun 18:4c812a3c7411 488
sakanakuuun 22:eed10c0b7fe0 489 while(1) {
sakanakuuun 22:eed10c0b7fe0 490 update_np();
sakanakuuun 22:eed10c0b7fe0 491 dx = coordinateX() - px;
sakanakuuun 22:eed10c0b7fe0 492 dy = coordinateY() - py;
sakanakuuun 18:4c812a3c7411 493
sakanakuuun 22:eed10c0b7fe0 494 if(dx>max_disorder) {
sakanakuuun 22:eed10c0b7fe0 495 dx = max_disorder;
sakanakuuun 22:eed10c0b7fe0 496 } else if(dx<-max_disorder) {
sakanakuuun 22:eed10c0b7fe0 497 dx = -max_disorder;
sakanakuuun 22:eed10c0b7fe0 498 }
sakanakuuun 18:4c812a3c7411 499
sakanakuuun 22:eed10c0b7fe0 500 move(-(30 - k*dx), -(32 + k*dx));
sakanakuuun 18:4c812a3c7411 501
sakanakuuun 22:eed10c0b7fe0 502 if(abs(dy)>length) {
sakanakuuun 22:eed10c0b7fe0 503 move(0, 0);
sakanakuuun 22:eed10c0b7fe0 504 break;
sakanakuuun 18:4c812a3c7411 505 }
sakanakuuun 22:eed10c0b7fe0 506 }
sakanakuuun 18:4c812a3c7411 507
sakanakuuun 22:eed10c0b7fe0 508 wait(0.2);
sakanakuuun 22:eed10c0b7fe0 509
sakanakuuun 22:eed10c0b7fe0 510 red = 0;
sakanakuuun 22:eed10c0b7fe0 511
sakanakuuun 18:4c812a3c7411 512 }
sakanakuuun 17:c167f4ed9070 513
sakanakuuun 12:f41918f71131 514 float nearPi(float rad)
sakanakuuun 12:f41918f71131 515 {
sakanakuuun 12:f41918f71131 516 float npi = 0;
sakanakuuun 18:4c812a3c7411 517
sakanakuuun 18:4c812a3c7411 518 while(1) {
sakanakuuun 12:f41918f71131 519 if(rad > npi + PI)
sakanakuuun 12:f41918f71131 520 npi += 2*PI;
sakanakuuun 12:f41918f71131 521 else if(rad < npi - PI)
sakanakuuun 12:f41918f71131 522 npi -= 2*PI;
sakanakuuun 12:f41918f71131 523 else
sakanakuuun 12:f41918f71131 524 return npi;
sakanakuuun 12:f41918f71131 525 }
sakanakuuun 12:f41918f71131 526 }
sakanakuuun 24:d041fc34d846 527
sakanakuuun 24:d041fc34d846 528 float giveatan(int targetx,int targety)
sakanakuuun 24:d041fc34d846 529 {
sakanakuuun 24:d041fc34d846 530 int x,y;
sakanakuuun 24:d041fc34d846 531 float phi;
sakanakuuun 24:d041fc34d846 532 update();
sakanakuuun 24:d041fc34d846 533 x = coordinateX();
sakanakuuun 24:d041fc34d846 534 y = coordinateY();
sakanakuuun 24:d041fc34d846 535
sakanakuuun 24:d041fc34d846 536 if(targetx - x == 0) {
sakanakuuun 24:d041fc34d846 537 if(targety > 0)
sakanakuuun 24:d041fc34d846 538 return PI/2;
sakanakuuun 24:d041fc34d846 539 else if(targety < 0)
sakanakuuun 24:d041fc34d846 540 return -PI/2;
sakanakuuun 24:d041fc34d846 541 else
sakanakuuun 24:d041fc34d846 542 return 0;
sakanakuuun 24:d041fc34d846 543 }
sakanakuuun 24:d041fc34d846 544
sakanakuuun 24:d041fc34d846 545 phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得
sakanakuuun 24:d041fc34d846 546 if(targetx - x < 0) {
sakanakuuun 24:d041fc34d846 547 if(targety - y > 0)
sakanakuuun 24:d041fc34d846 548 phi += PI;
sakanakuuun 24:d041fc34d846 549 else if(targety - y < 0)
sakanakuuun 24:d041fc34d846 550 phi -= PI;
sakanakuuun 24:d041fc34d846 551 }
sakanakuuun 24:d041fc34d846 552
sakanakuuun 24:d041fc34d846 553 return phi;
sakanakuuun 24:d041fc34d846 554 }