Tk A / Move2

aaa

Fork of Move by 涼太郎 中村

move.cpp@27:4242d6187281, 2016-09-10 (annotated)

Committer:
sakanakuuun
Date:
Sat Sep 10 17:41:32 2016 +0000
Revision:
27:4242d6187281
Parent:
26:0584d68e8f30
Child:
28:e4b81ad2e188
change turnrad(){wait(0.5)} -> {wait(0.2)}

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