Tk A
aaa
Diff: move.cpp
- Revision:
- 18:4c812a3c7411
- Parent:
- 17:c167f4ed9070
- Child:
- 19:0b0ed6b22de9
--- a/move.cpp Sat Sep 10 06:51:19 2016 +0000
+++ b/move.cpp Sat Sep 10 12:14:26 2016 +0000
@@ -2,6 +2,7 @@
#include "move.h"
#include "locate.h"
#include "stdlib.h"
+#include "math.h"
PwmOut M1cw(PA_11);
PwmOut M1ccw(PB_15);
@@ -183,11 +184,11 @@
hosei_turn(0, false, rad);
- wait(0.5);
+ wait(0.2);
hosei_turn(0, false, rad);
- wait(0.5);
+ wait(0.2);
green = 0;
}
@@ -205,7 +206,7 @@
float daikei;
int direction;
-
+
if(abs(x - coordinateX()) > abs(y - coordinateY())) {
y = coordinateY();
direction = X_PLUS;
@@ -219,11 +220,11 @@
d_length = &dy;
disorder = &dx;
}
-
+
update();
dx = x - coordinateX();
dy = y - coordinateY();
-
+
if(*d_length < 0) //x,y減少方向なら、*d_length<0
direction *= -1;
@@ -247,15 +248,15 @@
default:
return;
}
-
+
ptheta += nearPi(coordinateTheta() - ptheta);
-
+
turnrad(ptheta);
if(length == 0) return;
-
+
int i = 0;
-
+
while(1) {
update_np();
dx = x - coordinateX();
@@ -269,18 +270,17 @@
}
absd_length = abs(*d_length);
-
-
+
+
if(i++ < 5) {
daikei = i/5;
- }
- else if(absd_length < 300) {
+ } else if(absd_length < 300) {
daikei = absd_length / 300.0;
- }
-/*
- else if(absd_length > length - 30) {
- daikei = abs(length - absd_length) / 30.0;
-*/
+ }
+ /*
+ else if(absd_length > length - 30) {
+ daikei = abs(length - absd_length) / 30.0;
+ */
else
daikei = 1;
@@ -295,11 +295,209 @@
}
- wait(0.5);
+ wait(0.2);
yellow = 0;
}
+void pmove2(int x, int y)
+{
+ yellow = 1;
+ red=0;
+ float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
+ int k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
+
+ double length;
+ int d_length, disorder;
+ float dtheta, ptheta;
+ float daikei;
+
+
+ length = sqrt((double)x*x + (double)y*y);
+
+ pc2.printf("length:%f", length);
+
+ if(length == 0) {
+ red=1;
+ return;
+ }
+
+ ptheta = giveatan(x, y);
+
+ turnrad(ptheta);
+
+ virtual_setup();
+
+ int i = 0;
+
+ while(1) {
+ update_np();
+ virtual_update();
+
+ d_length = length - virtual_coordinateX();
+ disorder = virtual_coordinateY();
+ dtheta = virtual_coordinateTheta();
+
+ if(disorder>max_disorder) {
+ disorder = max_disorder;
+ } else if(disorder<-max_disorder) {
+ disorder = -max_disorder;
+ }
+
+ if(i++ < 5) {
+ 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
+ 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);
+
+
+ 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)) {
+ move(0, 0);
+ break;
+ }
+
+ }
+
+ wait(0.2);
+
+ yellow = 0;
+}
+
+float giveatan(int targetx,int targety)
+{
+ int x,y;
+ float theta;
+ float phi;
+ update();
+ x = coordinateX();
+ y = coordinateY();
+ theta = coordinateTheta();//自己位置取得
+ phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得
+ while(phi>PI) {
+ phi-=2*PI;
+ }
+ while(phi<=(-1)*PI) {
+ phi+=2*PI;
+ }
+
+ return phi;
+}
+
void back300()
{
red = 1;
@@ -342,7 +540,7 @@
//pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
}
- wait(0.5);
+ wait(0.2);
red = 0;
}
@@ -390,7 +588,7 @@
//pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
}
- wait(0.5);
+ wait(0.2);
red = 0;
}
@@ -413,7 +611,7 @@
length = 300;
- turnrad(-PI/2 + nearPi(coordinateTheta() + PI/2));
+ turnrad(PI/2 + nearPi(coordinateTheta() - PI/2));
while(1) {
update_np();
@@ -438,18 +636,67 @@
//pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
}
- wait(0.5);
+ wait(0.2);
- red = 0;
+ red
+
+ = 0;
}
+void nyback300()
+{
+ {
+ red = 1;
+
+ float k = 0.9;
+ int k_theta = 2;
+
+ int length, px, py, dx, dy;
+ float daikei;
+
+ update();
+
+ px = coordinateX();
+ py = coordinateY();
+
+ length = 300;
+
+ turnrad(-PI/2 + nearPi(coordinateTheta() + PI/2));
+
+ 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;
+ }
+
+
+ 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);
+ }
+
+ wait(0.2);
+
+ red = 0;
+ }
+}
float nearPi(float rad)
{
float npi = 0;
-
- while(1)
- {
+
+ while(1) {
if(rad > npi + PI)
npi += 2*PI;
else if(rad < npi - PI)