Tk A / Move2

aaa

Fork of Move by 涼太郎 中村

move.cpp@19:0b0ed6b22de9, 2016-09-10 (annotated)

Committer:
sakanakuuun
Date:
Sat Sep 10 12:25:58 2016 +0000
Revision:
19:0b0ed6b22de9
Parent:
18:4c812a3c7411
Child:
20:908443fd2b09
a

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 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 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 19:0b0ed6b22de9 316 length = sqrt(pow(x - coordinateTheta(), 2) + pow(y - coordinateTheta(), 2));
sakanakuuun 18:4c812a3c7411 317
sakanakuuun 18:4c812a3c7411 318 pc2.printf("length:%f", length);
sakanakuuun 18:4c812a3c7411 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 19:0b0ed6b22de9 326
sakanakuuun 19:0b0ed6b22de9 327 ptheta += nearPi(coordinateTheta() - ptheta);
sakanakuuun 19:0b0ed6b22de9 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 18:4c812a3c7411 353 }
sakanakuuun 18:4c812a3c7411 354 /*
sakanakuuun 18:4c812a3c7411 355 else if(absd_length > length - 30) {
sakanakuuun 18:4c812a3c7411 356 daikei = abs(length - absd_length) / 30.0;
sakanakuuun 18:4c812a3c7411 357 */
sakanakuuun 18:4c812a3c7411 358 else
sakanakuuun 18:4c812a3c7411 359 daikei = 1;
sakanakuuun 18:4c812a3c7411 360
sakanakuuun 18:4c812a3c7411 361 move(daikei * (rightspeed*(1-ratio) + k*disorder - k_theta*dtheta) + rightspeed*ratio,
sakanakuuun 18:4c812a3c7411 362 daikei * (leftspeed *(1-ratio) - k*disorder + k_theta*dtheta) + leftspeed *ratio);
sakanakuuun 18:4c812a3c7411 363
sakanakuuun 18:4c812a3c7411 364 if(d_length <= 0) {
sakanakuuun 18:4c812a3c7411 365 move(0, 0);
sakanakuuun 18:4c812a3c7411 366 break;
sakanakuuun 18:4c812a3c7411 367 }
sakanakuuun 18:4c812a3c7411 368
sakanakuuun 18:4c812a3c7411 369 }
sakanakuuun 18:4c812a3c7411 370
sakanakuuun 18:4c812a3c7411 371 wait(0.2);
sakanakuuun 18:4c812a3c7411 372
sakanakuuun 18:4c812a3c7411 373 yellow = 0;
sakanakuuun 18:4c812a3c7411 374 red = 0;
sakanakuuun 18:4c812a3c7411 375 }
sakanakuuun 18:4c812a3c7411 376
sakanakuuun 18:4c812a3c7411 377 void pmove3(int x, int y)
sakanakuuun 18:4c812a3c7411 378 {
sakanakuuun 18:4c812a3c7411 379 yellow = 1;
sakanakuuun 18:4c812a3c7411 380
sakanakuuun 18:4c812a3c7411 381 float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
sakanakuuun 18:4c812a3c7411 382 int k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
sakanakuuun 18:4c812a3c7411 383
sakanakuuun 18:4c812a3c7411 384 int length, dx, dy;
sakanakuuun 18:4c812a3c7411 385 int *d_length, *disorder;
sakanakuuun 18:4c812a3c7411 386 int absd_length;
sakanakuuun 18:4c812a3c7411 387 float dtheta, ptheta;
sakanakuuun 18:4c812a3c7411 388 float daikei;
sakanakuuun 18:4c812a3c7411 389
sakanakuuun 18:4c812a3c7411 390 int direction;
sakanakuuun 18:4c812a3c7411 391
sakanakuuun 18:4c812a3c7411 392 if(abs(x - coordinateX()) > abs(y - coordinateY())) {
sakanakuuun 18:4c812a3c7411 393 direction = X_PLUS;
sakanakuuun 18:4c812a3c7411 394 length = abs(x - coordinateX());
sakanakuuun 18:4c812a3c7411 395 d_length = &dx;
sakanakuuun 18:4c812a3c7411 396 disorder = &dy;
sakanakuuun 18:4c812a3c7411 397 } else {
sakanakuuun 18:4c812a3c7411 398 direction = Y_PLUS;
sakanakuuun 18:4c812a3c7411 399 length = abs(y - coordinateY());
sakanakuuun 18:4c812a3c7411 400 d_length = &dy;
sakanakuuun 18:4c812a3c7411 401 disorder = &dx;
sakanakuuun 18:4c812a3c7411 402 }
sakanakuuun 18:4c812a3c7411 403
sakanakuuun 18:4c812a3c7411 404 update();
sakanakuuun 18:4c812a3c7411 405 dx = x - coordinateX();
sakanakuuun 18:4c812a3c7411 406 dy = y - coordinateY();
sakanakuuun 18:4c812a3c7411 407
sakanakuuun 18:4c812a3c7411 408 if(*d_length < 0) //x,y減少方向なら、*d_length<0
sakanakuuun 18:4c812a3c7411 409 direction *= -1;
sakanakuuun 18:4c812a3c7411 410
sakanakuuun 18:4c812a3c7411 411 pc2.printf("direction:%d", direction);
sakanakuuun 18:4c812a3c7411 412
sakanakuuun 18:4c812a3c7411 413 switch(direction) {
sakanakuuun 18:4c812a3c7411 414 case X_PLUS:
sakanakuuun 18:4c812a3c7411 415 ptheta = 0;
sakanakuuun 18:4c812a3c7411 416 break;
sakanakuuun 18:4c812a3c7411 417 case Y_PLUS:
sakanakuuun 18:4c812a3c7411 418 k *= -1;
sakanakuuun 18:4c812a3c7411 419 ptheta = PI/2;
sakanakuuun 18:4c812a3c7411 420 break;
sakanakuuun 18:4c812a3c7411 421 case X_MINUS:
sakanakuuun 18:4c812a3c7411 422 k *= -1;
sakanakuuun 18:4c812a3c7411 423 ptheta = PI;
sakanakuuun 18:4c812a3c7411 424 break;
sakanakuuun 18:4c812a3c7411 425 case Y_MINUS:
sakanakuuun 18:4c812a3c7411 426 ptheta = -PI/2;
sakanakuuun 18:4c812a3c7411 427 break;
sakanakuuun 18:4c812a3c7411 428 default:
sakanakuuun 18:4c812a3c7411 429 return;
sakanakuuun 18:4c812a3c7411 430 }
sakanakuuun 18:4c812a3c7411 431
sakanakuuun 18:4c812a3c7411 432 ptheta += nearPi(coordinateTheta() - ptheta);
sakanakuuun 18:4c812a3c7411 433
sakanakuuun 18:4c812a3c7411 434 turnrad(ptheta);
sakanakuuun 18:4c812a3c7411 435
sakanakuuun 18:4c812a3c7411 436 if(length == 0) return;
sakanakuuun 18:4c812a3c7411 437
sakanakuuun 18:4c812a3c7411 438 int i = 0;
sakanakuuun 18:4c812a3c7411 439
sakanakuuun 18:4c812a3c7411 440 while(1) {
sakanakuuun 18:4c812a3c7411 441 update_np();
sakanakuuun 18:4c812a3c7411 442 dx = x - coordinateX();
sakanakuuun 18:4c812a3c7411 443 dy = y - coordinateY();
sakanakuuun 18:4c812a3c7411 444 dtheta = coordinateTheta() - ptheta;
sakanakuuun 18:4c812a3c7411 445
sakanakuuun 18:4c812a3c7411 446 if(*disorder>max_disorder) {
sakanakuuun 18:4c812a3c7411 447 *disorder = max_disorder;
sakanakuuun 18:4c812a3c7411 448 } else if(*disorder<-max_disorder) {
sakanakuuun 18:4c812a3c7411 449 *disorder = -max_disorder;
sakanakuuun 18:4c812a3c7411 450 }
sakanakuuun 18:4c812a3c7411 451
sakanakuuun 18:4c812a3c7411 452 absd_length = abs(*d_length);
sakanakuuun 18:4c812a3c7411 453
sakanakuuun 18:4c812a3c7411 454
sakanakuuun 18:4c812a3c7411 455 if(i++ < 5) {
sakanakuuun 18:4c812a3c7411 456 daikei = i/5;
sakanakuuun 18:4c812a3c7411 457 } else if(absd_length < 300) {
sakanakuuun 18:4c812a3c7411 458 daikei = absd_length / 300.0;
sakanakuuun 18:4c812a3c7411 459 }
sakanakuuun 18:4c812a3c7411 460 /*
sakanakuuun 18:4c812a3c7411 461 else if(absd_length > length - 30) {
sakanakuuun 18:4c812a3c7411 462 daikei = abs(length - absd_length) / 30.0;
sakanakuuun 18:4c812a3c7411 463 */
sakanakuuun 18:4c812a3c7411 464 else
sakanakuuun 18:4c812a3c7411 465 daikei = 1;
sakanakuuun 18:4c812a3c7411 466
sakanakuuun 18:4c812a3c7411 467 move(daikei * (rightspeed*(1-ratio) + k*(*disorder) - k_theta*dtheta) + rightspeed*ratio,
sakanakuuun 18:4c812a3c7411 468 daikei * (leftspeed *(1-ratio) - k*(*disorder) + k_theta*dtheta) + leftspeed *ratio);
sakanakuuun 18:4c812a3c7411 469
sakanakuuun 18:4c812a3c7411 470 //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 18:4c812a3c7411 471 if((direction > 0 && *d_length <= 0) || (direction < 0 && *d_length >= 0)) {
sakanakuuun 18:4c812a3c7411 472 move(0, 0);
sakanakuuun 18:4c812a3c7411 473 break;
sakanakuuun 18:4c812a3c7411 474 }
sakanakuuun 18:4c812a3c7411 475
sakanakuuun 18:4c812a3c7411 476 }
sakanakuuun 18:4c812a3c7411 477
sakanakuuun 18:4c812a3c7411 478 wait(0.2);
sakanakuuun 18:4c812a3c7411 479
sakanakuuun 18:4c812a3c7411 480 yellow = 0;
sakanakuuun 18:4c812a3c7411 481 }
sakanakuuun 18:4c812a3c7411 482
sakanakuuun 18:4c812a3c7411 483 float giveatan(int targetx,int targety)
sakanakuuun 18:4c812a3c7411 484 {
sakanakuuun 18:4c812a3c7411 485 int x,y;
sakanakuuun 18:4c812a3c7411 486 float theta;
sakanakuuun 18:4c812a3c7411 487 float phi;
sakanakuuun 18:4c812a3c7411 488 update();
sakanakuuun 18:4c812a3c7411 489 x = coordinateX();
sakanakuuun 18:4c812a3c7411 490 y = coordinateY();
sakanakuuun 18:4c812a3c7411 491 theta = coordinateTheta();//自己位置取得
sakanakuuun 18:4c812a3c7411 492 phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得
sakanakuuun 18:4c812a3c7411 493 while(phi>PI) {
sakanakuuun 18:4c812a3c7411 494 phi-=2*PI;
sakanakuuun 18:4c812a3c7411 495 }
sakanakuuun 18:4c812a3c7411 496 while(phi<=(-1)*PI) {
sakanakuuun 18:4c812a3c7411 497 phi+=2*PI;
sakanakuuun 18:4c812a3c7411 498 }
sakanakuuun 18:4c812a3c7411 499
sakanakuuun 18:4c812a3c7411 500 return phi;
sakanakuuun 18:4c812a3c7411 501 }
sakanakuuun 18:4c812a3c7411 502
sakanakuuun 6:0aa97a99c9cb 503 void back300()
sakanakuuun 7:7f1721542753 504 {
sakanakuuun 7:7f1721542753 505 red = 1;
sakanakuuun 7:7f1721542753 506
sakanakuuun 4:4c574be6325c 507 float k = 0.9;
sakanakuuun 4:4c574be6325c 508 int k_theta = 2;
sakanakuuun 4:4c574be6325c 509
sakanakuuun 6:0aa97a99c9cb 510 int length, px, py, dx, dy;
sakanakuuun 4:4c574be6325c 511 float daikei;
sakanakuuun 7:7f1721542753 512
sakanakuuun 6:0aa97a99c9cb 513 update();
sakanakuuun 7:7f1721542753 514
sakanakuuun 7:7f1721542753 515 px = coordinateX();
sakanakuuun 7:7f1721542753 516 py = coordinateY();
sakanakuuun 7:7f1721542753 517
sakanakuuun 6:0aa97a99c9cb 518 length = 300;
sakanakuuun 7:7f1721542753 519
sakanakuuun 15:403e9c57c1a1 520 turnrad(PI + nearPi(coordinateTheta() - PI));
sakanakuuun 7:7f1721542753 521
sakanakuuun 4:4c574be6325c 522 while(1) {
sakanakuuun 14:db58d3b0ecde 523 update_np();
sakanakuuun 6:0aa97a99c9cb 524 dx = coordinateX() - px;
sakanakuuun 6:0aa97a99c9cb 525 dy = coordinateY() - py;
sakanakuuun 4:4c574be6325c 526
sakanakuuun 12:f41918f71131 527 if(dy>max_disorder) {
sakanakuuun 12:f41918f71131 528 dy = max_disorder;
sakanakuuun 12:f41918f71131 529 } else if(dy<-max_disorder) {
sakanakuuun 12:f41918f71131 530 dy = -max_disorder;
sakanakuuun 6:0aa97a99c9cb 531 }
sakanakuuun 7:7f1721542753 532
sakanakuuun 4:4c574be6325c 533
sakanakuuun 8:3bf4addaaedd 534 move(-(30 + k*dy), -(32 - k*dy));
sakanakuuun 4:4c574be6325c 535
sakanakuuun 7:7f1721542753 536
sakanakuuun 6:0aa97a99c9cb 537 if(dx>length) {
sakanakuuun 4:4c574be6325c 538 move(0, 0);
sakanakuuun 4:4c574be6325c 539 break;
sakanakuuun 4:4c574be6325c 540 }
sakanakuuun 4:4c574be6325c 541
sakanakuuun 4:4c574be6325c 542 //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
sakanakuuun 4:4c574be6325c 543 }
sakanakuuun 4:4c574be6325c 544
sakanakuuun 18:4c812a3c7411 545 wait(0.2);
sakanakuuun 7:7f1721542753 546
sakanakuuun 7:7f1721542753 547 red = 0;
sakanakuuun 4:4c574be6325c 548 }
sakanakuuun 12:f41918f71131 549
sakanakuuun 16:140e758346ae 550
sakanakuuun 16:140e758346ae 551 void nxback300()
sakanakuuun 16:140e758346ae 552 {
sakanakuuun 16:140e758346ae 553 red = 1;
sakanakuuun 16:140e758346ae 554
sakanakuuun 16:140e758346ae 555 float k = 0.9;
sakanakuuun 16:140e758346ae 556 int k_theta = 2;
sakanakuuun 16:140e758346ae 557
sakanakuuun 16:140e758346ae 558 int length, px, py, dx, dy;
sakanakuuun 16:140e758346ae 559 float daikei;
sakanakuuun 16:140e758346ae 560
sakanakuuun 16:140e758346ae 561 update();
sakanakuuun 16:140e758346ae 562
sakanakuuun 16:140e758346ae 563 px = coordinateX();
sakanakuuun 16:140e758346ae 564 py = coordinateY();
sakanakuuun 16:140e758346ae 565
sakanakuuun 16:140e758346ae 566 length = 300;
sakanakuuun 16:140e758346ae 567
sakanakuuun 16:140e758346ae 568 turnrad(nearPi(coordinateTheta()));
sakanakuuun 16:140e758346ae 569
sakanakuuun 16:140e758346ae 570 while(1) {
sakanakuuun 16:140e758346ae 571 update_np();
sakanakuuun 16:140e758346ae 572 dx = coordinateX() - px;
sakanakuuun 16:140e758346ae 573 dy = coordinateY() - py;
sakanakuuun 16:140e758346ae 574
sakanakuuun 16:140e758346ae 575 if(dy>max_disorder) {
sakanakuuun 16:140e758346ae 576 dy = max_disorder;
sakanakuuun 16:140e758346ae 577 } else if(dy<-max_disorder) {
sakanakuuun 16:140e758346ae 578 dy = -max_disorder;
sakanakuuun 16:140e758346ae 579 }
sakanakuuun 16:140e758346ae 580
sakanakuuun 16:140e758346ae 581
sakanakuuun 16:140e758346ae 582 move(-(30 - k*dy), -(32 + k*dy));
sakanakuuun 16:140e758346ae 583
sakanakuuun 16:140e758346ae 584
sakanakuuun 16:140e758346ae 585 if(abs(dx)>length) {
sakanakuuun 16:140e758346ae 586 move(0, 0);
sakanakuuun 16:140e758346ae 587 break;
sakanakuuun 16:140e758346ae 588 }
sakanakuuun 16:140e758346ae 589
sakanakuuun 16:140e758346ae 590 //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
sakanakuuun 16:140e758346ae 591 }
sakanakuuun 16:140e758346ae 592
sakanakuuun 18:4c812a3c7411 593 wait(0.2);
sakanakuuun 16:140e758346ae 594
sakanakuuun 16:140e758346ae 595 red = 0;
sakanakuuun 16:140e758346ae 596 }
sakanakuuun 16:140e758346ae 597
sakanakuuun 16:140e758346ae 598
sakanakuuun 17:c167f4ed9070 599 void pyback300()
sakanakuuun 17:c167f4ed9070 600 {
sakanakuuun 17:c167f4ed9070 601 red = 1;
sakanakuuun 17:c167f4ed9070 602
sakanakuuun 17:c167f4ed9070 603 float k = 0.9;
sakanakuuun 17:c167f4ed9070 604 int k_theta = 2;
sakanakuuun 17:c167f4ed9070 605
sakanakuuun 17:c167f4ed9070 606 int length, px, py, dx, dy;
sakanakuuun 17:c167f4ed9070 607 float daikei;
sakanakuuun 17:c167f4ed9070 608
sakanakuuun 17:c167f4ed9070 609 update();
sakanakuuun 17:c167f4ed9070 610
sakanakuuun 17:c167f4ed9070 611 px = coordinateX();
sakanakuuun 17:c167f4ed9070 612 py = coordinateY();
sakanakuuun 17:c167f4ed9070 613
sakanakuuun 17:c167f4ed9070 614 length = 300;
sakanakuuun 17:c167f4ed9070 615
sakanakuuun 18:4c812a3c7411 616 turnrad(PI/2 + nearPi(coordinateTheta() - PI/2));
sakanakuuun 17:c167f4ed9070 617
sakanakuuun 17:c167f4ed9070 618 while(1) {
sakanakuuun 17:c167f4ed9070 619 update_np();
sakanakuuun 17:c167f4ed9070 620 dx = coordinateX() - px;
sakanakuuun 17:c167f4ed9070 621 dy = coordinateY() - py;
sakanakuuun 17:c167f4ed9070 622
sakanakuuun 17:c167f4ed9070 623 if(dx>max_disorder) {
sakanakuuun 17:c167f4ed9070 624 dx = max_disorder;
sakanakuuun 17:c167f4ed9070 625 } else if(dx<-max_disorder) {
sakanakuuun 17:c167f4ed9070 626 dx = -max_disorder;
sakanakuuun 17:c167f4ed9070 627 }
sakanakuuun 17:c167f4ed9070 628
sakanakuuun 17:c167f4ed9070 629
sakanakuuun 17:c167f4ed9070 630 move(-(30 - k*dx), -(32 + k*dx));
sakanakuuun 17:c167f4ed9070 631
sakanakuuun 17:c167f4ed9070 632
sakanakuuun 17:c167f4ed9070 633 if(dy>length) {
sakanakuuun 17:c167f4ed9070 634 move(0, 0);
sakanakuuun 17:c167f4ed9070 635 break;
sakanakuuun 17:c167f4ed9070 636 }
sakanakuuun 17:c167f4ed9070 637
sakanakuuun 17:c167f4ed9070 638 //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
sakanakuuun 17:c167f4ed9070 639 }
sakanakuuun 17:c167f4ed9070 640
sakanakuuun 18:4c812a3c7411 641 wait(0.2);
sakanakuuun 17:c167f4ed9070 642
sakanakuuun 18:4c812a3c7411 643 red
sakanakuuun 18:4c812a3c7411 644
sakanakuuun 18:4c812a3c7411 645 = 0;
sakanakuuun 17:c167f4ed9070 646 }
sakanakuuun 17:c167f4ed9070 647
sakanakuuun 18:4c812a3c7411 648 void nyback300()
sakanakuuun 18:4c812a3c7411 649 {
sakanakuuun 18:4c812a3c7411 650 {
sakanakuuun 18:4c812a3c7411 651 red = 1;
sakanakuuun 18:4c812a3c7411 652
sakanakuuun 18:4c812a3c7411 653 float k = 0.9;
sakanakuuun 18:4c812a3c7411 654 int k_theta = 2;
sakanakuuun 18:4c812a3c7411 655
sakanakuuun 18:4c812a3c7411 656 int length, px, py, dx, dy;
sakanakuuun 18:4c812a3c7411 657 float daikei;
sakanakuuun 18:4c812a3c7411 658
sakanakuuun 18:4c812a3c7411 659 update();
sakanakuuun 18:4c812a3c7411 660
sakanakuuun 18:4c812a3c7411 661 px = coordinateX();
sakanakuuun 18:4c812a3c7411 662 py = coordinateY();
sakanakuuun 18:4c812a3c7411 663
sakanakuuun 18:4c812a3c7411 664 length = 300;
sakanakuuun 18:4c812a3c7411 665
sakanakuuun 18:4c812a3c7411 666 turnrad(-PI/2 + nearPi(coordinateTheta() + PI/2));
sakanakuuun 18:4c812a3c7411 667
sakanakuuun 18:4c812a3c7411 668 while(1) {
sakanakuuun 18:4c812a3c7411 669 update_np();
sakanakuuun 18:4c812a3c7411 670 dx = coordinateX() - px;
sakanakuuun 18:4c812a3c7411 671 dy = coordinateY() - py;
sakanakuuun 18:4c812a3c7411 672
sakanakuuun 18:4c812a3c7411 673 if(dx>max_disorder) {
sakanakuuun 18:4c812a3c7411 674 dx = max_disorder;
sakanakuuun 18:4c812a3c7411 675 } else if(dx<-max_disorder) {
sakanakuuun 18:4c812a3c7411 676 dx = -max_disorder;
sakanakuuun 18:4c812a3c7411 677 }
sakanakuuun 18:4c812a3c7411 678
sakanakuuun 18:4c812a3c7411 679
sakanakuuun 18:4c812a3c7411 680 move(-(30 - k*dx), -(32 + k*dx));
sakanakuuun 18:4c812a3c7411 681
sakanakuuun 18:4c812a3c7411 682
sakanakuuun 18:4c812a3c7411 683 if(abs(dy)>length) {
sakanakuuun 18:4c812a3c7411 684 move(0, 0);
sakanakuuun 18:4c812a3c7411 685 break;
sakanakuuun 18:4c812a3c7411 686 }
sakanakuuun 18:4c812a3c7411 687
sakanakuuun 18:4c812a3c7411 688 //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
sakanakuuun 18:4c812a3c7411 689 }
sakanakuuun 18:4c812a3c7411 690
sakanakuuun 18:4c812a3c7411 691 wait(0.2);
sakanakuuun 18:4c812a3c7411 692
sakanakuuun 18:4c812a3c7411 693 red = 0;
sakanakuuun 18:4c812a3c7411 694 }
sakanakuuun 18:4c812a3c7411 695 }
sakanakuuun 17:c167f4ed9070 696
sakanakuuun 12:f41918f71131 697 float nearPi(float rad)
sakanakuuun 12:f41918f71131 698 {
sakanakuuun 12:f41918f71131 699 float npi = 0;
sakanakuuun 18:4c812a3c7411 700
sakanakuuun 18:4c812a3c7411 701 while(1) {
sakanakuuun 12:f41918f71131 702 if(rad > npi + PI)
sakanakuuun 12:f41918f71131 703 npi += 2*PI;
sakanakuuun 12:f41918f71131 704 else if(rad < npi - PI)
sakanakuuun 12:f41918f71131 705 npi -= 2*PI;
sakanakuuun 12:f41918f71131 706 else
sakanakuuun 12:f41918f71131 707 return npi;
sakanakuuun 12:f41918f71131 708 }
sakanakuuun 12:f41918f71131 709 }