Tk A / Move2

aaa

Fork of Move by 涼太郎 中村

move.cpp@16:140e758346ae, 2016-09-10 (annotated)

Committer:
sakanakuuun
Date:
Sat Sep 10 06:14:48 2016 +0000
Revision:
16:140e758346ae
Parent:
15:403e9c57c1a1
Child:
17:c167f4ed9070
aaaa; ;

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 0:d7ff86f25eaa 5
sakanakuuun 0:d7ff86f25eaa 6 PwmOut M1cw(PA_11);
sakanakuuun 0:d7ff86f25eaa 7 PwmOut M1ccw(PB_15);
sakanakuuun 0:d7ff86f25eaa 8 PwmOut M2ccw(PB_14);
sakanakuuun 0:d7ff86f25eaa 9 PwmOut M2cw(PB_13);
sakanakuuun 7:7f1721542753 10
sakanakuuun 7:7f1721542753 11 DigitalOut green (PC_2);
sakanakuuun 7:7f1721542753 12 DigitalOut yellow(PC_3);
sakanakuuun 7:7f1721542753 13 DigitalOut red (PC_0);
sakanakuuun 7:7f1721542753 14
sakanakuuun 2:f25a09c5e113 15 /*
sakanakuuun 2:f25a09c5e113 16 DigitalOut teamledblue(PC_10);
sakanakuuun 2:f25a09c5e113 17 DigitalOut teamledred(PC_12);
sakanakuuun 2:f25a09c5e113 18 */
sakanakuuun 2:f25a09c5e113 19
sakanakuuun 7:7f1721542753 20 Serial pc2(SERIAL_TX, SERIAL_RX); //pcと通信
sakanakuuun 2:f25a09c5e113 21
sakanakuuun 0:d7ff86f25eaa 22
sakanakuuun 0:d7ff86f25eaa 23 //const int allowlength=5;
sakanakuuun 0:d7ff86f25eaa 24 //const float allowdegree=0.02;
sakanakuuun 5:0e18cf25291a 25 const int rightspeed=70;
sakanakuuun 14:db58d3b0ecde 26 const int leftspeed=rightspeed + 4;
sakanakuuun 4:4c574be6325c 27 const int turnspeed=30*2;
sakanakuuun 13:176e543e6d64 28 const int hosei_turnspeed = 13;
sakanakuuun 13:176e543e6d64 29 const int max_disorder = 4;
sakanakuuun 14:db58d3b0ecde 30 const float ratio = 1.0/7.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 12:f41918f71131 89 move(-hosei_turnspeed, hosei_turnspeed);
sakanakuuun 0:d7ff86f25eaa 90 } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
sakanakuuun 12:f41918f71131 91 move(hosei_turnspeed, -hosei_turnspeed);
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 8:3bf4addaaedd 111 move((-np)*(turnspeed+2), (np)*turnspeed);
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 15:403e9c57c1a1 142 move((turnspeed+2), -turnspeed);
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 15:403e9c57c1a1 172 move((-1)*(turnspeed+2), turnspeed);
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 15:403e9c57c1a1 187 wait(0.5);
sakanakuuun 15:403e9c57c1a1 188
sakanakuuun 15:403e9c57c1a1 189 hosei_turn(0, false, rad);
sakanakuuun 15:403e9c57c1a1 190
sakanakuuun 15:403e9c57c1a1 191 wait(0.5);
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 7:7f1721542753 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 7:7f1721542753 223
sakanakuuun 7:7f1721542753 224 update();
sakanakuuun 7:7f1721542753 225 dx = x - coordinateX();
sakanakuuun 7:7f1721542753 226 dy = y - coordinateY();
sakanakuuun 12:f41918f71131 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 12:f41918f71131 251
sakanakuuun 13:176e543e6d64 252 ptheta += nearPi(coordinateTheta() - ptheta);
sakanakuuun 12:f41918f71131 253
sakanakuuun 15:403e9c57c1a1 254 turnrad(ptheta);
sakanakuuun 5:0e18cf25291a 255
sakanakuuun 2:f25a09c5e113 256 if(length == 0) return;
sakanakuuun 14:db58d3b0ecde 257
sakanakuuun 14:db58d3b0ecde 258 int i = 0;
sakanakuuun 14:db58d3b0ecde 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 14:db58d3b0ecde 273
sakanakuuun 14:db58d3b0ecde 274
sakanakuuun 14:db58d3b0ecde 275 if(i++ < 5) {
sakanakuuun 14:db58d3b0ecde 276 daikei = i/5;
sakanakuuun 14:db58d3b0ecde 277 }
sakanakuuun 14:db58d3b0ecde 278 else if(absd_length < 300) {
sakanakuuun 13:176e543e6d64 279 daikei = absd_length / 300.0;
sakanakuuun 14:db58d3b0ecde 280 }
sakanakuuun 14:db58d3b0ecde 281 /*
sakanakuuun 14:db58d3b0ecde 282 else if(absd_length > length - 30) {
sakanakuuun 7:7f1721542753 283 daikei = abs(length - absd_length) / 30.0;
sakanakuuun 14:db58d3b0ecde 284 */
sakanakuuun 14:db58d3b0ecde 285 else
sakanakuuun 4:4c574be6325c 286 daikei = 1;
sakanakuuun 4:4c574be6325c 287
sakanakuuun 14:db58d3b0ecde 288 move(daikei * (rightspeed*(1-ratio) + k*(*disorder) - k_theta*dtheta) + rightspeed*ratio,
sakanakuuun 14:db58d3b0ecde 289 daikei * (leftspeed *(1-ratio) - k*(*disorder) + k_theta*dtheta) + leftspeed *ratio);
sakanakuuun 4:4c574be6325c 290
choutin 9:7e99a1c80656 291 //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 292 if((direction > 0 && *d_length <= 0) || (direction < 0 && *d_length >= 0)) {
sakanakuuun 2:f25a09c5e113 293 move(0, 0);
sakanakuuun 1:405e28b64fdb 294 break;
sakanakuuun 1:405e28b64fdb 295 }
sakanakuuun 0:d7ff86f25eaa 296
sakanakuuun 0:d7ff86f25eaa 297 }
sakanakuuun 0:d7ff86f25eaa 298
sakanakuuun 2:f25a09c5e113 299 wait(0.5);
sakanakuuun 7:7f1721542753 300
sakanakuuun 7:7f1721542753 301 yellow = 0;
sakanakuuun 0:d7ff86f25eaa 302 }
sakanakuuun 3:cecaa0154f92 303
sakanakuuun 6:0aa97a99c9cb 304 void back300()
sakanakuuun 7:7f1721542753 305 {
sakanakuuun 7:7f1721542753 306 red = 1;
sakanakuuun 7:7f1721542753 307
sakanakuuun 4:4c574be6325c 308 float k = 0.9;
sakanakuuun 4:4c574be6325c 309 int k_theta = 2;
sakanakuuun 4:4c574be6325c 310
sakanakuuun 6:0aa97a99c9cb 311 int length, px, py, dx, dy;
sakanakuuun 4:4c574be6325c 312 float daikei;
sakanakuuun 7:7f1721542753 313
sakanakuuun 6:0aa97a99c9cb 314 update();
sakanakuuun 7:7f1721542753 315
sakanakuuun 7:7f1721542753 316 px = coordinateX();
sakanakuuun 7:7f1721542753 317 py = coordinateY();
sakanakuuun 7:7f1721542753 318
sakanakuuun 6:0aa97a99c9cb 319 length = 300;
sakanakuuun 7:7f1721542753 320
sakanakuuun 15:403e9c57c1a1 321 turnrad(PI + nearPi(coordinateTheta() - PI));
sakanakuuun 7:7f1721542753 322
sakanakuuun 4:4c574be6325c 323 while(1) {
sakanakuuun 14:db58d3b0ecde 324 update_np();
sakanakuuun 6:0aa97a99c9cb 325 dx = coordinateX() - px;
sakanakuuun 6:0aa97a99c9cb 326 dy = coordinateY() - py;
sakanakuuun 4:4c574be6325c 327
sakanakuuun 12:f41918f71131 328 if(dy>max_disorder) {
sakanakuuun 12:f41918f71131 329 dy = max_disorder;
sakanakuuun 12:f41918f71131 330 } else if(dy<-max_disorder) {
sakanakuuun 12:f41918f71131 331 dy = -max_disorder;
sakanakuuun 6:0aa97a99c9cb 332 }
sakanakuuun 7:7f1721542753 333
sakanakuuun 4:4c574be6325c 334
sakanakuuun 8:3bf4addaaedd 335 move(-(30 + k*dy), -(32 - k*dy));
sakanakuuun 4:4c574be6325c 336
sakanakuuun 7:7f1721542753 337
sakanakuuun 6:0aa97a99c9cb 338 if(dx>length) {
sakanakuuun 4:4c574be6325c 339 move(0, 0);
sakanakuuun 4:4c574be6325c 340 break;
sakanakuuun 4:4c574be6325c 341 }
sakanakuuun 4:4c574be6325c 342
sakanakuuun 4:4c574be6325c 343 //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
sakanakuuun 4:4c574be6325c 344 }
sakanakuuun 4:4c574be6325c 345
sakanakuuun 4:4c574be6325c 346 wait(0.5);
sakanakuuun 7:7f1721542753 347
sakanakuuun 7:7f1721542753 348 red = 0;
sakanakuuun 4:4c574be6325c 349 }
sakanakuuun 12:f41918f71131 350
sakanakuuun 16:140e758346ae 351
sakanakuuun 16:140e758346ae 352
sakanakuuun 16:140e758346ae 353 void nxback300()
sakanakuuun 16:140e758346ae 354 {
sakanakuuun 16:140e758346ae 355 red = 1;
sakanakuuun 16:140e758346ae 356
sakanakuuun 16:140e758346ae 357 float k = 0.9;
sakanakuuun 16:140e758346ae 358 int k_theta = 2;
sakanakuuun 16:140e758346ae 359
sakanakuuun 16:140e758346ae 360 int length, px, py, dx, dy;
sakanakuuun 16:140e758346ae 361 float daikei;
sakanakuuun 16:140e758346ae 362
sakanakuuun 16:140e758346ae 363 update();
sakanakuuun 16:140e758346ae 364
sakanakuuun 16:140e758346ae 365 px = coordinateX();
sakanakuuun 16:140e758346ae 366 py = coordinateY();
sakanakuuun 16:140e758346ae 367
sakanakuuun 16:140e758346ae 368 length = 300;
sakanakuuun 16:140e758346ae 369
sakanakuuun 16:140e758346ae 370 turnrad(nearPi(coordinateTheta()));
sakanakuuun 16:140e758346ae 371
sakanakuuun 16:140e758346ae 372 while(1) {
sakanakuuun 16:140e758346ae 373 update_np();
sakanakuuun 16:140e758346ae 374 dx = coordinateX() - px;
sakanakuuun 16:140e758346ae 375 dy = coordinateY() - py;
sakanakuuun 16:140e758346ae 376
sakanakuuun 16:140e758346ae 377 if(dy>max_disorder) {
sakanakuuun 16:140e758346ae 378 dy = max_disorder;
sakanakuuun 16:140e758346ae 379 } else if(dy<-max_disorder) {
sakanakuuun 16:140e758346ae 380 dy = -max_disorder;
sakanakuuun 16:140e758346ae 381 }
sakanakuuun 16:140e758346ae 382
sakanakuuun 16:140e758346ae 383
sakanakuuun 16:140e758346ae 384 move(-(30 - k*dy), -(32 + k*dy));
sakanakuuun 16:140e758346ae 385
sakanakuuun 16:140e758346ae 386
sakanakuuun 16:140e758346ae 387 if(abs(dx)>length) {
sakanakuuun 16:140e758346ae 388 move(0, 0);
sakanakuuun 16:140e758346ae 389 break;
sakanakuuun 16:140e758346ae 390 }
sakanakuuun 16:140e758346ae 391
sakanakuuun 16:140e758346ae 392 //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
sakanakuuun 16:140e758346ae 393 }
sakanakuuun 16:140e758346ae 394
sakanakuuun 16:140e758346ae 395 wait(0.5);
sakanakuuun 16:140e758346ae 396
sakanakuuun 16:140e758346ae 397 red = 0;
sakanakuuun 16:140e758346ae 398 }
sakanakuuun 16:140e758346ae 399
sakanakuuun 16:140e758346ae 400
sakanakuuun 12:f41918f71131 401 float nearPi(float rad)
sakanakuuun 12:f41918f71131 402 {
sakanakuuun 12:f41918f71131 403 float npi = 0;
sakanakuuun 12:f41918f71131 404
sakanakuuun 12:f41918f71131 405 while(1)
sakanakuuun 12:f41918f71131 406 {
sakanakuuun 12:f41918f71131 407 if(rad > npi + PI)
sakanakuuun 12:f41918f71131 408 npi += 2*PI;
sakanakuuun 12:f41918f71131 409 else if(rad < npi - PI)
sakanakuuun 12:f41918f71131 410 npi -= 2*PI;
sakanakuuun 12:f41918f71131 411 else
sakanakuuun 12:f41918f71131 412 return npi;
sakanakuuun 12:f41918f71131 413 }
sakanakuuun 12:f41918f71131 414 }