Tk A
aaa
Diff: move.cpp
- Revision:
- 1:405e28b64fdb
- Parent:
- 0:d7ff86f25eaa
- Child:
- 2:f25a09c5e113
--- a/move.cpp Fri Sep 02 12:40:45 2016 +0000
+++ b/move.cpp Sat Sep 03 10:53:20 2016 +0000
@@ -12,7 +12,9 @@
const int rightspeed=29*2;
const int leftspeed=31*2;
const int turnspeed=15*2;
-
+const float k_x = 0.9;
+const float k_y = 0.9;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+const int k_theta = 500;
//const float PIfact=2.89;
@@ -86,15 +88,13 @@
void turncw()
{
- float pt;
- move((-1)*turnspeed,turnspeed);
-
update();
- pt=coordinateTheta();
+ float pt = coordinateTheta();;
+ move(-turnspeed, turnspeed);
while(1) {
update();
- if(pt-coordinateTheta()>PI/2) {
+ if(coordinateTheta() - pt > -PI/2) {
move(0,0);
break;
}
@@ -103,15 +103,13 @@
void turnccw()
{
- float pt;
- move(turnspeed,(-1)*turnspeed);
-
update();
- pt=coordinateTheta();
+ float pt = coordinateTheta();;
+ move(turnspeed, -turnspeed);
while(1) {
update();
- if(pt-coordinateTheta()<(-1)*PI/2) {
+ if(coordinateTheta() - pt < PI/2) {
move(0,0);
break;
}
@@ -120,40 +118,36 @@
void turn_cw()
{
- float pt;
- move((-1)*turnspeed,turnspeed);
-
update();
- pt=coordinateTheta();
+ float pt = coordinateTheta();;
+ move(-turnspeed, turnspeed);
while(1) {
update();
- if(pt-coordinateTheta()>PI/2) {
+ if(coordinateTheta() - pt < -PI/2) {
move(0,0);
break;
}
}
- hosei_turn(pt, true, PI/2);
+ hosei_turn(pt, CW, PI/2);
}
void turn_ccw()
{
- float pt;
- move(turnspeed,(-1)*turnspeed);
-
update();
- pt=coordinateTheta();
+ float pt = coordinateTheta();;
+ move(turnspeed, -turnspeed);
while(1) {
update();
- if(pt-coordinateTheta()<(-1)*PI/2) {
+ if(coordinateTheta() - pt > PI/2) {
move(0,0);
break;
}
}
- hosei_turn(pt, false, PI/2);
+ hosei_turn(pt, CCW, PI/2);
}
void hosei_turn(float pt, bool cw, float rad)
@@ -174,23 +168,217 @@
}
}
-void turn_direction(int rad)
+
+void turn_abs_rad(float rad)
{
- move(turnspeed, (-1)*turnspeed);
+ update();
- update();
+ int np;
+ if(coordinateTheta() > rad) np = 1;
+ else if(coordinateTheta() < rad) np = -1;
+ else return;
+
+ move((-np)*turnspeed, (np)*turnspeed);
while(1) {
update();
- if(coordinateTheta() < rad + 0.1 && coordinateTheta() > rad - 0.1) {
+ if(coordinateTheta() < rad + 0.1 && rad - 0.1 < coordinateTheta()) {
move(0,0);
break;
}
}
- hosei_turn(0, false, rad);
+ hosei_turn(0, false, rad);
+}
+
+
+void pmovex(int length)
+{
+ int px,py,ptheta,dx,dy,dtheta;
+
+ update();
+ px=coordinateX();
+ py=coordinateY();
+ ptheta=coordinateTheta();
+ move(rightspeed,leftspeed);
+
+ while(1) {
+ update();
+ dx=coordinateX()-px;
+ dy=coordinateY()-py;
+ dtheta=coordinateTheta()-ptheta;
+ if(dy>3) {
+ dy=3;
+ } else if(dy<-3) {
+ dy=-3;
+ }
+ move(rightspeed - k_y * dy - k_theta * dtheta,leftspeed + k_y *dy + k_theta * dtheta);
+
+ if(dx>length) {
+ move(0,0);
+ break;
+ }
+ }
+}
+
+void pmovey(int length)
+{
+ int px,py,ptheta,dx,dy,dtheta;
+
+ update();
+ px=coordinateX();
+ py=coordinateY();
+ ptheta=coordinateTheta();
+ move(rightspeed,leftspeed);
+
+ while(1) {
+ update();
+ dx=coordinateX()-px;
+ dy=coordinateY()-py;
+ dtheta=coordinateTheta()-ptheta;
+
+ if(dx>7) {
+ dx=7;
+ } else if(dx<-7) {
+ dx=-7;
+ }
+ move(rightspeed-k_x*dx-k_theta*dtheta,leftspeed+k_x*dx+k_theta*dtheta);
+
+ if(dy>length) {
+ move(0,0);
+ break;
+ }
+ }
+}
+
+/*
+void pmovex_minus(int length)
+{
+ int px,py,ptheta,dx,dy,dtheta;
+
+ update();
+ px=coordinateX();
+ py=coordinateY();
+ ptheta=coordinateTheta();
+ move(rightspeed,leftspeed);
+
+ while(1) {
+ update();
+ dx=coordinateX()-px;
+ dy=coordinateY()-py;
+ dtheta=coordinateTheta()-ptheta;
+
+ if(dy>7) {
+ dy=7;
+ }
+ if(dy<-7) {
+ dy=-7;
+ }
+ move(rightspeed - k_y * dy - k_theta * dtheta, leftspeed + k_y *dy + k_theta * dtheta);
+
+ if(dx < length) {
+ move(0,0);
+ break;
+ }
+ }
}
-void pmovex(int length)
+
+void pmovey_minus()
+{
+ int px,py,ptheta,dx,dy,dtheta;
+ int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+ int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+
+ update();
+ px=coordinateX();
+ py=coordinateY();
+ ptheta=coordinateTheta();
+ move(rightspeed,leftspeed);
+
+ while(1) {
+ update();
+ dx=coordinateX()-px;
+ dy=coordinateY()-py;
+ dtheta=coordinateTheta()-ptheta;
+
+ if(dx>7) {
+ dx=7;
+ }
+ if(dx<-7) {
+ dx=-7;
+ }
+ move(rightspeed - k_x * dx - k_theta * dtheta,leftspeed + k_x *dx + k_theta * dtheta);
+
+ if(dy<length) {
+ move(0,0);
+ break;
+ }
+ }
+}
+*/
+
+void pmovex_to(int x, int y)
+{
+ int px,py,ptheta,dx,dy,dtheta;
+
+ if(x <= coordinateX()) return;
+ update();
+ py=y;
+ ptheta=coordinateTheta();
+ move(rightspeed,leftspeed);
+
+ while(1) {
+ update();
+ dy=coordinateY()-py;
+ dtheta=coordinateTheta()-ptheta;
+ if(dy>3) {
+ dy=3;
+ } else if(dy<-3) {
+ dy=-3;
+ }
+ move(rightspeed - k_y * dy - k_theta * dtheta,leftspeed + k_y *dy + k_theta * dtheta);
+
+ if(coordinateX() > x) {
+ move(0,0);
+ break;
+ }
+ }
+}
+
+/*
+void pmovey_to(int x, int y)
+{
+ int px,py,ptheta,dx,dy,dtheta;
+ int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+ int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+
+ update();
+ px=coordinateX();
+ py=coordinateY();
+ ptheta=coordinateTheta();
+ move(rightspeed,leftspeed);
+
+ while(1) {
+ update();
+ dx=coordinateX()-px;
+ dy=coordinateY()-py;
+ dtheta=coordinateTheta()-ptheta;
+
+ if(dx>7) {
+ dx=7;
+ } else if(dx<-7) {
+ dx=-7;
+ }
+ move(rightspeed-k_x*dx-k_theta*dtheta,leftspeed+k_x*dx+k_theta*dtheta);
+
+ if(dy>length) {
+ move(0,0);
+ break;
+ }
+ }
+}
+
+void pmovex_minus_to(int x, int y)
{
int px,py,ptheta,dx,dy,dtheta;
int k_y=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
@@ -214,16 +402,16 @@
if(dy<-7) {
dy=-7;
}
- move(rightspeed-k_y*dy-k_theta*dtheta,leftspeed+k_y*dy+k_theta*dtheta);
+ move(rightspeed - k_y * dy - k_theta * dtheta, leftspeed + k_y *dy + k_theta * dtheta);
- if(dx>length) {
+ if(dx < length) {
move(0,0);
break;
}
}
}
-void pmovey(int length)
+void pmovey_minus_to(int x, int y)
{
int px,py,ptheta,dx,dy,dtheta;
int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
@@ -247,11 +435,12 @@
if(dx<-7) {
dx=-7;
}
- move(rightspeed-k_x*dx-k_theta*dtheta,leftspeed+k_x*dx+k_theta*dtheta);
+ move(rightspeed - k_x * dx - k_theta * dtheta,leftspeed + k_x *dx + k_theta * dtheta);
- if(dy>length) {
+ if(dy<length) {
move(0,0);
break;
}
}
}
+*/
\ No newline at end of file