涼太郎 中村 / Move

k

Dependents:   3rdcompfixstart 2ndcomp 4thcomp 6th33222_copy

Fork of Move by Tk A

move.cpp@12:4e9cadc225f5, 2016-09-10 (annotated)

Committer:
choutin
Date:
Sat Sep 10 13:30:03 2016 +0000
Revision:
12:4e9cadc225f5
Parent:
10:6d38d1b6cad5 
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"
choutin 12:4e9cadc225f5 5 #include "math.h"
choutin 12:4e9cadc225f5 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);
choutin 12:4e9cadc225f5 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);
choutin 12:4e9cadc225f5 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 */
choutin 12:4e9cadc225f5 20
sakanakuuun 7:7f1721542753 21 Serial pc2(SERIAL_TX, SERIAL_RX); //pcと通信
choutin 12:4e9cadc225f5 22
choutin 12:4e9cadc225f5 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;
choutin 12:4e9cadc225f5 27 const int leftspeed=rightspeed + 4;
choutin 12:4e9cadc225f5 28 const int hosei_ = 13;
choutin 12:4e9cadc225f5 29 const int max_disorder = 4;
choutin 12:4e9cadc225f5 30 const float ratio = 1.0/7.5;
sakanakuuun 0:d7ff86f25eaa 31 //const float PIfact=2.89;
choutin 12:4e9cadc225f5 32
choutin 12:4e9cadc225f5 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);
choutin 12:4e9cadc225f5 40
sakanakuuun 0:d7ff86f25eaa 41 }
choutin 12:4e9cadc225f5 42
sakanakuuun 0:d7ff86f25eaa 43 void move(int left,int right)
sakanakuuun 0:d7ff86f25eaa 44 {
choutin 12:4e9cadc225f5 45
sakanakuuun 0:d7ff86f25eaa 46 float rightduty,leftduty;
choutin 12:4e9cadc225f5 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 }
choutin 12:4e9cadc225f5 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 }
choutin 12:4e9cadc225f5 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 }
choutin 12:4e9cadc225f5 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) {
choutin 12:4e9cadc225f5 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) {
choutin 12:4e9cadc225f5 89 move(-hosei_, hosei_);
sakanakuuun 0:d7ff86f25eaa 90 } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
choutin 12:4e9cadc225f5 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 }
choutin 12:4e9cadc225f5 97
sakanakuuun 0:d7ff86f25eaa 98 }
choutin 12:4e9cadc225f5 99
choutin 12:4e9cadc225f5 100 void turnrad(float rad)
sakanakuuun 0:d7ff86f25eaa 101 {
sakanakuuun 7:7f1721542753 102 green = 1;
choutin 12:4e9cadc225f5 103
choutin 12:4e9cadc225f5 104 update_np();
choutin 12:4e9cadc225f5 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;
choutin 12:4e9cadc225f5 110
choutin 12:4e9cadc225f5 111 move((-np)*rightspeed, (np)*leftspeed);
choutin 12:4e9cadc225f5 112
choutin 12:4e9cadc225f5 113 while(1) {
choutin 12:4e9cadc225f5 114 update_np();
choutin 12:4e9cadc225f5 115 //pc.printf("t:%f\n\r", coordinateTheta());
choutin 12:4e9cadc225f5 116 if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
choutin 12:4e9cadc225f5 117 move(0,0);
choutin 12:4e9cadc225f5 118 break;
choutin 12:4e9cadc225f5 119 }
choutin 12:4e9cadc225f5 120 }
choutin 12:4e9cadc225f5 121
choutin 12:4e9cadc225f5 122
choutin 12:4e9cadc225f5 123
choutin 12:4e9cadc225f5 124 hosei_turn(0, false, rad);
choutin 12:4e9cadc225f5 125
choutin 12:4e9cadc225f5 126 wait(0.5);
choutin 12:4e9cadc225f5 127
choutin 12:4e9cadc225f5 128 hosei_turn(0, false, rad);
choutin 12:4e9cadc225f5 129
choutin 12:4e9cadc225f5 130 wait(0.5);
choutin 12:4e9cadc225f5 131 green = 0;
choutin 12:4e9cadc225f5 132
choutin 12:4e9cadc225f5 133 }
choutin 12:4e9cadc225f5 134
choutin 12:4e9cadc225f5 135
choutin 12:4e9cadc225f5 136 void turnrad_ccw(float rad)
choutin 12:4e9cadc225f5 137 {
choutin 12:4e9cadc225f5 138 green = 1;
choutin 12:4e9cadc225f5 139
choutin 12:4e9cadc225f5 140 update();
choutin 12:4e9cadc225f5 141
choutin 12:4e9cadc225f5 142 move(rightspeed, -leftspeed);
choutin 12:4e9cadc225f5 143
sakanakuuun 0:d7ff86f25eaa 144 while(1) {
sakanakuuun 0:d7ff86f25eaa 145 update();
sakanakuuun 2:f25a09c5e113 146 //pc.printf("t:%f\n\r", coordinateTheta());
sakanakuuun 2:f25a09c5e113 147 if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
sakanakuuun 0:d7ff86f25eaa 148 move(0,0);
sakanakuuun 0:d7ff86f25eaa 149 break;
sakanakuuun 0:d7ff86f25eaa 150 }
sakanakuuun 0:d7ff86f25eaa 151 }
choutin 12:4e9cadc225f5 152
choutin 12:4e9cadc225f5 153
choutin 12:4e9cadc225f5 154
sakanakuuun 2:f25a09c5e113 155 hosei_turn(0, false, rad);
choutin 12:4e9cadc225f5 156
sakanakuuun 2:f25a09c5e113 157 wait(0.5);
choutin 12:4e9cadc225f5 158
sakanakuuun 2:f25a09c5e113 159 hosei_turn(0, false, rad);
choutin 12:4e9cadc225f5 160
choutin 12:4e9cadc225f5 161
sakanakuuun 2:f25a09c5e113 162 wait(0.5);
sakanakuuun 7:7f1721542753 163 green = 0;
sakanakuuun 1:405e28b64fdb 164 }
choutin 12:4e9cadc225f5 165
choutin 12:4e9cadc225f5 166 void turnrad_cw(float rad)
choutin 12:4e9cadc225f5 167 {
choutin 12:4e9cadc225f5 168 green = 1;
choutin 12:4e9cadc225f5 169
choutin 12:4e9cadc225f5 170 update();
choutin 12:4e9cadc225f5 171
choutin 12:4e9cadc225f5 172 move((-1)*rightspeed, leftspeed);
choutin 12:4e9cadc225f5 173
choutin 12:4e9cadc225f5 174 while(1) {
choutin 12:4e9cadc225f5 175 update();
choutin 12:4e9cadc225f5 176 //pc.printf("t:%f\n\r", coordinateTheta());
choutin 12:4e9cadc225f5 177 if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
choutin 12:4e9cadc225f5 178 move(0,0);
choutin 12:4e9cadc225f5 179 break;
choutin 12:4e9cadc225f5 180 }
choutin 12:4e9cadc225f5 181 }
choutin 12:4e9cadc225f5 182
choutin 12:4e9cadc225f5 183
choutin 12:4e9cadc225f5 184
choutin 12:4e9cadc225f5 185 hosei_turn(0, false, rad);
choutin 12:4e9cadc225f5 186
choutin 12:4e9cadc225f5 187 wait(0.2);
choutin 12:4e9cadc225f5 188
choutin 12:4e9cadc225f5 189 hosei_turn(0, false, rad);
choutin 12:4e9cadc225f5 190
choutin 12:4e9cadc225f5 191 wait(0.2);
choutin 12:4e9cadc225f5 192 green = 0;
choutin 12:4e9cadc225f5 193 }
choutin 12:4e9cadc225f5 194
sakanakuuun 5:0e18cf25291a 195 void pmove(int x, int y)
sakanakuuun 7:7f1721542753 196 {
sakanakuuun 7:7f1721542753 197 yellow = 1;
choutin 12:4e9cadc225f5 198
choutin 12:4e9cadc225f5 199 float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
choutin 12:4e9cadc225f5 200 int k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
choutin 12:4e9cadc225f5 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;
choutin 12:4e9cadc225f5 207
sakanakuuun 7:7f1721542753 208 int direction;
choutin 12:4e9cadc225f5 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 }
choutin 12:4e9cadc225f5 223
sakanakuuun 7:7f1721542753 224 update();
sakanakuuun 7:7f1721542753 225 dx = x - coordinateX();
sakanakuuun 7:7f1721542753 226 dy = y - coordinateY();
choutin 12:4e9cadc225f5 227
sakanakuuun 7:7f1721542753 228 if(*d_length < 0) //x,y減少方向なら、*d_length<0
sakanakuuun 5:0e18cf25291a 229 direction *= -1;
choutin 12:4e9cadc225f5 230
sakanakuuun 7:7f1721542753 231 pc2.printf("direction:%d", direction);
choutin 12:4e9cadc225f5 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 }
choutin 12:4e9cadc225f5 251
choutin 12:4e9cadc225f5 252 ptheta += nearPi(coordinateTheta() - ptheta);
choutin 12:4e9cadc225f5 253
choutin 12:4e9cadc225f5 254 turnrad(ptheta);
choutin 12:4e9cadc225f5 255
sakanakuuun 2:f25a09c5e113 256 if(length == 0) return;
choutin 12:4e9cadc225f5 257
choutin 12:4e9cadc225f5 258 int i = 0;
choutin 12:4e9cadc225f5 259
sakanakuuun 1:405e28b64fdb 260 while(1) {
choutin 12:4e9cadc225f5 261 update_np();
sakanakuuun 5:0e18cf25291a 262 dx = x - coordinateX();
sakanakuuun 5:0e18cf25291a 263 dy = y - coordinateY();
sakanakuuun 2:f25a09c5e113 264 dtheta = coordinateTheta() - ptheta;
choutin 12:4e9cadc225f5 265
choutin 12:4e9cadc225f5 266 if(*disorder>max_disorder) {
choutin 12:4e9cadc225f5 267 *disorder = max_disorder;
choutin 12:4e9cadc225f5 268 } else if(*disorder<-max_disorder) {
choutin 12:4e9cadc225f5 269 *disorder = -max_disorder;
sakanakuuun 1:405e28b64fdb 270 }
choutin 12:4e9cadc225f5 271
sakanakuuun 5:0e18cf25291a 272 absd_length = abs(*d_length);
choutin 12:4e9cadc225f5 273
choutin 12:4e9cadc225f5 274
choutin 12:4e9cadc225f5 275 if(i++ < 5) {
choutin 12:4e9cadc225f5 276 daikei = i/5;
choutin 12:4e9cadc225f5 277 } else if(absd_length < 300) {
choutin 12:4e9cadc225f5 278 daikei = absd_length / 300.0;
choutin 12:4e9cadc225f5 279 }
choutin 12:4e9cadc225f5 280 /*
choutin 12:4e9cadc225f5 281 else if(absd_length > length - 30) {
choutin 12:4e9cadc225f5 282 daikei = abs(length - absd_length) / 30.0;
choutin 12:4e9cadc225f5 283 */
choutin 12:4e9cadc225f5 284 else
choutin 12:4e9cadc225f5 285 daikei = 1;
choutin 12:4e9cadc225f5 286
choutin 12:4e9cadc225f5 287 move(daikei * (rightspeed*(1-ratio) + k*(*disorder) - k_theta*dtheta) + rightspeed*ratio,
choutin 12:4e9cadc225f5 288 daikei * (leftspeed *(1-ratio) - k*(*disorder) + k_theta*dtheta) + leftspeed *ratio);
choutin 12:4e9cadc225f5 289
choutin 12:4e9cadc225f5 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);
choutin 12:4e9cadc225f5 291 if((direction > 0 && *d_length <= 0) || (direction < 0 && *d_length >= 0)) {
choutin 12:4e9cadc225f5 292 move(0, 0);
choutin 12:4e9cadc225f5 293 break;
choutin 12:4e9cadc225f5 294 }
choutin 12:4e9cadc225f5 295
choutin 12:4e9cadc225f5 296 }
choutin 12:4e9cadc225f5 297
choutin 12:4e9cadc225f5 298 wait(0.2);
choutin 12:4e9cadc225f5 299
choutin 12:4e9cadc225f5 300 yellow = 0;
choutin 12:4e9cadc225f5 301 }
choutin 12:4e9cadc225f5 302
choutin 12:4e9cadc225f5 303 void pmove2(int x, int y)
choutin 12:4e9cadc225f5 304 {
choutin 12:4e9cadc225f5 305 yellow = 1;
choutin 12:4e9cadc225f5 306 red=0;
choutin 12:4e9cadc225f5 307 float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
choutin 12:4e9cadc225f5 308 int k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
choutin 12:4e9cadc225f5 309
choutin 12:4e9cadc225f5 310 double length, d_length;
choutin 12:4e9cadc225f5 311 int disorder;
choutin 12:4e9cadc225f5 312 float dtheta, ptheta;
choutin 12:4e9cadc225f5 313 float daikei;
choutin 12:4e9cadc225f5 314
choutin 12:4e9cadc225f5 315
choutin 12:4e9cadc225f5 316 length = sqrt(pow(x - coordinateTheta(), 2) + pow(y - coordinateTheta(), 2));
choutin 12:4e9cadc225f5 317
choutin 12:4e9cadc225f5 318 pc2.printf("length:%f", length);
choutin 12:4e9cadc225f5 319
choutin 12:4e9cadc225f5 320 if(length == 0) {
choutin 12:4e9cadc225f5 321 red=1;
choutin 12:4e9cadc225f5 322 return;
choutin 12:4e9cadc225f5 323 }
choutin 12:4e9cadc225f5 324
choutin 12:4e9cadc225f5 325 ptheta = giveatan(x, y);
choutin 12:4e9cadc225f5 326
choutin 12:4e9cadc225f5 327 ptheta += nearPi(coordinateTheta() - ptheta);
choutin 12:4e9cadc225f5 328
choutin 12:4e9cadc225f5 329 turnrad(ptheta);
choutin 12:4e9cadc225f5 330
choutin 12:4e9cadc225f5 331 virtual_setup();
choutin 12:4e9cadc225f5 332
choutin 12:4e9cadc225f5 333 int i = 0;
choutin 12:4e9cadc225f5 334
choutin 12:4e9cadc225f5 335 while(1) {
choutin 12:4e9cadc225f5 336 update();
choutin 12:4e9cadc225f5 337 virtual_update();
choutin 12:4e9cadc225f5 338
choutin 12:4e9cadc225f5 339 d_length = length - virtual_coordinateX();
choutin 12:4e9cadc225f5 340 disorder = virtual_coordinateY();
choutin 12:4e9cadc225f5 341 dtheta = virtual_coordinateTheta();
choutin 12:4e9cadc225f5 342
choutin 12:4e9cadc225f5 343 if(disorder>max_disorder) {
choutin 12:4e9cadc225f5 344 disorder = max_disorder;
choutin 12:4e9cadc225f5 345 } else if(disorder<-max_disorder) {
choutin 12:4e9cadc225f5 346 disorder = -max_disorder;
choutin 12:4e9cadc225f5 347 }
choutin 12:4e9cadc225f5 348
choutin 12:4e9cadc225f5 349 if(i++ < 5) {
choutin 12:4e9cadc225f5 350 daikei = i/5;
choutin 12:4e9cadc225f5 351 } else if(d_length < 300) {
choutin 12:4e9cadc225f5 352 daikei = d_length / 300.0;
choutin 12:4e9cadc225f5 353 }
choutin 12:4e9cadc225f5 354 /*
choutin 12:4e9cadc225f5 355 else if(absd_length > length - 30) {
choutin 12:4e9cadc225f5 356 daikei = abs(length - absd_length) / 30.0;
choutin 12:4e9cadc225f5 357 */
choutin 12:4e9cadc225f5 358 else
sakanakuuun 4:4c574be6325c 359 daikei = 1;
choutin 12:4e9cadc225f5 360
choutin 12:4e9cadc225f5 361 move(daikei * (rightspeed*(1-ratio) + k*disorder - k_theta*dtheta) + rightspeed*ratio,
choutin 12:4e9cadc225f5 362 daikei * (leftspeed *(1-ratio) - k*disorder + k_theta*dtheta) + leftspeed *ratio);
choutin 12:4e9cadc225f5 363
choutin 12:4e9cadc225f5 364 pc2.printf("length:%f, d_length:%f, vx:%d, vy:%d", length, d_length, virtual_coordinateX(), virtual_coordinateY());
choutin 12:4e9cadc225f5 365
choutin 12:4e9cadc225f5 366 if(d_length <= 0) {
choutin 12:4e9cadc225f5 367 move(0, 0);
choutin 12:4e9cadc225f5 368 break;
choutin 12:4e9cadc225f5 369 }
choutin 12:4e9cadc225f5 370
choutin 12:4e9cadc225f5 371 }
choutin 12:4e9cadc225f5 372
choutin 12:4e9cadc225f5 373 wait(0.2);
choutin 12:4e9cadc225f5 374
choutin 12:4e9cadc225f5 375 yellow = 0;
choutin 12:4e9cadc225f5 376 red = 0;
choutin 12:4e9cadc225f5 377 }
choutin 12:4e9cadc225f5 378
choutin 12:4e9cadc225f5 379 void pmove3(int x, int y)
choutin 12:4e9cadc225f5 380 {
choutin 12:4e9cadc225f5 381 yellow = 1;
choutin 12:4e9cadc225f5 382
choutin 12:4e9cadc225f5 383 float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
choutin 12:4e9cadc225f5 384 int k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
choutin 12:4e9cadc225f5 385
choutin 12:4e9cadc225f5 386 int length, dx, dy;
choutin 12:4e9cadc225f5 387 int *d_length, *disorder;
choutin 12:4e9cadc225f5 388 int absd_length;
choutin 12:4e9cadc225f5 389 float dtheta, ptheta;
choutin 12:4e9cadc225f5 390 float daikei;
choutin 12:4e9cadc225f5 391
choutin 12:4e9cadc225f5 392 int direction;
choutin 12:4e9cadc225f5 393
choutin 12:4e9cadc225f5 394 if(abs(x - coordinateX()) > abs(y - coordinateY())) {
choutin 12:4e9cadc225f5 395 direction = X_PLUS;
choutin 12:4e9cadc225f5 396 length = abs(x - coordinateX());
choutin 12:4e9cadc225f5 397 d_length = &dx;
choutin 12:4e9cadc225f5 398 disorder = &dy;
choutin 12:4e9cadc225f5 399 } else {
choutin 12:4e9cadc225f5 400 direction = Y_PLUS;
choutin 12:4e9cadc225f5 401 length = abs(y - coordinateY());
choutin 12:4e9cadc225f5 402 d_length = &dy;
choutin 12:4e9cadc225f5 403 disorder = &dx;
choutin 12:4e9cadc225f5 404 }
choutin 12:4e9cadc225f5 405
choutin 12:4e9cadc225f5 406 update();
choutin 12:4e9cadc225f5 407 dx = x - coordinateX();
choutin 12:4e9cadc225f5 408 dy = y - coordinateY();
choutin 12:4e9cadc225f5 409
choutin 12:4e9cadc225f5 410 if(*d_length < 0) //x,y減少方向なら、*d_length<0
choutin 12:4e9cadc225f5 411 direction *= -1;
choutin 12:4e9cadc225f5 412
choutin 12:4e9cadc225f5 413 pc2.printf("direction:%d", direction);
choutin 12:4e9cadc225f5 414
choutin 12:4e9cadc225f5 415 switch(direction) {
choutin 12:4e9cadc225f5 416 case X_PLUS:
choutin 12:4e9cadc225f5 417 ptheta = 0;
choutin 12:4e9cadc225f5 418 break;
choutin 12:4e9cadc225f5 419 case Y_PLUS:
choutin 12:4e9cadc225f5 420 k *= -1;
choutin 12:4e9cadc225f5 421 ptheta = PI/2;
choutin 12:4e9cadc225f5 422 break;
choutin 12:4e9cadc225f5 423 case X_MINUS:
choutin 12:4e9cadc225f5 424 k *= -1;
choutin 12:4e9cadc225f5 425 ptheta = PI;
choutin 12:4e9cadc225f5 426 break;
choutin 12:4e9cadc225f5 427 case Y_MINUS:
choutin 12:4e9cadc225f5 428 ptheta = -PI/2;
choutin 12:4e9cadc225f5 429 break;
choutin 12:4e9cadc225f5 430 default:
choutin 12:4e9cadc225f5 431 return;
choutin 12:4e9cadc225f5 432 }
choutin 12:4e9cadc225f5 433
choutin 12:4e9cadc225f5 434 ptheta += nearPi(coordinateTheta() - ptheta);
choutin 12:4e9cadc225f5 435
choutin 12:4e9cadc225f5 436 turnrad(ptheta);
choutin 12:4e9cadc225f5 437
choutin 12:4e9cadc225f5 438 if(length == 0) return;
choutin 12:4e9cadc225f5 439
choutin 12:4e9cadc225f5 440 int i = 0;
choutin 12:4e9cadc225f5 441
choutin 12:4e9cadc225f5 442 while(1) {
choutin 12:4e9cadc225f5 443 update_np();
choutin 12:4e9cadc225f5 444 dx = x - coordinateX();
choutin 12:4e9cadc225f5 445 dy = y - coordinateY();
choutin 12:4e9cadc225f5 446 dtheta = coordinateTheta() - ptheta;
choutin 12:4e9cadc225f5 447
choutin 12:4e9cadc225f5 448 if(*disorder>max_disorder) {
choutin 12:4e9cadc225f5 449 *disorder = max_disorder;
choutin 12:4e9cadc225f5 450 } else if(*disorder<-max_disorder) {
choutin 12:4e9cadc225f5 451 *disorder = -max_disorder;
choutin 12:4e9cadc225f5 452 }
choutin 12:4e9cadc225f5 453
choutin 12:4e9cadc225f5 454 absd_length = abs(*d_length);
choutin 12:4e9cadc225f5 455
choutin 12:4e9cadc225f5 456
choutin 12:4e9cadc225f5 457 if(i++ < 5) {
choutin 12:4e9cadc225f5 458 daikei = i/5;
choutin 12:4e9cadc225f5 459 } else if(absd_length < 300) {
choutin 12:4e9cadc225f5 460 daikei = absd_length / 300.0;
choutin 12:4e9cadc225f5 461 }
choutin 12:4e9cadc225f5 462 /*
choutin 12:4e9cadc225f5 463 else if(absd_length > length - 30) {
choutin 12:4e9cadc225f5 464 daikei = abs(length - absd_length) / 30.0;
choutin 12:4e9cadc225f5 465 */
choutin 12:4e9cadc225f5 466 else
choutin 12:4e9cadc225f5 467 daikei = 1;
choutin 12:4e9cadc225f5 468
choutin 12:4e9cadc225f5 469 move(daikei * (rightspeed*(1-ratio) + k*(*disorder) - k_theta*dtheta) + rightspeed*ratio,
choutin 12:4e9cadc225f5 470 daikei * (leftspeed *(1-ratio) - k*(*disorder) + k_theta*dtheta) + leftspeed *ratio);
choutin 12:4e9cadc225f5 471
choutin 9:7e99a1c80656 472 //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 473 if((direction > 0 && *d_length <= 0) || (direction < 0 && *d_length >= 0)) {
sakanakuuun 2:f25a09c5e113 474 move(0, 0);
sakanakuuun 1:405e28b64fdb 475 break;
sakanakuuun 1:405e28b64fdb 476 }
choutin 12:4e9cadc225f5 477
sakanakuuun 0:d7ff86f25eaa 478 }
choutin 12:4e9cadc225f5 479
choutin 12:4e9cadc225f5 480 wait(0.2);
choutin 12:4e9cadc225f5 481
sakanakuuun 7:7f1721542753 482 yellow = 0;
sakanakuuun 0:d7ff86f25eaa 483 }
choutin 12:4e9cadc225f5 484
choutin 12:4e9cadc225f5 485 float giveatan(int targetx,int targety)
choutin 12:4e9cadc225f5 486 {
choutin 12:4e9cadc225f5 487 int x,y;
choutin 12:4e9cadc225f5 488 float theta;
choutin 12:4e9cadc225f5 489 float phi;
choutin 12:4e9cadc225f5 490 update();
choutin 12:4e9cadc225f5 491 x = coordinateX();
choutin 12:4e9cadc225f5 492 y = coordinateY();
choutin 12:4e9cadc225f5 493 theta = coordinateTheta();//自己位置取得
choutin 12:4e9cadc225f5 494
choutin 12:4e9cadc225f5 495 if(targetx - x == 0) return 0;
choutin 12:4e9cadc225f5 496
choutin 12:4e9cadc225f5 497 phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得
choutin 12:4e9cadc225f5 498 if(targetx - x < 0)
choutin 12:4e9cadc225f5 499 {
choutin 12:4e9cadc225f5 500 if(targety - y > 0)
choutin 12:4e9cadc225f5 501 phi += PI;
choutin 12:4e9cadc225f5 502 else if(targety - y < 0)
choutin 12:4e9cadc225f5 503 phi -= PI;
choutin 12:4e9cadc225f5 504 }
choutin 12:4e9cadc225f5 505
choutin 12:4e9cadc225f5 506 return phi;
choutin 12:4e9cadc225f5 507 }
choutin 12:4e9cadc225f5 508
sakanakuuun 6:0aa97a99c9cb 509 void back300()
sakanakuuun 7:7f1721542753 510 {
sakanakuuun 7:7f1721542753 511 red = 1;
choutin 12:4e9cadc225f5 512
sakanakuuun 4:4c574be6325c 513 float k = 0.9;
sakanakuuun 4:4c574be6325c 514 int k_theta = 2;
choutin 12:4e9cadc225f5 515
sakanakuuun 6:0aa97a99c9cb 516 int length, px, py, dx, dy;
sakanakuuun 4:4c574be6325c 517 float daikei;
choutin 12:4e9cadc225f5 518
sakanakuuun 6:0aa97a99c9cb 519 update();
choutin 12:4e9cadc225f5 520
sakanakuuun 7:7f1721542753 521 px = coordinateX();
sakanakuuun 7:7f1721542753 522 py = coordinateY();
choutin 12:4e9cadc225f5 523
sakanakuuun 6:0aa97a99c9cb 524 length = 300;
choutin 12:4e9cadc225f5 525
choutin 12:4e9cadc225f5 526 turnrad(PI + nearPi(coordinateTheta() - PI));
choutin 12:4e9cadc225f5 527
sakanakuuun 4:4c574be6325c 528 while(1) {
choutin 12:4e9cadc225f5 529 update_np();
sakanakuuun 6:0aa97a99c9cb 530 dx = coordinateX() - px;
sakanakuuun 6:0aa97a99c9cb 531 dy = coordinateY() - py;
choutin 12:4e9cadc225f5 532
choutin 12:4e9cadc225f5 533 if(dy>max_disorder) {
choutin 12:4e9cadc225f5 534 dy = max_disorder;
choutin 12:4e9cadc225f5 535 } else if(dy<-max_disorder) {
choutin 12:4e9cadc225f5 536 dy = -max_disorder;
sakanakuuun 6:0aa97a99c9cb 537 }
choutin 12:4e9cadc225f5 538
choutin 12:4e9cadc225f5 539
sakanakuuun 8:3bf4addaaedd 540 move(-(30 + k*dy), -(32 - k*dy));
choutin 12:4e9cadc225f5 541
choutin 12:4e9cadc225f5 542
sakanakuuun 6:0aa97a99c9cb 543 if(dx>length) {
sakanakuuun 4:4c574be6325c 544 move(0, 0);
sakanakuuun 4:4c574be6325c 545 break;
sakanakuuun 4:4c574be6325c 546 }
choutin 12:4e9cadc225f5 547
choutin 12:4e9cadc225f5 548 //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
choutin 12:4e9cadc225f5 549 }
choutin 12:4e9cadc225f5 550
choutin 12:4e9cadc225f5 551 wait(0.2);
choutin 12:4e9cadc225f5 552
choutin 12:4e9cadc225f5 553 red = 0;
choutin 12:4e9cadc225f5 554 }
choutin 12:4e9cadc225f5 555
choutin 12:4e9cadc225f5 556
choutin 12:4e9cadc225f5 557 void nxback300()
choutin 12:4e9cadc225f5 558 {
choutin 12:4e9cadc225f5 559 red = 1;
choutin 12:4e9cadc225f5 560
choutin 12:4e9cadc225f5 561 float k = 0.9;
choutin 12:4e9cadc225f5 562 int k_theta = 2;
choutin 12:4e9cadc225f5 563
choutin 12:4e9cadc225f5 564 int length, px, py, dx, dy;
choutin 12:4e9cadc225f5 565 float daikei;
choutin 12:4e9cadc225f5 566
choutin 12:4e9cadc225f5 567 update();
choutin 12:4e9cadc225f5 568
choutin 12:4e9cadc225f5 569 px = coordinateX();
choutin 12:4e9cadc225f5 570 py = coordinateY();
choutin 12:4e9cadc225f5 571
choutin 12:4e9cadc225f5 572 length = 300;
choutin 12:4e9cadc225f5 573
choutin 12:4e9cadc225f5 574 turnrad(nearPi(coordinateTheta()));
choutin 12:4e9cadc225f5 575
choutin 12:4e9cadc225f5 576 while(1) {
choutin 12:4e9cadc225f5 577 update_np();
choutin 12:4e9cadc225f5 578 dx = coordinateX() - px;
choutin 12:4e9cadc225f5 579 dy = coordinateY() - py;
choutin 12:4e9cadc225f5 580
choutin 12:4e9cadc225f5 581 if(dy>max_disorder) {
choutin 12:4e9cadc225f5 582 dy = max_disorder;
choutin 12:4e9cadc225f5 583 } else if(dy<-max_disorder) {
choutin 12:4e9cadc225f5 584 dy = -max_disorder;
choutin 12:4e9cadc225f5 585 }
choutin 12:4e9cadc225f5 586
choutin 12:4e9cadc225f5 587
choutin 12:4e9cadc225f5 588 move(-(30 - k*dy), -(32 + k*dy));
choutin 12:4e9cadc225f5 589
choutin 12:4e9cadc225f5 590
choutin 12:4e9cadc225f5 591 if(abs(dx)>length) {
choutin 12:4e9cadc225f5 592 move(0, 0);
choutin 12:4e9cadc225f5 593 break;
choutin 12:4e9cadc225f5 594 }
choutin 12:4e9cadc225f5 595
sakanakuuun 4:4c574be6325c 596 //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
sakanakuuun 4:4c574be6325c 597 }
choutin 12:4e9cadc225f5 598
choutin 12:4e9cadc225f5 599 wait(0.2);
choutin 12:4e9cadc225f5 600
sakanakuuun 7:7f1721542753 601 red = 0;
sakanakuuun 4:4c574be6325c 602 }
choutin 12:4e9cadc225f5 603
choutin 12:4e9cadc225f5 604
choutin 12:4e9cadc225f5 605 void pyback300()
choutin 12:4e9cadc225f5 606 {
choutin 12:4e9cadc225f5 607 red = 1;
choutin 12:4e9cadc225f5 608
choutin 12:4e9cadc225f5 609 float k = 0.9;
choutin 12:4e9cadc225f5 610 int k_theta = 2;
choutin 12:4e9cadc225f5 611
choutin 12:4e9cadc225f5 612 int length, px, py, dx, dy;
choutin 12:4e9cadc225f5 613 float daikei;
choutin 12:4e9cadc225f5 614
choutin 12:4e9cadc225f5 615 update();
choutin 12:4e9cadc225f5 616
choutin 12:4e9cadc225f5 617 px = coordinateX();
choutin 12:4e9cadc225f5 618 py = coordinateY();
choutin 12:4e9cadc225f5 619
choutin 12:4e9cadc225f5 620 length = 300;
choutin 12:4e9cadc225f5 621
choutin 12:4e9cadc225f5 622 turnrad(PI/2 + nearPi(coordinateTheta() - PI/2));
choutin 12:4e9cadc225f5 623
choutin 12:4e9cadc225f5 624 while(1) {
choutin 12:4e9cadc225f5 625 update_np();
choutin 12:4e9cadc225f5 626 dx = coordinateX() - px;
choutin 12:4e9cadc225f5 627 dy = coordinateY() - py;
choutin 12:4e9cadc225f5 628
choutin 12:4e9cadc225f5 629 if(dx>max_disorder) {
choutin 12:4e9cadc225f5 630 dx = max_disorder;
choutin 12:4e9cadc225f5 631 } else if(dx<-max_disorder) {
choutin 12:4e9cadc225f5 632 dx = -max_disorder;
choutin 12:4e9cadc225f5 633 }
choutin 12:4e9cadc225f5 634
choutin 12:4e9cadc225f5 635
choutin 12:4e9cadc225f5 636 move(-(30 - k*dx), -(32 + k*dx));
choutin 12:4e9cadc225f5 637
choutin 12:4e9cadc225f5 638
choutin 12:4e9cadc225f5 639 if(dy>length) {
choutin 12:4e9cadc225f5 640 move(0, 0);
choutin 12:4e9cadc225f5 641 break;
choutin 12:4e9cadc225f5 642 }
choutin 12:4e9cadc225f5 643
choutin 12:4e9cadc225f5 644 //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
choutin 12:4e9cadc225f5 645 }
choutin 12:4e9cadc225f5 646
choutin 12:4e9cadc225f5 647 wait(0.2);
choutin 12:4e9cadc225f5 648
choutin 12:4e9cadc225f5 649 red
choutin 12:4e9cadc225f5 650
choutin 12:4e9cadc225f5 651 = 0;
choutin 12:4e9cadc225f5 652 }
choutin 12:4e9cadc225f5 653
choutin 12:4e9cadc225f5 654 void nyback300()
choutin 12:4e9cadc225f5 655 {
choutin 12:4e9cadc225f5 656 {
choutin 12:4e9cadc225f5 657 red = 1;
choutin 12:4e9cadc225f5 658
choutin 12:4e9cadc225f5 659 float k = 0.9;
choutin 12:4e9cadc225f5 660 int k_theta = 2;
choutin 12:4e9cadc225f5 661
choutin 12:4e9cadc225f5 662 int length, px, py, dx, dy;
choutin 12:4e9cadc225f5 663 float daikei;
choutin 12:4e9cadc225f5 664
choutin 12:4e9cadc225f5 665 update();
choutin 12:4e9cadc225f5 666
choutin 12:4e9cadc225f5 667 px = coordinateX();
choutin 12:4e9cadc225f5 668 py = coordinateY();
choutin 12:4e9cadc225f5 669
choutin 12:4e9cadc225f5 670 length = 300;
choutin 12:4e9cadc225f5 671
choutin 12:4e9cadc225f5 672 turnrad(-PI/2 + nearPi(coordinateTheta() + PI/2));
choutin 12:4e9cadc225f5 673
choutin 12:4e9cadc225f5 674 while(1) {
choutin 12:4e9cadc225f5 675 update_np();
choutin 12:4e9cadc225f5 676 dx = coordinateX() - px;
choutin 12:4e9cadc225f5 677 dy = coordinateY() - py;
choutin 12:4e9cadc225f5 678
choutin 12:4e9cadc225f5 679 if(dx>max_disorder) {
choutin 12:4e9cadc225f5 680 dx = max_disorder;
choutin 12:4e9cadc225f5 681 } else if(dx<-max_disorder) {
choutin 12:4e9cadc225f5 682 dx = -max_disorder;
choutin 12:4e9cadc225f5 683 }
choutin 12:4e9cadc225f5 684
choutin 12:4e9cadc225f5 685
choutin 12:4e9cadc225f5 686 move(-(30 - k*dx), -(32 + k*dx));
choutin 12:4e9cadc225f5 687
choutin 12:4e9cadc225f5 688
choutin 12:4e9cadc225f5 689 if(abs(dy)>length) {
choutin 12:4e9cadc225f5 690 move(0, 0);
choutin 12:4e9cadc225f5 691 break;
choutin 12:4e9cadc225f5 692 }
choutin 12:4e9cadc225f5 693
choutin 12:4e9cadc225f5 694 //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
choutin 12:4e9cadc225f5 695 }
choutin 12:4e9cadc225f5 696
choutin 12:4e9cadc225f5 697 wait(0.2);
choutin 12:4e9cadc225f5 698
choutin 12:4e9cadc225f5 699 red = 0;
choutin 12:4e9cadc225f5 700 }
choutin 12:4e9cadc225f5 701 }
choutin 12:4e9cadc225f5 702
choutin 12:4e9cadc225f5 703 float nearPi(float rad)
choutin 12:4e9cadc225f5 704 {
choutin 12:4e9cadc225f5 705 float npi = 0;
choutin 12:4e9cadc225f5 706
choutin 12:4e9cadc225f5 707 while(1) {
choutin 12:4e9cadc225f5 708 if(rad > npi + PI)
choutin 12:4e9cadc225f5 709 npi += 2*PI;
choutin 12:4e9cadc225f5 710 else if(rad < npi - PI)
choutin 12:4e9cadc225f5 711 npi -= 2*PI;
choutin 12:4e9cadc225f5 712 else
choutin 12:4e9cadc225f5 713 return npi;
choutin 12:4e9cadc225f5 714 }
choutin 12:4e9cadc225f5 715 }
choutin 12:4e9cadc225f5 716