Tk A / Move2

aaa

Fork of Move by 涼太郎 中村

move.cpp@24:d041fc34d846, 2016-09-10 (annotated)

Committer:
sakanakuuun
Date:
Sat Sep 10 17:30:41 2016 +0000
Revision:
24:d041fc34d846
Parent:
23:e30ffaeb3e0f
Child:
25:fa9b75c175fd
very very 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 //pc.printf("t:%f\n\r", coordinateTheta());
sakanakuuun 15:403e9c57c1a1 131 if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
sakanakuuun 15:403e9c57c1a1 132 move(0,0);
sakanakuuun 15:403e9c57c1a1 133 break;
sakanakuuun 15:403e9c57c1a1 134 }
sakanakuuun 15:403e9c57c1a1 135 }
sakanakuuun 15:403e9c57c1a1 136
sakanakuuun 15:403e9c57c1a1 137 hosei_turn(0, false, rad);
sakanakuuun 15:403e9c57c1a1 138 wait(0.5);
sakanakuuun 15:403e9c57c1a1 139 hosei_turn(0, false, rad);
sakanakuuun 15:403e9c57c1a1 140 wait(0.5);
sakanakuuun 15:403e9c57c1a1 141 green = 0;
sakanakuuun 15:403e9c57c1a1 142 }
sakanakuuun 15:403e9c57c1a1 143
sakanakuuun 15:403e9c57c1a1 144 void turnrad_cw(float rad)
sakanakuuun 15:403e9c57c1a1 145 {
sakanakuuun 15:403e9c57c1a1 146 green = 1;
sakanakuuun 15:403e9c57c1a1 147
sakanakuuun 15:403e9c57c1a1 148 update();
sakanakuuun 15:403e9c57c1a1 149
sakanakuuun 24:d041fc34d846 150 move((-1)*rightspeed, leftspeed);
sakanakuuun 15:403e9c57c1a1 151
sakanakuuun 15:403e9c57c1a1 152 while(1) {
sakanakuuun 15:403e9c57c1a1 153 update();
sakanakuuun 15:403e9c57c1a1 154 //pc.printf("t:%f\n\r", coordinateTheta());
sakanakuuun 15:403e9c57c1a1 155 if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
sakanakuuun 15:403e9c57c1a1 156 move(0,0);
sakanakuuun 15:403e9c57c1a1 157 break;
sakanakuuun 15:403e9c57c1a1 158 }
sakanakuuun 15:403e9c57c1a1 159 }
sakanakuuun 15:403e9c57c1a1 160
sakanakuuun 15:403e9c57c1a1 161 hosei_turn(0, false, rad);
sakanakuuun 18:4c812a3c7411 162 wait(0.2);
sakanakuuun 15:403e9c57c1a1 163 hosei_turn(0, false, rad);
sakanakuuun 18:4c812a3c7411 164 wait(0.2);
sakanakuuun 15:403e9c57c1a1 165 green = 0;
sakanakuuun 15:403e9c57c1a1 166 }
sakanakuuun 15:403e9c57c1a1 167
sakanakuuun 5:0e18cf25291a 168 void pmove(int x, int y)
sakanakuuun 7:7f1721542753 169 {
sakanakuuun 7:7f1721542753 170 yellow = 1;
sakanakuuun 7:7f1721542753 171
sakanakuuun 14:db58d3b0ecde 172 float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
sakanakuuun 14:db58d3b0ecde 173 int k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
sakanakuuun 7:7f1721542753 174
sakanakuuun 4:4c574be6325c 175 int length, dx, dy;
sakanakuuun 2:f25a09c5e113 176 int *d_length, *disorder;
sakanakuuun 5:0e18cf25291a 177 int absd_length;
sakanakuuun 4:4c574be6325c 178 float dtheta, ptheta;
sakanakuuun 2:f25a09c5e113 179 float daikei;
sakanakuuun 1:405e28b64fdb 180
sakanakuuun 7:7f1721542753 181 int direction;
sakanakuuun 18:4c812a3c7411 182
sakanakuuun 5:0e18cf25291a 183 if(abs(x - coordinateX()) > abs(y - coordinateY())) {
sakanakuuun 5:0e18cf25291a 184 y = coordinateY();
sakanakuuun 5:0e18cf25291a 185 direction = X_PLUS;
sakanakuuun 5:0e18cf25291a 186 length = abs(x - coordinateX());
sakanakuuun 5:0e18cf25291a 187 d_length = &dx;
sakanakuuun 5:0e18cf25291a 188 disorder = &dy;
sakanakuuun 7:7f1721542753 189 } else {
sakanakuuun 5:0e18cf25291a 190 x = coordinateX();
sakanakuuun 5:0e18cf25291a 191 direction = Y_PLUS;
sakanakuuun 5:0e18cf25291a 192 length = abs(y - coordinateY());
sakanakuuun 5:0e18cf25291a 193 d_length = &dy;
sakanakuuun 5:0e18cf25291a 194 disorder = &dx;
sakanakuuun 5:0e18cf25291a 195 }
sakanakuuun 18:4c812a3c7411 196
sakanakuuun 7:7f1721542753 197 update();
sakanakuuun 7:7f1721542753 198 dx = x - coordinateX();
sakanakuuun 7:7f1721542753 199 dy = y - coordinateY();
sakanakuuun 18:4c812a3c7411 200
sakanakuuun 7:7f1721542753 201 if(*d_length < 0) //x,y減少方向なら、*d_length<0
sakanakuuun 5:0e18cf25291a 202 direction *= -1;
sakanakuuun 1:405e28b64fdb 203
sakanakuuun 7:7f1721542753 204 pc2.printf("direction:%d", direction);
sakanakuuun 7:7f1721542753 205
sakanakuuun 2:f25a09c5e113 206 switch(direction) {
sakanakuuun 2:f25a09c5e113 207 case X_PLUS:
sakanakuuun 2:f25a09c5e113 208 ptheta = 0;
sakanakuuun 2:f25a09c5e113 209 break;
sakanakuuun 2:f25a09c5e113 210 case Y_PLUS:
sakanakuuun 2:f25a09c5e113 211 k *= -1;
sakanakuuun 2:f25a09c5e113 212 ptheta = PI/2;
sakanakuuun 2:f25a09c5e113 213 break;
sakanakuuun 2:f25a09c5e113 214 case X_MINUS:
sakanakuuun 2:f25a09c5e113 215 k *= -1;
sakanakuuun 2:f25a09c5e113 216 ptheta = PI;
sakanakuuun 2:f25a09c5e113 217 break;
sakanakuuun 2:f25a09c5e113 218 case Y_MINUS:
choutin 10:6d38d1b6cad5 219 ptheta = -PI/2;
sakanakuuun 2:f25a09c5e113 220 break;
sakanakuuun 2:f25a09c5e113 221 default:
sakanakuuun 2:f25a09c5e113 222 return;
sakanakuuun 2:f25a09c5e113 223 }
sakanakuuun 18:4c812a3c7411 224
sakanakuuun 13:176e543e6d64 225 ptheta += nearPi(coordinateTheta() - ptheta);
sakanakuuun 18:4c812a3c7411 226
sakanakuuun 15:403e9c57c1a1 227 turnrad(ptheta);
sakanakuuun 5:0e18cf25291a 228
sakanakuuun 2:f25a09c5e113 229 if(length == 0) return;
sakanakuuun 18:4c812a3c7411 230
sakanakuuun 14:db58d3b0ecde 231 int i = 0;
sakanakuuun 18:4c812a3c7411 232
sakanakuuun 1:405e28b64fdb 233 while(1) {
sakanakuuun 14:db58d3b0ecde 234 update_np();
sakanakuuun 5:0e18cf25291a 235 dx = x - coordinateX();
sakanakuuun 5:0e18cf25291a 236 dy = y - coordinateY();
sakanakuuun 2:f25a09c5e113 237 dtheta = coordinateTheta() - ptheta;
sakanakuuun 2:f25a09c5e113 238
sakanakuuun 12:f41918f71131 239 if(*disorder>max_disorder) {
sakanakuuun 12:f41918f71131 240 *disorder = max_disorder;
sakanakuuun 12:f41918f71131 241 } else if(*disorder<-max_disorder) {
sakanakuuun 12:f41918f71131 242 *disorder = -max_disorder;
sakanakuuun 1:405e28b64fdb 243 }
sakanakuuun 1:405e28b64fdb 244
sakanakuuun 5:0e18cf25291a 245 absd_length = abs(*d_length);
sakanakuuun 18:4c812a3c7411 246
sakanakuuun 18:4c812a3c7411 247
sakanakuuun 14:db58d3b0ecde 248 if(i++ < 5) {
sakanakuuun 14:db58d3b0ecde 249 daikei = i/5;
sakanakuuun 18:4c812a3c7411 250 } else if(absd_length < 300) {
sakanakuuun 13:176e543e6d64 251 daikei = absd_length / 300.0;
sakanakuuun 18:4c812a3c7411 252 }
sakanakuuun 18:4c812a3c7411 253 /*
sakanakuuun 18:4c812a3c7411 254 else if(absd_length > length - 30) {
sakanakuuun 18:4c812a3c7411 255 daikei = abs(length - absd_length) / 30.0;
sakanakuuun 18:4c812a3c7411 256 */
sakanakuuun 14:db58d3b0ecde 257 else
sakanakuuun 4:4c574be6325c 258 daikei = 1;
sakanakuuun 4:4c574be6325c 259
sakanakuuun 24:d041fc34d846 260 move(daikei * (rightspeed*(1-ratio) + k*(*disorder) - k_theta*dtheta) + rightspeed*ratio,
sakanakuuun 24:d041fc34d846 261 daikei * (leftspeed *(1-ratio) - k*(*disorder) + k_theta*dtheta) + leftspeed *ratio);
sakanakuuun 4:4c574be6325c 262
choutin 9:7e99a1c80656 263 //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 264 if((direction > 0 && *d_length <= 0) || (direction < 0 && *d_length >= 0)) {
sakanakuuun 2:f25a09c5e113 265 move(0, 0);
sakanakuuun 1:405e28b64fdb 266 break;
sakanakuuun 1:405e28b64fdb 267 }
sakanakuuun 0:d7ff86f25eaa 268
sakanakuuun 0:d7ff86f25eaa 269 }
sakanakuuun 0:d7ff86f25eaa 270
sakanakuuun 18:4c812a3c7411 271 wait(0.2);
sakanakuuun 7:7f1721542753 272
sakanakuuun 7:7f1721542753 273 yellow = 0;
sakanakuuun 0:d7ff86f25eaa 274 }
sakanakuuun 3:cecaa0154f92 275
sakanakuuun 18:4c812a3c7411 276 void pmove2(int x, int y)
sakanakuuun 18:4c812a3c7411 277 {
sakanakuuun 18:4c812a3c7411 278 yellow = 1;
sakanakuuun 18:4c812a3c7411 279 red=0;
sakanakuuun 18:4c812a3c7411 280 float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
sakanakuuun 18:4c812a3c7411 281 int k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
sakanakuuun 18:4c812a3c7411 282
sakanakuuun 18:4c812a3c7411 283 double length;
sakanakuuun 18:4c812a3c7411 284 int d_length, disorder;
sakanakuuun 18:4c812a3c7411 285 float dtheta, ptheta;
sakanakuuun 18:4c812a3c7411 286 float daikei;
sakanakuuun 18:4c812a3c7411 287
sakanakuuun 22:eed10c0b7fe0 288 length = sqrt(pow((double)(x - coordinateX()), 2) + pow((double)(y - coordinateY()), 2));
sakanakuuun 22:eed10c0b7fe0 289
sakanakuuun 18:4c812a3c7411 290 pc2.printf("length:%f", length);
sakanakuuun 22:eed10c0b7fe0 291
sakanakuuun 18:4c812a3c7411 292 if(length == 0) {
sakanakuuun 18:4c812a3c7411 293 red=1;
sakanakuuun 18:4c812a3c7411 294 return;
sakanakuuun 18:4c812a3c7411 295 }
sakanakuuun 18:4c812a3c7411 296
sakanakuuun 18:4c812a3c7411 297 ptheta = giveatan(x, y);
sakanakuuun 22:eed10c0b7fe0 298
sakanakuuun 19:0b0ed6b22de9 299 ptheta += nearPi(coordinateTheta() - ptheta);
sakanakuuun 22:eed10c0b7fe0 300
sakanakuuun 18:4c812a3c7411 301 turnrad(ptheta);
sakanakuuun 18:4c812a3c7411 302
sakanakuuun 18:4c812a3c7411 303 virtual_setup();
sakanakuuun 18:4c812a3c7411 304
sakanakuuun 18:4c812a3c7411 305 int i = 0;
sakanakuuun 18:4c812a3c7411 306
sakanakuuun 18:4c812a3c7411 307 while(1) {
sakanakuuun 18:4c812a3c7411 308 update_np();
sakanakuuun 18:4c812a3c7411 309 virtual_update();
sakanakuuun 18:4c812a3c7411 310
sakanakuuun 18:4c812a3c7411 311 d_length = length - virtual_coordinateX();
sakanakuuun 18:4c812a3c7411 312 disorder = virtual_coordinateY();
sakanakuuun 18:4c812a3c7411 313 dtheta = virtual_coordinateTheta();
sakanakuuun 18:4c812a3c7411 314
sakanakuuun 18:4c812a3c7411 315 if(disorder>max_disorder) {
sakanakuuun 18:4c812a3c7411 316 disorder = max_disorder;
sakanakuuun 18:4c812a3c7411 317 } else if(disorder<-max_disorder) {
sakanakuuun 18:4c812a3c7411 318 disorder = -max_disorder;
sakanakuuun 18:4c812a3c7411 319 }
sakanakuuun 18:4c812a3c7411 320
sakanakuuun 18:4c812a3c7411 321 if(i++ < 5) {
sakanakuuun 18:4c812a3c7411 322 daikei = i/5;
sakanakuuun 18:4c812a3c7411 323 } else if(d_length < 300) {
sakanakuuun 18:4c812a3c7411 324 daikei = d_length / 300.0;
sakanakuuun 22:eed10c0b7fe0 325 } else
sakanakuuun 18:4c812a3c7411 326 daikei = 1;
sakanakuuun 18:4c812a3c7411 327
sakanakuuun 24:d041fc34d846 328 move(daikei * (rightspeed*(1-ratio) - k*disorder - k_theta*dtheta) + rightspeed*ratio,
sakanakuuun 24:d041fc34d846 329 daikei * (leftspeed *(1-ratio) + k*disorder + k_theta*dtheta) + leftspeed *ratio);
sakanakuuun 18:4c812a3c7411 330
sakanakuuun 18:4c812a3c7411 331
sakanakuuun 22:eed10c0b7fe0 332 pc2.printf("length:%f, d_length:%d, vx:%d, vy:%d\n\r", length, d_length, virtual_coordinateX(), virtual_coordinateY());
sakanakuuun 22:eed10c0b7fe0 333 if(d_length <= 0) {
sakanakuuun 18:4c812a3c7411 334 move(0, 0);
sakanakuuun 18:4c812a3c7411 335 break;
sakanakuuun 18:4c812a3c7411 336 }
sakanakuuun 18:4c812a3c7411 337
sakanakuuun 18:4c812a3c7411 338 }
sakanakuuun 18:4c812a3c7411 339
sakanakuuun 18:4c812a3c7411 340 wait(0.2);
sakanakuuun 18:4c812a3c7411 341
sakanakuuun 18:4c812a3c7411 342 yellow = 0;
sakanakuuun 22:eed10c0b7fe0 343 red = 0;
sakanakuuun 18:4c812a3c7411 344 }
sakanakuuun 18:4c812a3c7411 345
sakanakuuun 6:0aa97a99c9cb 346 void back300()
sakanakuuun 7:7f1721542753 347 {
sakanakuuun 7:7f1721542753 348 red = 1;
sakanakuuun 7:7f1721542753 349
sakanakuuun 4:4c574be6325c 350 float k = 0.9;
sakanakuuun 6:0aa97a99c9cb 351 int length, px, py, dx, dy;
sakanakuuun 7:7f1721542753 352
sakanakuuun 6:0aa97a99c9cb 353 update();
sakanakuuun 7:7f1721542753 354
sakanakuuun 7:7f1721542753 355 px = coordinateX();
sakanakuuun 7:7f1721542753 356 py = coordinateY();
sakanakuuun 7:7f1721542753 357
sakanakuuun 6:0aa97a99c9cb 358 length = 300;
sakanakuuun 7:7f1721542753 359
sakanakuuun 15:403e9c57c1a1 360 turnrad(PI + nearPi(coordinateTheta() - PI));
sakanakuuun 7:7f1721542753 361
sakanakuuun 4:4c574be6325c 362 while(1) {
sakanakuuun 14:db58d3b0ecde 363 update_np();
sakanakuuun 6:0aa97a99c9cb 364 dx = coordinateX() - px;
sakanakuuun 6:0aa97a99c9cb 365 dy = coordinateY() - py;
sakanakuuun 4:4c574be6325c 366
sakanakuuun 12:f41918f71131 367 if(dy>max_disorder) {
sakanakuuun 12:f41918f71131 368 dy = max_disorder;
sakanakuuun 12:f41918f71131 369 } else if(dy<-max_disorder) {
sakanakuuun 12:f41918f71131 370 dy = -max_disorder;
sakanakuuun 6:0aa97a99c9cb 371 }
sakanakuuun 7:7f1721542753 372
sakanakuuun 8:3bf4addaaedd 373 move(-(30 + k*dy), -(32 - k*dy));
sakanakuuun 4:4c574be6325c 374
sakanakuuun 6:0aa97a99c9cb 375 if(dx>length) {
sakanakuuun 4:4c574be6325c 376 move(0, 0);
sakanakuuun 4:4c574be6325c 377 break;
sakanakuuun 4:4c574be6325c 378 }
sakanakuuun 4:4c574be6325c 379 }
sakanakuuun 4:4c574be6325c 380
sakanakuuun 18:4c812a3c7411 381 wait(0.2);
sakanakuuun 7:7f1721542753 382
sakanakuuun 7:7f1721542753 383 red = 0;
sakanakuuun 4:4c574be6325c 384 }
sakanakuuun 12:f41918f71131 385
sakanakuuun 16:140e758346ae 386
sakanakuuun 22:eed10c0b7fe0 387 void nxback300(int team)
sakanakuuun 16:140e758346ae 388 {
sakanakuuun 16:140e758346ae 389 red = 1;
sakanakuuun 16:140e758346ae 390
sakanakuuun 16:140e758346ae 391 float k = 0.9;
sakanakuuun 16:140e758346ae 392 int length, px, py, dx, dy;
sakanakuuun 16:140e758346ae 393
sakanakuuun 16:140e758346ae 394 update();
sakanakuuun 16:140e758346ae 395
sakanakuuun 16:140e758346ae 396 px = coordinateX();
sakanakuuun 16:140e758346ae 397 py = coordinateY();
sakanakuuun 16:140e758346ae 398
sakanakuuun 16:140e758346ae 399 length = 300;
sakanakuuun 16:140e758346ae 400
sakanakuuun 16:140e758346ae 401 turnrad(nearPi(coordinateTheta()));
sakanakuuun 16:140e758346ae 402
sakanakuuun 16:140e758346ae 403 while(1) {
sakanakuuun 16:140e758346ae 404 update_np();
sakanakuuun 16:140e758346ae 405 dx = coordinateX() - px;
sakanakuuun 16:140e758346ae 406 dy = coordinateY() - py;
sakanakuuun 16:140e758346ae 407
sakanakuuun 16:140e758346ae 408 if(dy>max_disorder) {
sakanakuuun 16:140e758346ae 409 dy = max_disorder;
sakanakuuun 16:140e758346ae 410 } else if(dy<-max_disorder) {
sakanakuuun 16:140e758346ae 411 dy = -max_disorder;
sakanakuuun 16:140e758346ae 412 }
sakanakuuun 16:140e758346ae 413
sakanakuuun 16:140e758346ae 414 move(-(30 - k*dy), -(32 + k*dy));
sakanakuuun 16:140e758346ae 415
sakanakuuun 16:140e758346ae 416 if(abs(dx)>length) {
sakanakuuun 16:140e758346ae 417 move(0, 0);
sakanakuuun 16:140e758346ae 418 break;
sakanakuuun 16:140e758346ae 419 }
sakanakuuun 16:140e758346ae 420 }
sakanakuuun 16:140e758346ae 421
sakanakuuun 18:4c812a3c7411 422 wait(0.2);
sakanakuuun 16:140e758346ae 423
sakanakuuun 16:140e758346ae 424 red = 0;
sakanakuuun 16:140e758346ae 425 }
sakanakuuun 16:140e758346ae 426
sakanakuuun 16:140e758346ae 427
sakanakuuun 22:eed10c0b7fe0 428 void pyback300(int team)
sakanakuuun 17:c167f4ed9070 429 {
sakanakuuun 22:eed10c0b7fe0 430 if(team == 0) {
sakanakuuun 22:eed10c0b7fe0 431 nyback300(1);
sakanakuuun 22:eed10c0b7fe0 432 return;
sakanakuuun 22:eed10c0b7fe0 433 }
sakanakuuun 22:eed10c0b7fe0 434
sakanakuuun 17:c167f4ed9070 435 red = 1;
sakanakuuun 17:c167f4ed9070 436
sakanakuuun 17:c167f4ed9070 437 float k = 0.9;
sakanakuuun 17:c167f4ed9070 438 int length, px, py, dx, dy;
sakanakuuun 17:c167f4ed9070 439
sakanakuuun 17:c167f4ed9070 440 update();
sakanakuuun 17:c167f4ed9070 441
sakanakuuun 17:c167f4ed9070 442 px = coordinateX();
sakanakuuun 17:c167f4ed9070 443 py = coordinateY();
sakanakuuun 17:c167f4ed9070 444
sakanakuuun 17:c167f4ed9070 445 length = 300;
sakanakuuun 17:c167f4ed9070 446
sakanakuuun 18:4c812a3c7411 447 turnrad(PI/2 + nearPi(coordinateTheta() - PI/2));
sakanakuuun 17:c167f4ed9070 448
sakanakuuun 17:c167f4ed9070 449 while(1) {
sakanakuuun 17:c167f4ed9070 450 update_np();
sakanakuuun 17:c167f4ed9070 451 dx = coordinateX() - px;
sakanakuuun 17:c167f4ed9070 452 dy = coordinateY() - py;
sakanakuuun 17:c167f4ed9070 453
sakanakuuun 17:c167f4ed9070 454 if(dx>max_disorder) {
sakanakuuun 17:c167f4ed9070 455 dx = max_disorder;
sakanakuuun 17:c167f4ed9070 456 } else if(dx<-max_disorder) {
sakanakuuun 17:c167f4ed9070 457 dx = -max_disorder;
sakanakuuun 17:c167f4ed9070 458 }
sakanakuuun 17:c167f4ed9070 459
sakanakuuun 17:c167f4ed9070 460 move(-(30 - k*dx), -(32 + k*dx));
sakanakuuun 17:c167f4ed9070 461
sakanakuuun 17:c167f4ed9070 462 if(dy>length) {
sakanakuuun 17:c167f4ed9070 463 move(0, 0);
sakanakuuun 17:c167f4ed9070 464 break;
sakanakuuun 17:c167f4ed9070 465 }
sakanakuuun 17:c167f4ed9070 466
sakanakuuun 17:c167f4ed9070 467 }
sakanakuuun 17:c167f4ed9070 468
sakanakuuun 18:4c812a3c7411 469 wait(0.2);
sakanakuuun 17:c167f4ed9070 470
sakanakuuun 24:d041fc34d846 471 red = 0;
sakanakuuun 17:c167f4ed9070 472 }
sakanakuuun 17:c167f4ed9070 473
sakanakuuun 22:eed10c0b7fe0 474 void nyback300(int team)
sakanakuuun 18:4c812a3c7411 475 {
sakanakuuun 24:d041fc34d846 476 if(team == 0) {
sakanakuuun 22:eed10c0b7fe0 477 pyback300(1);
sakanakuuun 22:eed10c0b7fe0 478 return;
sakanakuuun 22:eed10c0b7fe0 479 }
sakanakuuun 24:d041fc34d846 480
sakanakuuun 22:eed10c0b7fe0 481 red = 1;
sakanakuuun 18:4c812a3c7411 482
sakanakuuun 22:eed10c0b7fe0 483 float k = 0.9;
sakanakuuun 22:eed10c0b7fe0 484 int length, px, py, dx, dy;
sakanakuuun 18:4c812a3c7411 485
sakanakuuun 22:eed10c0b7fe0 486 update();
sakanakuuun 18:4c812a3c7411 487
sakanakuuun 22:eed10c0b7fe0 488 px = coordinateX();
sakanakuuun 22:eed10c0b7fe0 489 py = coordinateY();
sakanakuuun 18:4c812a3c7411 490
sakanakuuun 22:eed10c0b7fe0 491 length = 300;
sakanakuuun 18:4c812a3c7411 492
sakanakuuun 22:eed10c0b7fe0 493 turnrad(-PI/2 + nearPi(coordinateTheta() + PI/2));
sakanakuuun 18:4c812a3c7411 494
sakanakuuun 22:eed10c0b7fe0 495 while(1) {
sakanakuuun 22:eed10c0b7fe0 496 update_np();
sakanakuuun 22:eed10c0b7fe0 497 dx = coordinateX() - px;
sakanakuuun 22:eed10c0b7fe0 498 dy = coordinateY() - py;
sakanakuuun 18:4c812a3c7411 499
sakanakuuun 22:eed10c0b7fe0 500 if(dx>max_disorder) {
sakanakuuun 22:eed10c0b7fe0 501 dx = max_disorder;
sakanakuuun 22:eed10c0b7fe0 502 } else if(dx<-max_disorder) {
sakanakuuun 22:eed10c0b7fe0 503 dx = -max_disorder;
sakanakuuun 22:eed10c0b7fe0 504 }
sakanakuuun 18:4c812a3c7411 505
sakanakuuun 22:eed10c0b7fe0 506 move(-(30 - k*dx), -(32 + k*dx));
sakanakuuun 18:4c812a3c7411 507
sakanakuuun 22:eed10c0b7fe0 508 if(abs(dy)>length) {
sakanakuuun 22:eed10c0b7fe0 509 move(0, 0);
sakanakuuun 22:eed10c0b7fe0 510 break;
sakanakuuun 18:4c812a3c7411 511 }
sakanakuuun 22:eed10c0b7fe0 512 }
sakanakuuun 18:4c812a3c7411 513
sakanakuuun 22:eed10c0b7fe0 514 wait(0.2);
sakanakuuun 22:eed10c0b7fe0 515
sakanakuuun 22:eed10c0b7fe0 516 red = 0;
sakanakuuun 22:eed10c0b7fe0 517
sakanakuuun 18:4c812a3c7411 518 }
sakanakuuun 17:c167f4ed9070 519
sakanakuuun 12:f41918f71131 520 float nearPi(float rad)
sakanakuuun 12:f41918f71131 521 {
sakanakuuun 12:f41918f71131 522 float npi = 0;
sakanakuuun 18:4c812a3c7411 523
sakanakuuun 18:4c812a3c7411 524 while(1) {
sakanakuuun 12:f41918f71131 525 if(rad > npi + PI)
sakanakuuun 12:f41918f71131 526 npi += 2*PI;
sakanakuuun 12:f41918f71131 527 else if(rad < npi - PI)
sakanakuuun 12:f41918f71131 528 npi -= 2*PI;
sakanakuuun 12:f41918f71131 529 else
sakanakuuun 12:f41918f71131 530 return npi;
sakanakuuun 12:f41918f71131 531 }
sakanakuuun 12:f41918f71131 532 }
sakanakuuun 24:d041fc34d846 533
sakanakuuun 24:d041fc34d846 534 float giveatan(int targetx,int targety)
sakanakuuun 24:d041fc34d846 535 {
sakanakuuun 24:d041fc34d846 536 int x,y;
sakanakuuun 24:d041fc34d846 537 float phi;
sakanakuuun 24:d041fc34d846 538 update();
sakanakuuun 24:d041fc34d846 539 x = coordinateX();
sakanakuuun 24:d041fc34d846 540 y = coordinateY();
sakanakuuun 24:d041fc34d846 541
sakanakuuun 24:d041fc34d846 542 if(targetx - x == 0) {
sakanakuuun 24:d041fc34d846 543 if(targety > 0)
sakanakuuun 24:d041fc34d846 544 return PI/2;
sakanakuuun 24:d041fc34d846 545 else if(targety < 0)
sakanakuuun 24:d041fc34d846 546 return -PI/2;
sakanakuuun 24:d041fc34d846 547 else
sakanakuuun 24:d041fc34d846 548 return 0;
sakanakuuun 24:d041fc34d846 549 }
sakanakuuun 24:d041fc34d846 550
sakanakuuun 24:d041fc34d846 551 phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得
sakanakuuun 24:d041fc34d846 552 if(targetx - x < 0) {
sakanakuuun 24:d041fc34d846 553 if(targety - y > 0)
sakanakuuun 24:d041fc34d846 554 phi += PI;
sakanakuuun 24:d041fc34d846 555 else if(targety - y < 0)
sakanakuuun 24:d041fc34d846 556 phi -= PI;
sakanakuuun 24:d041fc34d846 557 }
sakanakuuun 24:d041fc34d846 558
sakanakuuun 24:d041fc34d846 559 return phi;
sakanakuuun 24:d041fc34d846 560 }