Tk A
aaa
Diff: move.cpp
- Revision:
- 24:d041fc34d846
- Parent:
- 23:e30ffaeb3e0f
- Child:
- 25:fa9b75c175fd
--- a/move.cpp Sat Sep 10 17:06:05 2016 +0000
+++ b/move.cpp Sat Sep 10 17:30:41 2016 +0000
@@ -4,6 +4,14 @@
#include "stdlib.h"
#include "math.h"
+/*************/
+const int rightspeed = 70;
+const int leftspeed = rightspeed + 4;
+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);
@@ -13,24 +21,8 @@
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 + 4;
-const int hosei_ = 13;
-const int max_disorder = 3;
-const float ratio = 1.0/8.5;
-//const float PIfact=2.89;
-
-
void initmotor()
{
M1cw.period_us(256);
@@ -84,11 +76,10 @@
else np = -1;
while(1) {
update_np();
- //pc.printf("t:%f\n\r", coordinateTheta());
if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
- move(-hosei_, hosei_);
+ move(-hosei_turnspeed, hosei_turnspeed);
} else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
- move(hosei_, -hosei_);
+ move(hosei_turnspeed, -hosei_turnspeed);
} else {
move(0,0);
return;
@@ -108,28 +99,21 @@
else if(coordinateTheta() < rad) np = -1;
else return;
- move((-np)*leftspeed, (np)*rightspeed);
+ 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;
-
}
@@ -139,7 +123,7 @@
update();
- move(leftspeed, -rightspeed);
+ move(rightspeed, -leftspeed);
while(1) {
update();
@@ -150,15 +134,9 @@
}
}
-
-
hosei_turn(0, false, rad);
-
wait(0.5);
-
hosei_turn(0, false, rad);
-
-
wait(0.5);
green = 0;
}
@@ -169,7 +147,7 @@
update();
- move((-1)*leftspeed, rightspeed);
+ move((-1)*rightspeed, leftspeed);
while(1) {
update();
@@ -180,14 +158,9 @@
}
}
-
-
hosei_turn(0, false, rad);
-
wait(0.2);
-
hosei_turn(0, false, rad);
-
wait(0.2);
green = 0;
}
@@ -284,8 +257,8 @@
else
daikei = 1;
- move(daikei * (leftspeed*(1-ratio) + k*(*disorder) - k_theta*dtheta) + leftspeed*ratio,
- daikei * (rightspeed *(1-ratio) - k*(*disorder) + k_theta*dtheta) + rightspeed *ratio);
+ 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)) {
@@ -312,7 +285,6 @@
float dtheta, ptheta;
float daikei;
-
length = sqrt(pow((double)(x - coordinateX()), 2) + pow((double)(y - coordinateY()), 2));
pc2.printf("length:%f", length);
@@ -353,8 +325,8 @@
} else
daikei = 1;
- move(daikei * (leftspeed*(1-ratio) - k*disorder - k_theta*dtheta) + leftspeed*ratio,
- daikei * (rightspeed *(1-ratio) + k*disorder + k_theta*dtheta) + rightspeed *ratio);
+ 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());
@@ -371,43 +343,12 @@
red = 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) {
- if(targety > 0)
- return PI/2;
- else
- return -PI/2;
- }
-
- 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();
@@ -429,16 +370,12 @@
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);
@@ -452,10 +389,7 @@
red = 1;
float k = 0.9;
- int k_theta = 2;
-
int length, px, py, dx, dy;
- float daikei;
update();
@@ -477,16 +411,12 @@
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.2);
@@ -505,10 +435,7 @@
red = 1;
float k = 0.9;
- int k_theta = 2;
-
int length, px, py, dx, dy;
- float daikei;
update();
@@ -530,40 +457,31 @@
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;
+ red = 0;
}
void nyback300(int team)
{
- if(team == 0)
- {
+ if(team == 0) {
pyback300(1);
return;
}
-
+
red = 1;
float k = 0.9;
- int k_theta = 2;
-
int length, px, py, dx, dy;
- float daikei;
update();
@@ -585,16 +503,12 @@
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);
@@ -616,3 +530,31 @@
return npi;
}
}
+
+float giveatan(int targetx,int targety)
+{
+ int x,y;
+ float phi;
+ update();
+ x = coordinateX();
+ y = coordinateY();
+
+ if(targetx - x == 0) {
+ if(targety > 0)
+ return PI/2;
+ else if(targety < 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