a
Dependencies: Locate Move Servo button mbed
Fork of 3servotest by
_hakaba/h_move.cpp
- Committer:
- choutin
- Date:
- 2016-09-08
- Revision:
- 14:66c5b0b26ba0
- Parent:
- 6:931d51a70200
File content as of revision 14:66c5b0b26ba0:
/* void pmovex2(int length) { int px,py,ptheta,dx,dy,dtheta; int k_y=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はkはP制御の感度を表す係数です。 int k_theta=5;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はkは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; } else if(dy<-7) { dy=-7; } if(-3 < dy && dy < 3){ move(rightspeed - k_y*dy - k_theta*dtheta, leftspeed + k_y*dy + k_theta*dtheta); } else{ move(rightspeed -50 - k_y*dy*dy - k_theta*dtheta, leftspeed -50 + k_y*dy*dy + k_theta*dtheta); } if(dx>length) { move(0,0); break; } } } */ /* void pmovey_to(int x, int y) { int px,py,ptheta,dx,dy,dtheta; int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はkはP制御の感度を表す係数です。 int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はkは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制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はkはP制御の感度を表す係数です。 int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はkは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; } } } void pmovey_minus_to(int x, int y) { int px,py,ptheta,dx,dy,dtheta; int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はkはP制御の感度を表す係数です。 int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はkは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; } } } */