Tk A / Move2

aaa

Fork of Move by 涼太郎 中村

move.cpp@32:e462489e2418, 2016-09-11 (annotated)

Committer:
sakanakuuun
Date:
Sun Sep 11 04:04:55 2016 +0000
Revision:
32:e462489e2418
Parent:
31:427bd43429e3 
jfa

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 yellow = 1;
sakanakuuun 18:4c812a3c7411 276 red=0;
sakanakuuun 18:4c812a3c7411 277 float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
sakanakuuun 18:4c812a3c7411 278 int k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
sakanakuuun 18:4c812a3c7411 279
sakanakuuun 18:4c812a3c7411 280 double length;
sakanakuuun 18:4c812a3c7411 281 int d_length, disorder;
sakanakuuun 18:4c812a3c7411 282 float dtheta, ptheta;
sakanakuuun 18:4c812a3c7411 283 float daikei;
sakanakuuun 18:4c812a3c7411 284
sakanakuuun 22:eed10c0b7fe0 285 length = sqrt(pow((double)(x - coordinateX()), 2) + pow((double)(y - coordinateY()), 2));
sakanakuuun 22:eed10c0b7fe0 286
sakanakuuun 18:4c812a3c7411 287 if(length == 0) {
sakanakuuun 18:4c812a3c7411 288 red=1;
sakanakuuun 18:4c812a3c7411 289 return;
sakanakuuun 18:4c812a3c7411 290 }
sakanakuuun 18:4c812a3c7411 291
sakanakuuun 18:4c812a3c7411 292 ptheta = giveatan(x, y);
sakanakuuun 22:eed10c0b7fe0 293
sakanakuuun 19:0b0ed6b22de9 294 ptheta += nearPi(coordinateTheta() - ptheta);
sakanakuuun 22:eed10c0b7fe0 295
sakanakuuun 18:4c812a3c7411 296 turnrad(ptheta);
sakanakuuun 18:4c812a3c7411 297
sakanakuuun 18:4c812a3c7411 298 virtual_setup();
sakanakuuun 18:4c812a3c7411 299
sakanakuuun 18:4c812a3c7411 300 int i = 0;
sakanakuuun 18:4c812a3c7411 301
sakanakuuun 18:4c812a3c7411 302 while(1) {
sakanakuuun 18:4c812a3c7411 303 update_np();
sakanakuuun 18:4c812a3c7411 304 virtual_update();
sakanakuuun 18:4c812a3c7411 305
sakanakuuun 18:4c812a3c7411 306 d_length = length - virtual_coordinateX();
sakanakuuun 18:4c812a3c7411 307 disorder = virtual_coordinateY();
sakanakuuun 18:4c812a3c7411 308 dtheta = virtual_coordinateTheta();
sakanakuuun 18:4c812a3c7411 309
sakanakuuun 18:4c812a3c7411 310 if(disorder>max_disorder) {
sakanakuuun 18:4c812a3c7411 311 disorder = max_disorder;
sakanakuuun 18:4c812a3c7411 312 } else if(disorder<-max_disorder) {
sakanakuuun 18:4c812a3c7411 313 disorder = -max_disorder;
sakanakuuun 18:4c812a3c7411 314 }
sakanakuuun 18:4c812a3c7411 315
sakanakuuun 18:4c812a3c7411 316 if(i++ < 5) {
sakanakuuun 18:4c812a3c7411 317 daikei = i/5;
sakanakuuun 18:4c812a3c7411 318 } else if(d_length < 300) {
sakanakuuun 18:4c812a3c7411 319 daikei = d_length / 300.0;
sakanakuuun 22:eed10c0b7fe0 320 } else
sakanakuuun 18:4c812a3c7411 321 daikei = 1;
sakanakuuun 18:4c812a3c7411 322
sakanakuuun 24:d041fc34d846 323 move(daikei * (rightspeed*(1-ratio) - k*disorder - k_theta*dtheta) + rightspeed*ratio,
sakanakuuun 24:d041fc34d846 324 daikei * (leftspeed *(1-ratio) + k*disorder + k_theta*dtheta) + leftspeed *ratio);
sakanakuuun 18:4c812a3c7411 325
sakanakuuun 18:4c812a3c7411 326
sakanakuuun 25:fa9b75c175fd 327 //pc2.printf("length:%f, d_length:%d, vx:%d, vy:%d\n\r", length, d_length, virtual_coordinateX(), virtual_coordinateY());
sakanakuuun 22:eed10c0b7fe0 328 if(d_length <= 0) {
sakanakuuun 18:4c812a3c7411 329 move(0, 0);
sakanakuuun 18:4c812a3c7411 330 break;
sakanakuuun 18:4c812a3c7411 331 }
sakanakuuun 18:4c812a3c7411 332
sakanakuuun 18:4c812a3c7411 333 }
sakanakuuun 18:4c812a3c7411 334
sakanakuuun 18:4c812a3c7411 335 wait(0.2);
sakanakuuun 18:4c812a3c7411 336
sakanakuuun 18:4c812a3c7411 337 yellow = 0;
sakanakuuun 22:eed10c0b7fe0 338 red = 0;
sakanakuuun 18:4c812a3c7411 339 }
sakanakuuun 18:4c812a3c7411 340
sakanakuuun 32:e462489e2418 341 void pmove2_fast()
sakanakuuun 32:e462489e2418 342 {
sakanakuuun 32:e462489e2418 343 yellow = 1;
sakanakuuun 32:e462489e2418 344 red=0;
sakanakuuun 32:e462489e2418 345 float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
sakanakuuun 32:e462489e2418 346 int k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
sakanakuuun 32:e462489e2418 347
sakanakuuun 32:e462489e2418 348 double length;
sakanakuuun 32:e462489e2418 349 int d_length, disorder;
sakanakuuun 32:e462489e2418 350 float dtheta, ptheta;
sakanakuuun 32:e462489e2418 351 float daikei;
sakanakuuun 32:e462489e2418 352
sakanakuuun 32:e462489e2418 353 length = sqrt(pow((double)(x - coordinateX()), 2) + pow((double)(y - coordinateY()), 2));
sakanakuuun 32:e462489e2418 354
sakanakuuun 32:e462489e2418 355 if(length == 0) {
sakanakuuun 32:e462489e2418 356 red=1;
sakanakuuun 32:e462489e2418 357 return;
sakanakuuun 32:e462489e2418 358 }
sakanakuuun 32:e462489e2418 359
sakanakuuun 32:e462489e2418 360 ptheta = giveatan(x, y);
sakanakuuun 32:e462489e2418 361
sakanakuuun 32:e462489e2418 362 ptheta += nearPi(coordinateTheta() - ptheta);
sakanakuuun 32:e462489e2418 363
sakanakuuun 32:e462489e2418 364 turnrad(ptheta);
sakanakuuun 32:e462489e2418 365
sakanakuuun 32:e462489e2418 366 virtual_setup();
sakanakuuun 32:e462489e2418 367
sakanakuuun 32:e462489e2418 368 int i = 0;
sakanakuuun 32:e462489e2418 369
sakanakuuun 32:e462489e2418 370 while(1) {
sakanakuuun 32:e462489e2418 371 update_np();
sakanakuuun 32:e462489e2418 372 virtual_update();
sakanakuuun 32:e462489e2418 373
sakanakuuun 32:e462489e2418 374 d_length = length - virtual_coordinateX();
sakanakuuun 32:e462489e2418 375 disorder = virtual_coordinateY();
sakanakuuun 32:e462489e2418 376 dtheta = virtual_coordinateTheta();
sakanakuuun 32:e462489e2418 377
sakanakuuun 32:e462489e2418 378 if(disorder>max_disorder) {
sakanakuuun 32:e462489e2418 379 disorder = max_disorder;
sakanakuuun 32:e462489e2418 380 } else if(disorder<-max_disorder) {
sakanakuuun 32:e462489e2418 381 disorder = -max_disorder;
sakanakuuun 32:e462489e2418 382 }
sakanakuuun 32:e462489e2418 383
sakanakuuun 32:e462489e2418 384 if(i++ < 5) {
sakanakuuun 32:e462489e2418 385 daikei = i/5;
sakanakuuun 32:e462489e2418 386 } else if(d_length < 300) {
sakanakuuun 32:e462489e2418 387 daikei = d_length / 300.0;
sakanakuuun 32:e462489e2418 388 } else
sakanakuuun 32:e462489e2418 389 daikei = 1;
sakanakuuun 32:e462489e2418 390
sakanakuuun 32:e462489e2418 391 move(daikei * (rightspeed*2*(1-ratio) - k*disorder - k_theta*dtheta) + rightspeed*ratio,
sakanakuuun 32:e462489e2418 392 daikei * (leftspeed*2*(1-ratio) + k*disorder + k_theta*dtheta) + leftspeed*ratio);
sakanakuuun 32:e462489e2418 393
sakanakuuun 32:e462489e2418 394
sakanakuuun 32:e462489e2418 395 //pc2.printf("length:%f, d_length:%d, vx:%d, vy:%d\n\r", length, d_length, virtual_coordinateX(), virtual_coordinateY());
sakanakuuun 32:e462489e2418 396 if(d_length <= 0) {
sakanakuuun 32:e462489e2418 397 move(0, 0);
sakanakuuun 32:e462489e2418 398 break;
sakanakuuun 32:e462489e2418 399 }
sakanakuuun 32:e462489e2418 400
sakanakuuun 32:e462489e2418 401 }
sakanakuuun 32:e462489e2418 402
sakanakuuun 32:e462489e2418 403 wait(0.2);
sakanakuuun 32:e462489e2418 404
sakanakuuun 32:e462489e2418 405 yellow = 0;
sakanakuuun 32:e462489e2418 406 red = 0;
sakanakuuun 32:e462489e2418 407 }
sakanakuuun 32:e462489e2418 408
sakanakuuun 6:0aa97a99c9cb 409 void back300()
sakanakuuun 7:7f1721542753 410 {
sakanakuuun 7:7f1721542753 411 red = 1;
sakanakuuun 7:7f1721542753 412
sakanakuuun 4:4c574be6325c 413 float k = 0.9;
sakanakuuun 6:0aa97a99c9cb 414 int length, px, py, dx, dy;
sakanakuuun 7:7f1721542753 415
sakanakuuun 6:0aa97a99c9cb 416 update();
sakanakuuun 7:7f1721542753 417
sakanakuuun 7:7f1721542753 418 px = coordinateX();
sakanakuuun 7:7f1721542753 419 py = coordinateY();
sakanakuuun 7:7f1721542753 420
sakanakuuun 6:0aa97a99c9cb 421 length = 300;
sakanakuuun 7:7f1721542753 422
sakanakuuun 15:403e9c57c1a1 423 turnrad(PI + nearPi(coordinateTheta() - PI));
sakanakuuun 7:7f1721542753 424
sakanakuuun 4:4c574be6325c 425 while(1) {
sakanakuuun 14:db58d3b0ecde 426 update_np();
sakanakuuun 6:0aa97a99c9cb 427 dx = coordinateX() - px;
sakanakuuun 6:0aa97a99c9cb 428 dy = coordinateY() - py;
sakanakuuun 4:4c574be6325c 429
sakanakuuun 12:f41918f71131 430 if(dy>max_disorder) {
sakanakuuun 12:f41918f71131 431 dy = max_disorder;
sakanakuuun 12:f41918f71131 432 } else if(dy<-max_disorder) {
sakanakuuun 12:f41918f71131 433 dy = -max_disorder;
sakanakuuun 6:0aa97a99c9cb 434 }
sakanakuuun 7:7f1721542753 435
sakanakuuun 8:3bf4addaaedd 436 move(-(30 + k*dy), -(32 - k*dy));
sakanakuuun 4:4c574be6325c 437
sakanakuuun 6:0aa97a99c9cb 438 if(dx>length) {
sakanakuuun 4:4c574be6325c 439 move(0, 0);
sakanakuuun 4:4c574be6325c 440 break;
sakanakuuun 4:4c574be6325c 441 }
sakanakuuun 4:4c574be6325c 442 }
sakanakuuun 4:4c574be6325c 443
sakanakuuun 18:4c812a3c7411 444 wait(0.2);
sakanakuuun 7:7f1721542753 445
sakanakuuun 7:7f1721542753 446 red = 0;
sakanakuuun 4:4c574be6325c 447 }
sakanakuuun 12:f41918f71131 448
sakanakuuun 16:140e758346ae 449
sakanakuuun 31:427bd43429e3 450 void nxback300()
sakanakuuun 16:140e758346ae 451 {
sakanakuuun 16:140e758346ae 452 red = 1;
sakanakuuun 16:140e758346ae 453
sakanakuuun 16:140e758346ae 454 float k = 0.9;
sakanakuuun 16:140e758346ae 455 int length, px, py, dx, dy;
sakanakuuun 16:140e758346ae 456
sakanakuuun 16:140e758346ae 457 update();
sakanakuuun 16:140e758346ae 458
sakanakuuun 16:140e758346ae 459 px = coordinateX();
sakanakuuun 16:140e758346ae 460 py = coordinateY();
sakanakuuun 16:140e758346ae 461
sakanakuuun 16:140e758346ae 462 length = 300;
sakanakuuun 16:140e758346ae 463
sakanakuuun 16:140e758346ae 464 turnrad(nearPi(coordinateTheta()));
sakanakuuun 16:140e758346ae 465
sakanakuuun 16:140e758346ae 466 while(1) {
sakanakuuun 16:140e758346ae 467 update_np();
sakanakuuun 16:140e758346ae 468 dx = coordinateX() - px;
sakanakuuun 16:140e758346ae 469 dy = coordinateY() - py;
sakanakuuun 16:140e758346ae 470
sakanakuuun 16:140e758346ae 471 if(dy>max_disorder) {
sakanakuuun 16:140e758346ae 472 dy = max_disorder;
sakanakuuun 16:140e758346ae 473 } else if(dy<-max_disorder) {
sakanakuuun 16:140e758346ae 474 dy = -max_disorder;
sakanakuuun 16:140e758346ae 475 }
sakanakuuun 16:140e758346ae 476
sakanakuuun 16:140e758346ae 477 move(-(30 - k*dy), -(32 + k*dy));
sakanakuuun 16:140e758346ae 478
sakanakuuun 16:140e758346ae 479 if(abs(dx)>length) {
sakanakuuun 16:140e758346ae 480 move(0, 0);
sakanakuuun 16:140e758346ae 481 break;
sakanakuuun 16:140e758346ae 482 }
sakanakuuun 16:140e758346ae 483 }
sakanakuuun 16:140e758346ae 484
sakanakuuun 18:4c812a3c7411 485 wait(0.2);
sakanakuuun 16:140e758346ae 486
sakanakuuun 16:140e758346ae 487 red = 0;
sakanakuuun 16:140e758346ae 488 }
sakanakuuun 16:140e758346ae 489
sakanakuuun 22:eed10c0b7fe0 490 void pyback300(int team)
sakanakuuun 17:c167f4ed9070 491 {
sakanakuuun 22:eed10c0b7fe0 492 if(team == 0) {
sakanakuuun 22:eed10c0b7fe0 493 nyback300(1);
sakanakuuun 22:eed10c0b7fe0 494 return;
sakanakuuun 22:eed10c0b7fe0 495 }
sakanakuuun 22:eed10c0b7fe0 496
sakanakuuun 17:c167f4ed9070 497 red = 1;
sakanakuuun 17:c167f4ed9070 498
sakanakuuun 17:c167f4ed9070 499 float k = 0.9;
sakanakuuun 17:c167f4ed9070 500 int length, px, py, dx, dy;
sakanakuuun 17:c167f4ed9070 501
sakanakuuun 17:c167f4ed9070 502 update();
sakanakuuun 17:c167f4ed9070 503
sakanakuuun 17:c167f4ed9070 504 px = coordinateX();
sakanakuuun 17:c167f4ed9070 505 py = coordinateY();
sakanakuuun 17:c167f4ed9070 506
sakanakuuun 17:c167f4ed9070 507 length = 300;
sakanakuuun 17:c167f4ed9070 508
sakanakuuun 18:4c812a3c7411 509 turnrad(PI/2 + nearPi(coordinateTheta() - PI/2));
sakanakuuun 17:c167f4ed9070 510
sakanakuuun 17:c167f4ed9070 511 while(1) {
sakanakuuun 17:c167f4ed9070 512 update_np();
sakanakuuun 17:c167f4ed9070 513 dx = coordinateX() - px;
sakanakuuun 17:c167f4ed9070 514 dy = coordinateY() - py;
sakanakuuun 17:c167f4ed9070 515
sakanakuuun 17:c167f4ed9070 516 if(dx>max_disorder) {
sakanakuuun 17:c167f4ed9070 517 dx = max_disorder;
sakanakuuun 17:c167f4ed9070 518 } else if(dx<-max_disorder) {
sakanakuuun 17:c167f4ed9070 519 dx = -max_disorder;
sakanakuuun 17:c167f4ed9070 520 }
sakanakuuun 17:c167f4ed9070 521
sakanakuuun 17:c167f4ed9070 522 move(-(30 - k*dx), -(32 + k*dx));
sakanakuuun 17:c167f4ed9070 523
sakanakuuun 17:c167f4ed9070 524 if(dy>length) {
sakanakuuun 17:c167f4ed9070 525 move(0, 0);
sakanakuuun 17:c167f4ed9070 526 break;
sakanakuuun 17:c167f4ed9070 527 }
sakanakuuun 17:c167f4ed9070 528
sakanakuuun 17:c167f4ed9070 529 }
sakanakuuun 17:c167f4ed9070 530
sakanakuuun 18:4c812a3c7411 531 wait(0.2);
sakanakuuun 17:c167f4ed9070 532
sakanakuuun 24:d041fc34d846 533 red = 0;
sakanakuuun 17:c167f4ed9070 534 }
sakanakuuun 17:c167f4ed9070 535
sakanakuuun 22:eed10c0b7fe0 536 void nyback300(int team)
sakanakuuun 18:4c812a3c7411 537 {
sakanakuuun 24:d041fc34d846 538 if(team == 0) {
sakanakuuun 22:eed10c0b7fe0 539 pyback300(1);
sakanakuuun 22:eed10c0b7fe0 540 return;
sakanakuuun 22:eed10c0b7fe0 541 }
sakanakuuun 24:d041fc34d846 542
sakanakuuun 22:eed10c0b7fe0 543 red = 1;
sakanakuuun 18:4c812a3c7411 544
sakanakuuun 22:eed10c0b7fe0 545 float k = 0.9;
sakanakuuun 22:eed10c0b7fe0 546 int length, px, py, dx, dy;
sakanakuuun 18:4c812a3c7411 547
sakanakuuun 22:eed10c0b7fe0 548 update();
sakanakuuun 18:4c812a3c7411 549
sakanakuuun 22:eed10c0b7fe0 550 px = coordinateX();
sakanakuuun 22:eed10c0b7fe0 551 py = coordinateY();
sakanakuuun 18:4c812a3c7411 552
sakanakuuun 22:eed10c0b7fe0 553 length = 300;
sakanakuuun 18:4c812a3c7411 554
sakanakuuun 22:eed10c0b7fe0 555 turnrad(-PI/2 + nearPi(coordinateTheta() + PI/2));
sakanakuuun 18:4c812a3c7411 556
sakanakuuun 22:eed10c0b7fe0 557 while(1) {
sakanakuuun 22:eed10c0b7fe0 558 update_np();
sakanakuuun 22:eed10c0b7fe0 559 dx = coordinateX() - px;
sakanakuuun 22:eed10c0b7fe0 560 dy = coordinateY() - py;
sakanakuuun 18:4c812a3c7411 561
sakanakuuun 22:eed10c0b7fe0 562 if(dx>max_disorder) {
sakanakuuun 22:eed10c0b7fe0 563 dx = max_disorder;
sakanakuuun 22:eed10c0b7fe0 564 } else if(dx<-max_disorder) {
sakanakuuun 22:eed10c0b7fe0 565 dx = -max_disorder;
sakanakuuun 22:eed10c0b7fe0 566 }
sakanakuuun 18:4c812a3c7411 567
sakanakuuun 22:eed10c0b7fe0 568 move(-(30 - k*dx), -(32 + k*dx));
sakanakuuun 18:4c812a3c7411 569
sakanakuuun 22:eed10c0b7fe0 570 if(abs(dy)>length) {
sakanakuuun 22:eed10c0b7fe0 571 move(0, 0);
sakanakuuun 22:eed10c0b7fe0 572 break;
sakanakuuun 18:4c812a3c7411 573 }
sakanakuuun 22:eed10c0b7fe0 574 }
sakanakuuun 18:4c812a3c7411 575
sakanakuuun 22:eed10c0b7fe0 576 wait(0.2);
sakanakuuun 22:eed10c0b7fe0 577
sakanakuuun 22:eed10c0b7fe0 578 red = 0;
sakanakuuun 18:4c812a3c7411 579 }
sakanakuuun 17:c167f4ed9070 580
sakanakuuun 12:f41918f71131 581 float nearPi(float rad)
sakanakuuun 12:f41918f71131 582 {
sakanakuuun 12:f41918f71131 583 float npi = 0;
sakanakuuun 18:4c812a3c7411 584
sakanakuuun 18:4c812a3c7411 585 while(1) {
sakanakuuun 12:f41918f71131 586 if(rad > npi + PI)
sakanakuuun 12:f41918f71131 587 npi += 2*PI;
sakanakuuun 12:f41918f71131 588 else if(rad < npi - PI)
sakanakuuun 12:f41918f71131 589 npi -= 2*PI;
sakanakuuun 12:f41918f71131 590 else
sakanakuuun 12:f41918f71131 591 return npi;
sakanakuuun 12:f41918f71131 592 }
sakanakuuun 12:f41918f71131 593 }
sakanakuuun 24:d041fc34d846 594
sakanakuuun 24:d041fc34d846 595 float giveatan(int targetx,int targety)
sakanakuuun 24:d041fc34d846 596 {
sakanakuuun 24:d041fc34d846 597 int x,y;
sakanakuuun 24:d041fc34d846 598 float phi;
sakanakuuun 24:d041fc34d846 599 update();
sakanakuuun 24:d041fc34d846 600 x = coordinateX();
sakanakuuun 24:d041fc34d846 601 y = coordinateY();
sakanakuuun 24:d041fc34d846 602
sakanakuuun 24:d041fc34d846 603 if(targetx - x == 0) {
sakanakuuun 24:d041fc34d846 604 if(targety > 0)
sakanakuuun 24:d041fc34d846 605 return PI/2;
sakanakuuun 24:d041fc34d846 606 else if(targety < 0)
sakanakuuun 24:d041fc34d846 607 return -PI/2;
sakanakuuun 24:d041fc34d846 608 else
sakanakuuun 24:d041fc34d846 609 return 0;
sakanakuuun 24:d041fc34d846 610 }
sakanakuuun 24:d041fc34d846 611
sakanakuuun 24:d041fc34d846 612 phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得
sakanakuuun 24:d041fc34d846 613 if(targetx - x < 0) {
sakanakuuun 24:d041fc34d846 614 if(targety - y > 0)
sakanakuuun 24:d041fc34d846 615 phi += PI;
sakanakuuun 24:d041fc34d846 616 else if(targety - y < 0)
sakanakuuun 24:d041fc34d846 617 phi -= PI;
sakanakuuun 24:d041fc34d846 618 }
sakanakuuun 24:d041fc34d846 619
sakanakuuun 24:d041fc34d846 620 return phi;
sakanakuuun 24:d041fc34d846 621 }