涼太郎 中村
k
Dependents: 3rdcompfixstart 2ndcomp 4thcomp 6th33222_copy
Diff: move.cpp
- Revision:
- 12:4e9cadc225f5
- Parent:
- 10:6d38d1b6cad5
--- a/move.cpp Fri Sep 09 05:41:17 2016 +0000
+++ b/move.cpp Sat Sep 10 13:30:03 2016 +0000
@@ -2,48 +2,49 @@
#include "move.h"
#include "locate.h"
#include "stdlib.h"
-
+#include "math.h"
+
PwmOut M1cw(PA_11);
PwmOut M1ccw(PB_15);
PwmOut M2ccw(PB_14);
PwmOut M2cw(PB_13);
-
+
DigitalOut green (PC_2);
DigitalOut yellow(PC_3);
DigitalOut red (PC_0);
-
+
/*
DigitalOut teamledblue(PC_10);
DigitalOut teamledred(PC_12);
*/
-
+
Serial pc2(SERIAL_TX, SERIAL_RX); //pcと通信
-
-
+
+
//const int allowlength=5;
//const float allowdegree=0.02;
const int rightspeed=70;
-const int leftspeed=rightspeed + 2;
-const int turnspeed=30*2;
-const float k = 0.9;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
-const int k_theta = 2;
+const int leftspeed=rightspeed + 4;
+const int hosei_ = 13;
+const int max_disorder = 4;
+const float ratio = 1.0/7.5;
//const float PIfact=2.89;
-
-
+
+
void initmotor()
{
M1cw.period_us(256);
M1ccw.period_us(256);
M2cw.period_us(256);
M2ccw.period_us(256);
-
+
}
-
+
void move(int left,int right)
{
-
+
float rightduty,leftduty;
-
+
if(right>256) {
right=256;
}
@@ -56,7 +57,7 @@
if(left<-256) {
left=-256;
}
-
+
rightduty=right/256.0;
leftduty=left/256.0;
if(right>0) {
@@ -66,7 +67,7 @@
M1cw.write(1);
M1ccw.write(1+rightduty);
}
-
+
if(left>0) {
M2cw.write(1-leftduty);
M2ccw.write(1);
@@ -75,40 +76,71 @@
M2ccw.write(1+leftduty);
}
}
-
+
void hosei_turn(float pt, bool cw, float rad)
{
int np;
if(cw) np = 1;
else np = -1;
while(1) {
- update();
+ update_np();
//pc.printf("t:%f\n\r", coordinateTheta());
if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
- move(-20, 20);
+ move(-hosei_, hosei_);
} else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
- move(20, -20);
+ move(hosei_, -hosei_);
} else {
move(0,0);
return;
}
}
-
+
}
-
-void turn_abs_rad(float rad)
+
+void turnrad(float rad)
{
green = 1;
-
- update();
-
+
+ update_np();
+
int np;
if(coordinateTheta() > rad) np = 1;
else if(coordinateTheta() < rad) np = -1;
else return;
-
- move((-np)*(turnspeed+2), (np)*turnspeed);
-
+
+ move((-np)*rightspeed, (np)*leftspeed);
+
+ while(1) {
+ update_np();
+ //pc.printf("t:%f\n\r", coordinateTheta());
+ if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
+ move(0,0);
+ break;
+ }
+ }
+
+
+
+ hosei_turn(0, false, rad);
+
+ wait(0.5);
+
+ hosei_turn(0, false, rad);
+
+ wait(0.5);
+ green = 0;
+
+}
+
+
+void turnrad_ccw(float rad)
+{
+ green = 1;
+
+ update();
+
+ move(rightspeed, -leftspeed);
+
while(1) {
update();
//pc.printf("t:%f\n\r", coordinateTheta());
@@ -117,36 +149,64 @@
break;
}
}
-
-
-
+
+
+
hosei_turn(0, false, rad);
-
+
wait(0.5);
-
+
hosei_turn(0, false, rad);
-
+
+
wait(0.5);
green = 0;
-
}
-
+
+void turnrad_cw(float rad)
+{
+ green = 1;
+
+ update();
+
+ move((-1)*rightspeed, leftspeed);
+
+ while(1) {
+ update();
+ //pc.printf("t:%f\n\r", coordinateTheta());
+ if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
+ move(0,0);
+ break;
+ }
+ }
+
+
+
+ hosei_turn(0, false, rad);
+
+ wait(0.2);
+
+ hosei_turn(0, false, rad);
+
+ wait(0.2);
+ green = 0;
+}
+
void pmove(int x, int y)
{
yellow = 1;
-
- float k = 0.9;//ズレ(mm)を回転数に反映させる比例定数
-
- int k_theta = 2;//ズレ(rad)を回転数に反映させる比例定数
-
+
+ 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())) {
y = coordinateY();
direction = X_PLUS;
@@ -160,16 +220,16 @@
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;
@@ -188,94 +248,469 @@
default:
return;
}
-
- turn_abs_rad(ptheta);
-
+
+ ptheta += nearPi(coordinateTheta() - ptheta);
+
+ turnrad(ptheta);
+
if(length == 0) return;
-
+
+ int i = 0;
+
while(1) {
- update();
+ update_np();
dx = x - coordinateX();
dy = y - coordinateY();
dtheta = coordinateTheta() - ptheta;
-
- if(*disorder>2) {
- *disorder = 2;
- } else if(*disorder<-2) {
- *disorder = -2;
+
+ if(*disorder>max_disorder) {
+ *disorder = max_disorder;
+ } else if(*disorder<-max_disorder) {
+ *disorder = -max_disorder;
}
-
+
absd_length = abs(*d_length);
-
- if(absd_length > length - 30) {
- daikei = abs(length - absd_length) / 30.0;
- } else if(absd_length < 150) {
- daikei = absd_length / 150.0;
- } else
+
+
+ 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;
+}
+
+void pmove2(int x, int y)
+{
+ yellow = 1;
+ red=0;
+ float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
+ int k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
+
+ double length, d_length;
+ int disorder;
+ float dtheta, ptheta;
+ float daikei;
+
+
+ length = sqrt(pow(x - coordinateTheta(), 2) + pow(y - coordinateTheta(), 2));
+
+ pc2.printf("length:%f", length);
+
+ if(length == 0) {
+ red=1;
+ return;
+ }
+
+ ptheta = giveatan(x, y);
+
+ ptheta += nearPi(coordinateTheta() - ptheta);
+
+ turnrad(ptheta);
+
+ virtual_setup();
+
+ int i = 0;
+
+ while(1) {
+ update();
+ 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*4/5.0 + k*(*disorder) - k_theta*dtheta) + rightspeed/5.0,
- daikei * (leftspeed*4/5.0 - k*(*disorder) + k_theta*dtheta) + leftspeed/5.0);
-
+
+ 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("length:%f, d_length:%f, vx:%d, vy:%d", length, d_length, virtual_coordinateX(), virtual_coordinateY());
+
+ 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.5);
-
+
+ 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();//自己位置取得
+
+ if(targetx - x == 0) return 0;
+
+ phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得
+ if(targetx - x < 0)
+ {
+ if(targety - y > 0)
+ phi += PI;
+ else if(targety - y < 0)
+ phi -= PI;
+ }
+
+ return phi;
+}
+
void back300()
{
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;
-
- if(length == 0) return;
-
- turn_abs_rad(PI);
-
+
+ turnrad(PI + nearPi(coordinateTheta() - PI));
+
while(1) {
- update();
+ update_np();
dx = coordinateX() - px;
dy = coordinateY() - py;
-
- if(dy>2) {
- dy = 2;
- } else if(dy<-2) {
- dy = -2;
+
+ if(dy>max_disorder) {
+ dy = max_disorder;
+ } else if(dy<-max_disorder) {
+ dy = -max_disorder;
}
-
-
+
+
move(-(30 + k*dy), -(32 - k*dy));
-
-
+
+
if(dx>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;
+}
+
+
+void nxback300()
+{
+ 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(nearPi(coordinateTheta()));
+
+ while(1) {
+ update_np();
+ dx = coordinateX() - px;
+ dy = coordinateY() - py;
+
+ if(dy>max_disorder) {
+ dy = max_disorder;
+ } else if(dy<-max_disorder) {
+ dy = -max_disorder;
+ }
+
+
+ move(-(30 - k*dy), -(32 + k*dy));
+
+
+ if(abs(dx)>length) {
+ move(0, 0);
+ break;
+ }
+
//pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
}
-
- wait(0.5);
-
+
+ wait(0.2);
+
red = 0;
}
+
+
+void pyback300()
+{
+ 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(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;
+}
+
+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) {
+ if(rad > npi + PI)
+ npi += 2*PI;
+ else if(rad < npi - PI)
+ npi -= 2*PI;
+ else
+ return npi;
+ }
+}
+
\ No newline at end of file