涼太郎 中村 / Move

k

Dependents:   3rdcompfixstart 2ndcomp 4thcomp 6th33222_copy

Fork of Move by Tk A

move.cpp@13:4501c9202500, 2016-09-11 (annotated)

Committer:
choutin
Date:
Sun Sep 11 11:39:42 2016 +0000
Revision:
13:4501c9202500
Parent:
11:451b0c1d06b7
Child:
14:138af628d979
a

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