Tk A / Move2

aaa

Fork of Move by 涼太郎 中村

move.cpp@30:0b09a05c67df, 2016-09-11 (annotated)

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