Tk A / Move2

aaa

Fork of Move by 涼太郎 中村

move.cpp@26:0584d68e8f30, 2016-09-10 (annotated)

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