Tk A / Move2

aaa

Fork of Move by 涼太郎 中村

move.cpp@22:eed10c0b7fe0, 2016-09-10 (annotated)

Committer:
sakanakuuun
Date:
Sat Sep 10 17:04:45 2016 +0000
Revision:
22:eed10c0b7fe0
Parent:
21:282c2b61e462
Child:
23:e30ffaeb3e0f
right <-> left?

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