Tk A
aaa
Diff: move.cpp
- Revision:
- 22:eed10c0b7fe0
- Parent:
- 21:282c2b61e462
- Child:
- 23:e30ffaeb3e0f
--- a/move.cpp Sat Sep 10 13:07:50 2016 +0000
+++ b/move.cpp Sat Sep 10 17:04:45 2016 +0000
@@ -26,8 +26,8 @@
const int rightspeed=70;
const int leftspeed=rightspeed + 4;
const int hosei_ = 13;
-const int max_disorder = 4;
-const float ratio = 1.0/7.5;
+const int max_disorder = 3;
+const float ratio = 1.0/8.5;
//const float PIfact=2.89;
@@ -313,19 +313,19 @@
float daikei;
- length = sqrt(pow(x - coordinateTheta(), 2) + pow(y - coordinateTheta(), 2));
-
+ length = sqrt(pow((double)(x - coordinateX()), 2) + pow((double)(y - coordinateY()), 2));
+
pc2.printf("length:%f", length);
-
+
if(length == 0) {
red=1;
return;
}
ptheta = giveatan(x, y);
-
+
ptheta += nearPi(coordinateTheta() - ptheta);
-
+
turnrad(ptheta);
virtual_setup();
@@ -350,125 +350,15 @@
daikei = i/5;
} else if(d_length < 300) {
daikei = d_length / 300.0;
- }
- /*
- else if(absd_length > length - 30) {
- daikei = abs(length - absd_length) / 30.0;
- */
- else
+ } else
daikei = 1;
- move(daikei * (rightspeed*(1-ratio) + k*disorder - k_theta*dtheta) + rightspeed*ratio,
- daikei * (leftspeed *(1-ratio) - k*disorder + k_theta*dtheta) + leftspeed *ratio);
-
- if(d_length <= 0) {
- move(0, 0);
- break;
- }
-
- }
-
- wait(0.2);
-
- yellow = 0;
- red = 0;
-}
-
-void pmove3(int x, int y)
-{
- yellow = 1;
-
- float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
- int k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
-
- int length, dx, dy;
- int *d_length, *disorder;
- int absd_length;
- float dtheta, ptheta;
- float daikei;
-
- int direction;
-
- if(abs(x - coordinateX()) > abs(y - coordinateY())) {
- direction = X_PLUS;
- length = abs(x - coordinateX());
- d_length = &dx;
- disorder = &dy;
- } else {
- direction = Y_PLUS;
- length = abs(y - coordinateY());
- d_length = &dy;
- disorder = &dx;
- }
-
- update();
- dx = x - coordinateX();
- dy = y - coordinateY();
-
- if(*d_length < 0) //x,y減少方向なら、*d_length<0
- direction *= -1;
-
- pc2.printf("direction:%d", direction);
-
- switch(direction) {
- case X_PLUS:
- ptheta = 0;
- break;
- case Y_PLUS:
- k *= -1;
- ptheta = PI/2;
- break;
- case X_MINUS:
- k *= -1;
- ptheta = PI;
- break;
- case Y_MINUS:
- ptheta = -PI/2;
- break;
- default:
- return;
- }
-
- ptheta += nearPi(coordinateTheta() - ptheta);
-
- turnrad(ptheta);
-
- if(length == 0) return;
-
- int i = 0;
-
- while(1) {
- update_np();
- dx = x - coordinateX();
- dy = y - coordinateY();
- dtheta = coordinateTheta() - ptheta;
-
- if(*disorder>max_disorder) {
- *disorder = max_disorder;
- } else if(*disorder<-max_disorder) {
- *disorder = -max_disorder;
- }
-
- absd_length = abs(*d_length);
+ move(daikei * (rightspeed*(1-ratio) - k*disorder - k_theta*dtheta) + rightspeed*ratio,
+ daikei * (leftspeed *(1-ratio) + k*disorder + k_theta*dtheta) + leftspeed *ratio);
- if(i++ < 5) {
- daikei = i/5;
- } else if(absd_length < 300) {
- daikei = absd_length / 300.0;
- }
- /*
- else if(absd_length > length - 30) {
- daikei = abs(length - absd_length) / 30.0;
- */
- else
- daikei = 1;
-
- move(daikei * (rightspeed*(1-ratio) + k*(*disorder) - k_theta*dtheta) + rightspeed*ratio,
- daikei * (leftspeed *(1-ratio) - k*(*disorder) + k_theta*dtheta) + leftspeed *ratio);
-
- //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);
- if((direction > 0 && *d_length <= 0) || (direction < 0 && *d_length >= 0)) {
+ pc2.printf("length:%f, d_length:%d, vx:%d, vy:%d\n\r", length, d_length, virtual_coordinateX(), virtual_coordinateY());
+ if(d_length <= 0) {
move(0, 0);
break;
}
@@ -478,6 +368,7 @@
wait(0.2);
yellow = 0;
+ red = 0;
}
float giveatan(int targetx,int targety)
@@ -489,18 +380,22 @@
x = coordinateX();
y = coordinateY();
theta = coordinateTheta();//自己位置取得
-
- if(targetx - x == 0) return 0;
-
+
+ if(targetx - x == 0) {
+ if(targety > 0)
+ return PI/2;
+ else
+ return -PI/2;
+ }
+
phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得
- if(targetx - x < 0)
- {
+ if(targetx - x < 0) {
if(targety - y > 0)
+ phi += PI;
+ else if(targety - y < 0)
phi -= PI;
- else if(targety - y < 0)
- phi += PI;
}
-
+
return phi;
}
@@ -552,7 +447,7 @@
}
-void nxback300()
+void nxback300(int team)
{
red = 1;
@@ -600,8 +495,13 @@
}
-void pyback300()
+void pyback300(int team)
{
+ if(team == 0) {
+ nyback300(1);
+ return;
+ }
+
red = 1;
float k = 0.9;
@@ -645,57 +545,62 @@
wait(0.2);
red
-
- = 0;
+
+ = 0;
}
-void nyback300()
+void nyback300(int team)
{
+ if(team == 0)
{
- red = 1;
+ pyback300(1);
+ return;
+ }
+
+ red = 1;
- float k = 0.9;
- int k_theta = 2;
+ float k = 0.9;
+ int k_theta = 2;
- int length, px, py, dx, dy;
- float daikei;
+ int length, px, py, dx, dy;
+ float daikei;
- update();
+ update();
- px = coordinateX();
- py = coordinateY();
+ px = coordinateX();
+ py = coordinateY();
- length = 300;
+ length = 300;
- turnrad(-PI/2 + nearPi(coordinateTheta() + PI/2));
+ turnrad(-PI/2 + nearPi(coordinateTheta() + PI/2));
- while(1) {
- update_np();
- dx = coordinateX() - px;
- dy = coordinateY() - py;
+ while(1) {
+ update_np();
+ dx = coordinateX() - px;
+ dy = coordinateY() - py;
- if(dx>max_disorder) {
- dx = max_disorder;
- } else if(dx<-max_disorder) {
- dx = -max_disorder;
- }
+ if(dx>max_disorder) {
+ dx = max_disorder;
+ } else if(dx<-max_disorder) {
+ dx = -max_disorder;
+ }
- move(-(30 - k*dx), -(32 + k*dx));
+ move(-(30 - k*dx), -(32 + k*dx));
- if(abs(dy)>length) {
- move(0, 0);
- break;
- }
-
- //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
+ if(abs(dy)>length) {
+ move(0, 0);
+ break;
}
- wait(0.2);
+ //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
+ }
- red = 0;
- }
+ wait(0.2);
+
+ red = 0;
+
}
float nearPi(float rad)