Tk A / Move2

aaa

Fork of Move by 涼太郎 中村

move.cpp@18:4c812a3c7411, 2016-09-10 (annotated)

Committer:
sakanakuuun
Date:
Sat Sep 10 12:14:26 2016 +0000
Revision:
18:4c812a3c7411
Parent:
17:c167f4ed9070
Child:
19:0b0ed6b22de9
f

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