Tk A
aaa
Diff: move.cpp
- Revision:
- 2:f25a09c5e113
- Parent:
- 1:405e28b64fdb
- Child:
- 3:cecaa0154f92
diff -r 405e28b64fdb -r f25a09c5e113 move.cpp
--- a/move.cpp Sat Sep 03 10:53:20 2016 +0000
+++ b/move.cpp Mon Sep 05 10:37:57 2016 +0000
@@ -1,20 +1,27 @@
#include "mbed.h"
#include "move.h"
#include "locate.h"
+#include "stdlib.h"
PwmOut M1cw(PA_11);
PwmOut M1ccw(PB_15);
PwmOut M2ccw(PB_14);
PwmOut M2cw(PB_13);
+/*
+DigitalOut teamledblue(PC_10);
+DigitalOut teamledred(PC_12);
+*/
+
+//Serial pc(SERIAL_TX, SERIAL_RX); //pcと通信
+
//const int allowlength=5;
//const float allowdegree=0.02;
-const int rightspeed=29*2;
-const int leftspeed=31*2;
+const int rightspeed=120;
+const int leftspeed=rightspeed + 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 k = 0.9;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+const int k_theta = 2;
//const float PIfact=2.89;
@@ -64,92 +71,6 @@
}
}
-
-void movelength(int length)
-{
- int px,py;
- update();
- px=coordinateX();
- py=coordinateY();
- //int pt=coordinateTheta();
-
- move(rightspeed,leftspeed);
- while(1) {
-
- 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 turncw()
-{
- update();
- float pt = coordinateTheta();;
- move(-turnspeed, turnspeed);
-
- while(1) {
- update();
- if(coordinateTheta() - pt > -PI/2) {
- move(0,0);
- break;
- }
- }
-}
-
-void turnccw()
-{
- update();
- float pt = coordinateTheta();;
- move(turnspeed, -turnspeed);
-
- while(1) {
- update();
- if(coordinateTheta() - pt < PI/2) {
- move(0,0);
- break;
- }
- }
-}
-
-void turn_cw()
-{
- update();
- float pt = coordinateTheta();;
- move(-turnspeed, turnspeed);
-
- while(1) {
- update();
- if(coordinateTheta() - pt < -PI/2) {
- move(0,0);
- break;
- }
- }
-
- hosei_turn(pt, CW, PI/2);
-}
-
-void turn_ccw()
-{
- update();
- float pt = coordinateTheta();;
- move(turnspeed, -turnspeed);
-
- while(1) {
- update();
- if(coordinateTheta() - pt > PI/2) {
- move(0,0);
- break;
- }
- }
-
- hosei_turn(pt, CCW, PI/2);
-}
-
void hosei_turn(float pt, bool cw, float rad)
{
int np;
@@ -157,18 +78,19 @@
else np = -1;
while(1) {
update();
+ //pc.printf("t:%f\n\r", coordinateTheta());
if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
- move(-15, 15);
+ move(-10, 10);
} else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
- move(15, -15);
+ move(10, -10);
} else {
move(0,0);
return;
}
}
+
}
-
void turn_abs_rad(float rad)
{
update();
@@ -182,265 +104,114 @@
while(1) {
update();
- if(coordinateTheta() < rad + 0.1 && rad - 0.1 < coordinateTheta()) {
+ //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);
-}
-void pmovex(int length)
-{
- int px,py,ptheta,dx,dy,dtheta;
+ hosei_turn(0, false, rad);
- update();
- px=coordinateX();
- py=coordinateY();
- ptheta=coordinateTheta();
- move(rightspeed,leftspeed);
+ wait(0.5);
- 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);
+ hosei_turn(0, false, rad);
- if(dx>length) {
- move(0,0);
- break;
- }
- }
+ wait(0.5);
}
-void pmovey(int length)
+
+void pmove_to_dir(int direction, int x, int y)
{
- int px,py,ptheta,dx,dy,dtheta;
+ float k = 0.9;
+ int k_theta = 2;
+
+ int length;
+ int dx, dy;
+ int *d_length, *disorder;
+ float dtheta;
+ float ptheta;
+ float daikei;
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;
+ switch(direction) {
+ case X_PLUS:
+ length = x - coordinateX();
+ y = coordinateY();
+ d_length = &dx;
+ disorder = &dy;
+ turn_abs_rad(0);
- update();
- px=coordinateX();
- py=coordinateY();
- ptheta=coordinateTheta();
- move(rightspeed,leftspeed);
-
- while(1) {
- update();
- dx=coordinateX()-px;
- dy=coordinateY()-py;
- dtheta=coordinateTheta()-ptheta;
+ ptheta = 0;
+ break;
+ case Y_PLUS:
+ length = y - coordinateY();
+ x = coordinateX();
+ d_length = &dy;
+ disorder = &dx;
+ k *= -1;
+ turn_abs_rad(PI/2);
+ ptheta = PI/2;
+ break;
+ case X_MINUS:
+ length = x - coordinateX();
+ y = coordinateY();
+ d_length = &dx;
+ disorder = &dy;
+ k *= -1;
+ turn_abs_rad(PI);
+ //pc.printf("finish_turn");
+ ptheta = PI;
+ break;
+ case Y_MINUS:
+ length = y - coordinateY();
+ x = coordinateX();
+ d_length = &dy;
+ disorder = &dx;
+ turn_abs_rad(PI*3/2);
+ ptheta = PI*3/2;
+ break;
+ default:
+ return;
+ }
- 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 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);
+ if(length == 0) return;
while(1) {
update();
- dx=coordinateX()-px;
- dy=coordinateY()-py;
- dtheta=coordinateTheta()-ptheta;
+ dx = coordinateX() - x;
+ dy = coordinateY() - y;
+ dtheta = coordinateTheta() - ptheta;
+
- if(dx>7) {
- dx=7;
- }
- if(dx<-7) {
- dx=-7;
+ if(*disorder>1) {
+ *disorder = 1;
+ } else if(*disorder<-1) {
+ *disorder = -1;
}
- 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;
+ if(abs(*d_length) > abs(length) -200) {
+ daikei = (float)(abs(length)-abs(*d_length)) / 200;
+ }
+ else if(abs(*d_length) < 150) {
+ daikei = (float)(abs(*d_length)) / 150;
}
- move(rightspeed - k_y * dy - k_theta * dtheta,leftspeed + k_y *dy + k_theta * dtheta);
-
- if(coordinateX() > x) {
- move(0,0);
+ else
+ daikei = 1;
+
+ move(daikei * (rightspeed*7/8.0 - k*(*disorder) - k_theta*dtheta) + rightspeed/8.0, daikei * (leftspeed*7/8.0 + k*(*disorder) + k_theta*dtheta) + leftspeed/8.0);
+
+ if((direction > 0 && *d_length >= 0) || (direction < 0 && *d_length <= 0)) {
+ 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制御の感度を表す係数です。
- 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(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;
- }
+ //pc.printf("d_length:%d disorder:%d rs:%f ls:%f g:%f\n\r", *d_length, *disorder, (rightspeed/2.0 - k*(*disorder) - k_theta*dtheta) + rightspeed/2.0, (leftspeed/2.0 + k*(*disorder) + k_theta*dtheta) + leftspeed/2.0, daikei);
}
-}
-
-void pmovey_minus_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;
- }
- 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;
- }
- }
+ wait(0.5);
}
-*/
\ No newline at end of file