涼太郎 中村
k
Dependents: 3rdcompfixstart 2ndcomp 4thcomp 6th33222_copy
Diff: move.cpp
- Revision:
- 13:4501c9202500
- Parent:
- 11:451b0c1d06b7
- Child:
- 14:138af628d979
--- a/move.cpp Fri Sep 09 05:43:40 2016 +0000
+++ b/move.cpp Sun Sep 11 11:39:42 2016 +0000
@@ -2,7 +2,16 @@
#include "move.h"
#include "locate.h"
#include "stdlib.h"
-
+#include "math.h"
+
+/*************/
+const int rightspeed = 70;
+const int leftspeed = (int)(rightspeed *1.1);
+const int hosei_turnspeed = 13;
+const int max_disorder = 3;
+const float ratio = 1.0/8.5;
+/*************/
+
PwmOut M1cw(PA_11);
PwmOut M1ccw(PB_15);
PwmOut M2ccw(PB_14);
@@ -11,54 +20,44 @@
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と通信
+int tcolor;
-//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 float PIfact=2.89;
-
-
-void initmotor()
+void initmotor(int team)
{
+ tcolor = team;
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) {
+
+ if(right>256)
right=256;
- }
- if(left>256) {
+ else if(right<-256)
+ right=-256;
+
+ if(left>256)
left=256;
- }
- if(right<-256) {
- right=-256;
+ else if(left<-256)
+ left=-256;
+
+ if(tcolor == -1)
+ {
+ int t = right;
+ right = left;
+ left = t;
}
- if(left<-256) {
- left=-256;
- }
-
+
rightduty=right/256.0;
leftduty=left/256.0;
+
if(right>0) {
M1cw.write(1-rightduty);
M1ccw.write(1);
@@ -66,7 +65,7 @@
M1cw.write(1);
M1ccw.write(1+rightduty);
}
-
+
if(left>0) {
M2cw.write(1-leftduty);
M2ccw.write(1);
@@ -75,78 +74,152 @@
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();
- //pc.printf("t:%f\n\r", coordinateTheta());
+ update_np();
if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
- move(-20, 20);
+ move(-hosei_turnspeed, hosei_turnspeed);
} else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
- move(20, -20);
+ move(hosei_turnspeed, -hosei_turnspeed);
} 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();
+ 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 turnrad_ccw(float rad, int team)
+{
+ if(team == 0)
+ {
+ turnrad_cw(rad, 1);
+ return;
+ }
+
+ green = 1;
+
+ update();
+
+ move(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 turnrad_cw(float rad, int team)
+{
+ if(team == 0)
+ {
+ turnrad_ccw(rad, 1);
+ return;
+ }
+
+ green = 1;
+
+ update();
+
+ move((-1)*rightspeed, leftspeed);
+
+ while(1) {
+ update();
+ 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;
+}
- hosei_turn(0, false, rad);
-
- wait(0.5);
+void movelength(int length)
+{
+ int px,py,pt;
+ update();
+ px=coordinateX();
+ py=coordinateY();
+ pt=coordinateTheta();
- hosei_turn(0, false, rad);
+ move(rightspeed,leftspeed);
+ while(1) {
- wait(0.5);
- green = 0;
+ update();
+ //pc.printf("dx:%d, dy:%d, l:%d x:%d y:%d t:%f\n\r",px-coordinateX(),py-coordinateY(),length,coordinateX(),coordinateY(), coordinateTheta());
+ if(((px-coordinateX())*(px-coordinateX())+(py-coordinateY())*(py-coordinateY()))>length*length) {
+ break;
+ }
+ }
+ move(0,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 +233,14 @@
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 +259,332 @@
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;
+
+
+ if(i++ < 5) {
+ daikei = i/5;
+ } else if(absd_length < 300) {
+ daikei = absd_length / 300.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("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;
}
-
-
+
+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(pow((double)(x - coordinateX()), 2) + pow((double)(y - coordinateY()), 2));
+
+ if(length == 0) {
+ red=1;
+ return;
+ }
+
+ ptheta = giveatan(x, y);
+
+ ptheta += nearPi(coordinateTheta() - ptheta);
+
+ pc2.printf("pt:%f", ptheta);
+ 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
+ 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("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;
+ }
+
+ }
+
+ wait(0.2);
+
+ yellow = 0;
+ red = 0;
+}
+
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.5);
-
+
+ wait(0.2);
+
+ red = 0;
+}
+
+
+void nxback300()
+{
+ red = 1;
+
+ float k = 0.9;
+ int length, px, py, dx, dy;
+
+ 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;
+ }
+ }
+
+ wait(0.2);
+
red = 0;
}
+
+void pyback300(int team)
+{
+ if(team == 0) {
+ nyback300(1);
+ return;
+ }
+
+ red = 1;
+
+ float k = 0.9;
+ int length, px, py, dx, dy;
+
+ 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;
+ }
+
+ }
+
+ wait(0.2);
+
+ red = 0;
+}
+
+void nyback300(int team)
+{
+ if(team == 0) {
+ pyback300(1);
+ return;
+ }
+
+ red = 1;
+
+ float k = 0.9;
+ int length, px, py, dx, dy;
+
+ 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;
+ }
+ }
+
+ 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;
+ }
+}
+
+float giveatan(int targetx,int targety)
+{
+ int x,y;
+ float phi;
+ update();
+ x = coordinateX();
+ y = coordinateY();
+
+ if(targetx - x == 0) {
+ if(targety - y > 0)
+ return PI/2;
+ else if(targety - y < 0)
+ return -PI/2;
+ else
+ 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;
+}
\ No newline at end of file