Tk A / Move2

aaa

Fork of Move by 涼太郎 中村

move.cpp@17:c167f4ed9070, 2016-09-10 (annotated)

Committer:
sakanakuuun
Date:
Sat Sep 10 06:51:19 2016 +0000
Revision:
17:c167f4ed9070
Parent:
16:140e758346ae
Child:
18:4c812a3c7411
aaa

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 17:c167f4ed9070 27 const int hosei_ = 13;
sakanakuuun 13:176e543e6d64 28 const int max_disorder = 4;
sakanakuuun 14:db58d3b0ecde 29 const float ratio = 1.0/7.5;
sakanakuuun 0:d7ff86f25eaa 30 //const float PIfact=2.89;
sakanakuuun 0:d7ff86f25eaa 31
sakanakuuun 0:d7ff86f25eaa 32
sakanakuuun 0:d7ff86f25eaa 33 void initmotor()
sakanakuuun 0:d7ff86f25eaa 34 {
sakanakuuun 0:d7ff86f25eaa 35 M1cw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 36 M1ccw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 37 M2cw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 38 M2ccw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 39
sakanakuuun 0:d7ff86f25eaa 40 }
sakanakuuun 0:d7ff86f25eaa 41
sakanakuuun 0:d7ff86f25eaa 42 void move(int left,int right)
sakanakuuun 0:d7ff86f25eaa 43 {
sakanakuuun 0:d7ff86f25eaa 44
sakanakuuun 0:d7ff86f25eaa 45 float rightduty,leftduty;
sakanakuuun 0:d7ff86f25eaa 46
sakanakuuun 0:d7ff86f25eaa 47 if(right>256) {
sakanakuuun 0:d7ff86f25eaa 48 right=256;
sakanakuuun 0:d7ff86f25eaa 49 }
sakanakuuun 0:d7ff86f25eaa 50 if(left>256) {
sakanakuuun 0:d7ff86f25eaa 51 left=256;
sakanakuuun 0:d7ff86f25eaa 52 }
sakanakuuun 0:d7ff86f25eaa 53 if(right<-256) {
sakanakuuun 0:d7ff86f25eaa 54 right=-256;
sakanakuuun 0:d7ff86f25eaa 55 }
sakanakuuun 0:d7ff86f25eaa 56 if(left<-256) {
sakanakuuun 0:d7ff86f25eaa 57 left=-256;
sakanakuuun 0:d7ff86f25eaa 58 }
sakanakuuun 0:d7ff86f25eaa 59
sakanakuuun 0:d7ff86f25eaa 60 rightduty=right/256.0;
sakanakuuun 0:d7ff86f25eaa 61 leftduty=left/256.0;
sakanakuuun 0:d7ff86f25eaa 62 if(right>0) {
sakanakuuun 0:d7ff86f25eaa 63 M1cw.write(1-rightduty);
sakanakuuun 0:d7ff86f25eaa 64 M1ccw.write(1);
sakanakuuun 0:d7ff86f25eaa 65 } else {
sakanakuuun 0:d7ff86f25eaa 66 M1cw.write(1);
sakanakuuun 0:d7ff86f25eaa 67 M1ccw.write(1+rightduty);
sakanakuuun 0:d7ff86f25eaa 68 }
sakanakuuun 0:d7ff86f25eaa 69
sakanakuuun 0:d7ff86f25eaa 70 if(left>0) {
sakanakuuun 0:d7ff86f25eaa 71 M2cw.write(1-leftduty);
sakanakuuun 0:d7ff86f25eaa 72 M2ccw.write(1);
sakanakuuun 0:d7ff86f25eaa 73 } else {
sakanakuuun 0:d7ff86f25eaa 74 M2cw.write(1);
sakanakuuun 0:d7ff86f25eaa 75 M2ccw.write(1+leftduty);
sakanakuuun 0:d7ff86f25eaa 76 }
sakanakuuun 0:d7ff86f25eaa 77 }
sakanakuuun 0:d7ff86f25eaa 78
sakanakuuun 0:d7ff86f25eaa 79 void hosei_turn(float pt, bool cw, float rad)
sakanakuuun 0:d7ff86f25eaa 80 {
sakanakuuun 0:d7ff86f25eaa 81 int np;
sakanakuuun 0:d7ff86f25eaa 82 if(cw) np = 1;
sakanakuuun 0:d7ff86f25eaa 83 else np = -1;
sakanakuuun 0:d7ff86f25eaa 84 while(1) {
sakanakuuun 14:db58d3b0ecde 85 update_np();
sakanakuuun 2:f25a09c5e113 86 //pc.printf("t:%f\n\r", coordinateTheta());
sakanakuuun 0:d7ff86f25eaa 87 if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
sakanakuuun 17:c167f4ed9070 88 move(-hosei_, hosei_);
sakanakuuun 0:d7ff86f25eaa 89 } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
sakanakuuun 17:c167f4ed9070 90 move(hosei_, -hosei_);
sakanakuuun 0:d7ff86f25eaa 91 } else {
sakanakuuun 0:d7ff86f25eaa 92 move(0,0);
sakanakuuun 0:d7ff86f25eaa 93 return;
sakanakuuun 0:d7ff86f25eaa 94 }
sakanakuuun 0:d7ff86f25eaa 95 }
sakanakuuun 2:f25a09c5e113 96
sakanakuuun 0:d7ff86f25eaa 97 }
sakanakuuun 0:d7ff86f25eaa 98
sakanakuuun 15:403e9c57c1a1 99 void turnrad(float rad)
sakanakuuun 0:d7ff86f25eaa 100 {
sakanakuuun 7:7f1721542753 101 green = 1;
sakanakuuun 7:7f1721542753 102
sakanakuuun 14:db58d3b0ecde 103 update_np();
sakanakuuun 0:d7ff86f25eaa 104
sakanakuuun 1:405e28b64fdb 105 int np;
sakanakuuun 1:405e28b64fdb 106 if(coordinateTheta() > rad) np = 1;
sakanakuuun 1:405e28b64fdb 107 else if(coordinateTheta() < rad) np = -1;
sakanakuuun 1:405e28b64fdb 108 else return;
sakanakuuun 1:405e28b64fdb 109
sakanakuuun 17:c167f4ed9070 110 move((-np)*rightspeed, (np)*leftspeed);
sakanakuuun 0:d7ff86f25eaa 111
sakanakuuun 0:d7ff86f25eaa 112 while(1) {
sakanakuuun 14:db58d3b0ecde 113 update_np();
sakanakuuun 2:f25a09c5e113 114 //pc.printf("t:%f\n\r", coordinateTheta());
sakanakuuun 2:f25a09c5e113 115 if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
sakanakuuun 0:d7ff86f25eaa 116 move(0,0);
sakanakuuun 0:d7ff86f25eaa 117 break;
sakanakuuun 0:d7ff86f25eaa 118 }
sakanakuuun 0:d7ff86f25eaa 119 }
sakanakuuun 0:d7ff86f25eaa 120
sakanakuuun 1:405e28b64fdb 121
sakanakuuun 1:405e28b64fdb 122
sakanakuuun 2:f25a09c5e113 123 hosei_turn(0, false, rad);
sakanakuuun 1:405e28b64fdb 124
sakanakuuun 2:f25a09c5e113 125 wait(0.5);
sakanakuuun 1:405e28b64fdb 126
sakanakuuun 2:f25a09c5e113 127 hosei_turn(0, false, rad);
sakanakuuun 1:405e28b64fdb 128
sakanakuuun 2:f25a09c5e113 129 wait(0.5);
sakanakuuun 7:7f1721542753 130 green = 0;
sakanakuuun 7:7f1721542753 131
sakanakuuun 1:405e28b64fdb 132 }
sakanakuuun 1:405e28b64fdb 133
sakanakuuun 15:403e9c57c1a1 134
sakanakuuun 15:403e9c57c1a1 135 void turnrad_ccw(float rad)
sakanakuuun 15:403e9c57c1a1 136 {
sakanakuuun 15:403e9c57c1a1 137 green = 1;
sakanakuuun 15:403e9c57c1a1 138
sakanakuuun 15:403e9c57c1a1 139 update();
sakanakuuun 15:403e9c57c1a1 140
sakanakuuun 17:c167f4ed9070 141 move(rightspeed, -leftspeed);
sakanakuuun 15:403e9c57c1a1 142
sakanakuuun 15:403e9c57c1a1 143 while(1) {
sakanakuuun 15:403e9c57c1a1 144 update();
sakanakuuun 15:403e9c57c1a1 145 //pc.printf("t:%f\n\r", coordinateTheta());
sakanakuuun 15:403e9c57c1a1 146 if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
sakanakuuun 15:403e9c57c1a1 147 move(0,0);
sakanakuuun 15:403e9c57c1a1 148 break;
sakanakuuun 15:403e9c57c1a1 149 }
sakanakuuun 15:403e9c57c1a1 150 }
sakanakuuun 15:403e9c57c1a1 151
sakanakuuun 15:403e9c57c1a1 152
sakanakuuun 15:403e9c57c1a1 153
sakanakuuun 15:403e9c57c1a1 154 hosei_turn(0, false, rad);
sakanakuuun 15:403e9c57c1a1 155
sakanakuuun 15:403e9c57c1a1 156 wait(0.5);
sakanakuuun 15:403e9c57c1a1 157
sakanakuuun 15:403e9c57c1a1 158 hosei_turn(0, false, rad);
sakanakuuun 15:403e9c57c1a1 159
sakanakuuun 15:403e9c57c1a1 160
sakanakuuun 15:403e9c57c1a1 161 wait(0.5);
sakanakuuun 15:403e9c57c1a1 162 green = 0;
sakanakuuun 15:403e9c57c1a1 163 }
sakanakuuun 15:403e9c57c1a1 164
sakanakuuun 15:403e9c57c1a1 165 void turnrad_cw(float rad)
sakanakuuun 15:403e9c57c1a1 166 {
sakanakuuun 15:403e9c57c1a1 167 green = 1;
sakanakuuun 15:403e9c57c1a1 168
sakanakuuun 15:403e9c57c1a1 169 update();
sakanakuuun 15:403e9c57c1a1 170
sakanakuuun 17:c167f4ed9070 171 move((-1)*rightspeed, leftspeed);
sakanakuuun 15:403e9c57c1a1 172
sakanakuuun 15:403e9c57c1a1 173 while(1) {
sakanakuuun 15:403e9c57c1a1 174 update();
sakanakuuun 15:403e9c57c1a1 175 //pc.printf("t:%f\n\r", coordinateTheta());
sakanakuuun 15:403e9c57c1a1 176 if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
sakanakuuun 15:403e9c57c1a1 177 move(0,0);
sakanakuuun 15:403e9c57c1a1 178 break;
sakanakuuun 15:403e9c57c1a1 179 }
sakanakuuun 15:403e9c57c1a1 180 }
sakanakuuun 15:403e9c57c1a1 181
sakanakuuun 15:403e9c57c1a1 182
sakanakuuun 15:403e9c57c1a1 183
sakanakuuun 15:403e9c57c1a1 184 hosei_turn(0, false, rad);
sakanakuuun 15:403e9c57c1a1 185
sakanakuuun 15:403e9c57c1a1 186 wait(0.5);
sakanakuuun 15:403e9c57c1a1 187
sakanakuuun 15:403e9c57c1a1 188 hosei_turn(0, false, rad);
sakanakuuun 15:403e9c57c1a1 189
sakanakuuun 15:403e9c57c1a1 190 wait(0.5);
sakanakuuun 15:403e9c57c1a1 191 green = 0;
sakanakuuun 15:403e9c57c1a1 192 }
sakanakuuun 15:403e9c57c1a1 193
sakanakuuun 5:0e18cf25291a 194 void pmove(int x, int y)
sakanakuuun 7:7f1721542753 195 {
sakanakuuun 7:7f1721542753 196 yellow = 1;
sakanakuuun 7:7f1721542753 197
sakanakuuun 14:db58d3b0ecde 198 float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
sakanakuuun 14:db58d3b0ecde 199 int k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
sakanakuuun 7:7f1721542753 200
sakanakuuun 4:4c574be6325c 201 int length, dx, dy;
sakanakuuun 2:f25a09c5e113 202 int *d_length, *disorder;
sakanakuuun 5:0e18cf25291a 203 int absd_length;
sakanakuuun 4:4c574be6325c 204 float dtheta, ptheta;
sakanakuuun 2:f25a09c5e113 205 float daikei;
sakanakuuun 1:405e28b64fdb 206
sakanakuuun 7:7f1721542753 207 int direction;
sakanakuuun 7:7f1721542753 208
sakanakuuun 5:0e18cf25291a 209 if(abs(x - coordinateX()) > abs(y - coordinateY())) {
sakanakuuun 5:0e18cf25291a 210 y = coordinateY();
sakanakuuun 5:0e18cf25291a 211 direction = X_PLUS;
sakanakuuun 5:0e18cf25291a 212 length = abs(x - coordinateX());
sakanakuuun 5:0e18cf25291a 213 d_length = &dx;
sakanakuuun 5:0e18cf25291a 214 disorder = &dy;
sakanakuuun 7:7f1721542753 215 } else {
sakanakuuun 5:0e18cf25291a 216 x = coordinateX();
sakanakuuun 5:0e18cf25291a 217 direction = Y_PLUS;
sakanakuuun 5:0e18cf25291a 218 length = abs(y - coordinateY());
sakanakuuun 5:0e18cf25291a 219 d_length = &dy;
sakanakuuun 5:0e18cf25291a 220 disorder = &dx;
sakanakuuun 5:0e18cf25291a 221 }
sakanakuuun 7:7f1721542753 222
sakanakuuun 7:7f1721542753 223 update();
sakanakuuun 7:7f1721542753 224 dx = x - coordinateX();
sakanakuuun 7:7f1721542753 225 dy = y - coordinateY();
sakanakuuun 12:f41918f71131 226
sakanakuuun 7:7f1721542753 227 if(*d_length < 0) //x,y減少方向なら、*d_length<0
sakanakuuun 5:0e18cf25291a 228 direction *= -1;
sakanakuuun 1:405e28b64fdb 229
sakanakuuun 7:7f1721542753 230 pc2.printf("direction:%d", direction);
sakanakuuun 7:7f1721542753 231
sakanakuuun 2:f25a09c5e113 232 switch(direction) {
sakanakuuun 2:f25a09c5e113 233 case X_PLUS:
sakanakuuun 2:f25a09c5e113 234 ptheta = 0;
sakanakuuun 2:f25a09c5e113 235 break;
sakanakuuun 2:f25a09c5e113 236 case Y_PLUS:
sakanakuuun 2:f25a09c5e113 237 k *= -1;
sakanakuuun 2:f25a09c5e113 238 ptheta = PI/2;
sakanakuuun 2:f25a09c5e113 239 break;
sakanakuuun 2:f25a09c5e113 240 case X_MINUS:
sakanakuuun 2:f25a09c5e113 241 k *= -1;
sakanakuuun 2:f25a09c5e113 242 ptheta = PI;
sakanakuuun 2:f25a09c5e113 243 break;
sakanakuuun 2:f25a09c5e113 244 case Y_MINUS:
choutin 10:6d38d1b6cad5 245 ptheta = -PI/2;
sakanakuuun 2:f25a09c5e113 246 break;
sakanakuuun 2:f25a09c5e113 247 default:
sakanakuuun 2:f25a09c5e113 248 return;
sakanakuuun 2:f25a09c5e113 249 }
sakanakuuun 12:f41918f71131 250
sakanakuuun 13:176e543e6d64 251 ptheta += nearPi(coordinateTheta() - ptheta);
sakanakuuun 12:f41918f71131 252
sakanakuuun 15:403e9c57c1a1 253 turnrad(ptheta);
sakanakuuun 5:0e18cf25291a 254
sakanakuuun 2:f25a09c5e113 255 if(length == 0) return;
sakanakuuun 14:db58d3b0ecde 256
sakanakuuun 14:db58d3b0ecde 257 int i = 0;
sakanakuuun 14:db58d3b0ecde 258
sakanakuuun 1:405e28b64fdb 259 while(1) {
sakanakuuun 14:db58d3b0ecde 260 update_np();
sakanakuuun 5:0e18cf25291a 261 dx = x - coordinateX();
sakanakuuun 5:0e18cf25291a 262 dy = y - coordinateY();
sakanakuuun 2:f25a09c5e113 263 dtheta = coordinateTheta() - ptheta;
sakanakuuun 2:f25a09c5e113 264
sakanakuuun 12:f41918f71131 265 if(*disorder>max_disorder) {
sakanakuuun 12:f41918f71131 266 *disorder = max_disorder;
sakanakuuun 12:f41918f71131 267 } else if(*disorder<-max_disorder) {
sakanakuuun 12:f41918f71131 268 *disorder = -max_disorder;
sakanakuuun 1:405e28b64fdb 269 }
sakanakuuun 1:405e28b64fdb 270
sakanakuuun 5:0e18cf25291a 271 absd_length = abs(*d_length);
sakanakuuun 14:db58d3b0ecde 272
sakanakuuun 14:db58d3b0ecde 273
sakanakuuun 14:db58d3b0ecde 274 if(i++ < 5) {
sakanakuuun 14:db58d3b0ecde 275 daikei = i/5;
sakanakuuun 14:db58d3b0ecde 276 }
sakanakuuun 14:db58d3b0ecde 277 else if(absd_length < 300) {
sakanakuuun 13:176e543e6d64 278 daikei = absd_length / 300.0;
sakanakuuun 14:db58d3b0ecde 279 }
sakanakuuun 14:db58d3b0ecde 280 /*
sakanakuuun 14:db58d3b0ecde 281 else if(absd_length > length - 30) {
sakanakuuun 7:7f1721542753 282 daikei = abs(length - absd_length) / 30.0;
sakanakuuun 14:db58d3b0ecde 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 2:f25a09c5e113 298 wait(0.5);
sakanakuuun 7:7f1721542753 299
sakanakuuun 7:7f1721542753 300 yellow = 0;
sakanakuuun 0:d7ff86f25eaa 301 }
sakanakuuun 3:cecaa0154f92 302
sakanakuuun 6:0aa97a99c9cb 303 void back300()
sakanakuuun 7:7f1721542753 304 {
sakanakuuun 7:7f1721542753 305 red = 1;
sakanakuuun 7:7f1721542753 306
sakanakuuun 4:4c574be6325c 307 float k = 0.9;
sakanakuuun 4:4c574be6325c 308 int k_theta = 2;
sakanakuuun 4:4c574be6325c 309
sakanakuuun 6:0aa97a99c9cb 310 int length, px, py, dx, dy;
sakanakuuun 4:4c574be6325c 311 float daikei;
sakanakuuun 7:7f1721542753 312
sakanakuuun 6:0aa97a99c9cb 313 update();
sakanakuuun 7:7f1721542753 314
sakanakuuun 7:7f1721542753 315 px = coordinateX();
sakanakuuun 7:7f1721542753 316 py = coordinateY();
sakanakuuun 7:7f1721542753 317
sakanakuuun 6:0aa97a99c9cb 318 length = 300;
sakanakuuun 7:7f1721542753 319
sakanakuuun 15:403e9c57c1a1 320 turnrad(PI + nearPi(coordinateTheta() - PI));
sakanakuuun 7:7f1721542753 321
sakanakuuun 4:4c574be6325c 322 while(1) {
sakanakuuun 14:db58d3b0ecde 323 update_np();
sakanakuuun 6:0aa97a99c9cb 324 dx = coordinateX() - px;
sakanakuuun 6:0aa97a99c9cb 325 dy = coordinateY() - py;
sakanakuuun 4:4c574be6325c 326
sakanakuuun 12:f41918f71131 327 if(dy>max_disorder) {
sakanakuuun 12:f41918f71131 328 dy = max_disorder;
sakanakuuun 12:f41918f71131 329 } else if(dy<-max_disorder) {
sakanakuuun 12:f41918f71131 330 dy = -max_disorder;
sakanakuuun 6:0aa97a99c9cb 331 }
sakanakuuun 7:7f1721542753 332
sakanakuuun 4:4c574be6325c 333
sakanakuuun 8:3bf4addaaedd 334 move(-(30 + k*dy), -(32 - k*dy));
sakanakuuun 4:4c574be6325c 335
sakanakuuun 7:7f1721542753 336
sakanakuuun 6:0aa97a99c9cb 337 if(dx>length) {
sakanakuuun 4:4c574be6325c 338 move(0, 0);
sakanakuuun 4:4c574be6325c 339 break;
sakanakuuun 4:4c574be6325c 340 }
sakanakuuun 4:4c574be6325c 341
sakanakuuun 4:4c574be6325c 342 //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
sakanakuuun 4:4c574be6325c 343 }
sakanakuuun 4:4c574be6325c 344
sakanakuuun 4:4c574be6325c 345 wait(0.5);
sakanakuuun 7:7f1721542753 346
sakanakuuun 7:7f1721542753 347 red = 0;
sakanakuuun 4:4c574be6325c 348 }
sakanakuuun 12:f41918f71131 349
sakanakuuun 16:140e758346ae 350
sakanakuuun 16:140e758346ae 351 void nxback300()
sakanakuuun 16:140e758346ae 352 {
sakanakuuun 16:140e758346ae 353 red = 1;
sakanakuuun 16:140e758346ae 354
sakanakuuun 16:140e758346ae 355 float k = 0.9;
sakanakuuun 16:140e758346ae 356 int k_theta = 2;
sakanakuuun 16:140e758346ae 357
sakanakuuun 16:140e758346ae 358 int length, px, py, dx, dy;
sakanakuuun 16:140e758346ae 359 float daikei;
sakanakuuun 16:140e758346ae 360
sakanakuuun 16:140e758346ae 361 update();
sakanakuuun 16:140e758346ae 362
sakanakuuun 16:140e758346ae 363 px = coordinateX();
sakanakuuun 16:140e758346ae 364 py = coordinateY();
sakanakuuun 16:140e758346ae 365
sakanakuuun 16:140e758346ae 366 length = 300;
sakanakuuun 16:140e758346ae 367
sakanakuuun 16:140e758346ae 368 turnrad(nearPi(coordinateTheta()));
sakanakuuun 16:140e758346ae 369
sakanakuuun 16:140e758346ae 370 while(1) {
sakanakuuun 16:140e758346ae 371 update_np();
sakanakuuun 16:140e758346ae 372 dx = coordinateX() - px;
sakanakuuun 16:140e758346ae 373 dy = coordinateY() - py;
sakanakuuun 16:140e758346ae 374
sakanakuuun 16:140e758346ae 375 if(dy>max_disorder) {
sakanakuuun 16:140e758346ae 376 dy = max_disorder;
sakanakuuun 16:140e758346ae 377 } else if(dy<-max_disorder) {
sakanakuuun 16:140e758346ae 378 dy = -max_disorder;
sakanakuuun 16:140e758346ae 379 }
sakanakuuun 16:140e758346ae 380
sakanakuuun 16:140e758346ae 381
sakanakuuun 16:140e758346ae 382 move(-(30 - k*dy), -(32 + k*dy));
sakanakuuun 16:140e758346ae 383
sakanakuuun 16:140e758346ae 384
sakanakuuun 16:140e758346ae 385 if(abs(dx)>length) {
sakanakuuun 16:140e758346ae 386 move(0, 0);
sakanakuuun 16:140e758346ae 387 break;
sakanakuuun 16:140e758346ae 388 }
sakanakuuun 16:140e758346ae 389
sakanakuuun 16:140e758346ae 390 //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
sakanakuuun 16:140e758346ae 391 }
sakanakuuun 16:140e758346ae 392
sakanakuuun 16:140e758346ae 393 wait(0.5);
sakanakuuun 16:140e758346ae 394
sakanakuuun 16:140e758346ae 395 red = 0;
sakanakuuun 16:140e758346ae 396 }
sakanakuuun 16:140e758346ae 397
sakanakuuun 16:140e758346ae 398
sakanakuuun 17:c167f4ed9070 399 void pyback300()
sakanakuuun 17:c167f4ed9070 400 {
sakanakuuun 17:c167f4ed9070 401 red = 1;
sakanakuuun 17:c167f4ed9070 402
sakanakuuun 17:c167f4ed9070 403 float k = 0.9;
sakanakuuun 17:c167f4ed9070 404 int k_theta = 2;
sakanakuuun 17:c167f4ed9070 405
sakanakuuun 17:c167f4ed9070 406 int length, px, py, dx, dy;
sakanakuuun 17:c167f4ed9070 407 float daikei;
sakanakuuun 17:c167f4ed9070 408
sakanakuuun 17:c167f4ed9070 409 update();
sakanakuuun 17:c167f4ed9070 410
sakanakuuun 17:c167f4ed9070 411 px = coordinateX();
sakanakuuun 17:c167f4ed9070 412 py = coordinateY();
sakanakuuun 17:c167f4ed9070 413
sakanakuuun 17:c167f4ed9070 414 length = 300;
sakanakuuun 17:c167f4ed9070 415
sakanakuuun 17:c167f4ed9070 416 turnrad(-PI/2 + nearPi(coordinateTheta() + PI/2));
sakanakuuun 17:c167f4ed9070 417
sakanakuuun 17:c167f4ed9070 418 while(1) {
sakanakuuun 17:c167f4ed9070 419 update_np();
sakanakuuun 17:c167f4ed9070 420 dx = coordinateX() - px;
sakanakuuun 17:c167f4ed9070 421 dy = coordinateY() - py;
sakanakuuun 17:c167f4ed9070 422
sakanakuuun 17:c167f4ed9070 423 if(dx>max_disorder) {
sakanakuuun 17:c167f4ed9070 424 dx = max_disorder;
sakanakuuun 17:c167f4ed9070 425 } else if(dx<-max_disorder) {
sakanakuuun 17:c167f4ed9070 426 dx = -max_disorder;
sakanakuuun 17:c167f4ed9070 427 }
sakanakuuun 17:c167f4ed9070 428
sakanakuuun 17:c167f4ed9070 429
sakanakuuun 17:c167f4ed9070 430 move(-(30 - k*dx), -(32 + k*dx));
sakanakuuun 17:c167f4ed9070 431
sakanakuuun 17:c167f4ed9070 432
sakanakuuun 17:c167f4ed9070 433 if(dy>length) {
sakanakuuun 17:c167f4ed9070 434 move(0, 0);
sakanakuuun 17:c167f4ed9070 435 break;
sakanakuuun 17:c167f4ed9070 436 }
sakanakuuun 17:c167f4ed9070 437
sakanakuuun 17:c167f4ed9070 438 //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
sakanakuuun 17:c167f4ed9070 439 }
sakanakuuun 17:c167f4ed9070 440
sakanakuuun 17:c167f4ed9070 441 wait(0.5);
sakanakuuun 17:c167f4ed9070 442
sakanakuuun 17:c167f4ed9070 443 red = 0;
sakanakuuun 17:c167f4ed9070 444 }
sakanakuuun 17:c167f4ed9070 445
sakanakuuun 17:c167f4ed9070 446
sakanakuuun 12:f41918f71131 447 float nearPi(float rad)
sakanakuuun 12:f41918f71131 448 {
sakanakuuun 12:f41918f71131 449 float npi = 0;
sakanakuuun 12:f41918f71131 450
sakanakuuun 12:f41918f71131 451 while(1)
sakanakuuun 12:f41918f71131 452 {
sakanakuuun 12:f41918f71131 453 if(rad > npi + PI)
sakanakuuun 12:f41918f71131 454 npi += 2*PI;
sakanakuuun 12:f41918f71131 455 else if(rad < npi - PI)
sakanakuuun 12:f41918f71131 456 npi -= 2*PI;
sakanakuuun 12:f41918f71131 457 else
sakanakuuun 12:f41918f71131 458 return npi;
sakanakuuun 12:f41918f71131 459 }
sakanakuuun 12:f41918f71131 460 }