k

Dependents:   3rdcompfixstart 2ndcomp 4thcomp 6th33222_copy

Fork of Move by Tk A

Committer:
choutin
Date:
Sat Sep 17 23:20:56 2016 +0000
Revision:
15:39c7e97b37c4
Parent:
14:138af628d979
?

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