涼太郎 中村 / Mbed 2 deprecated 3servotest

a

Dependencies:   Locate Move Servo button mbed

Fork of ARAI45th by Tk A

_hakaba/h_move.cpp@4:1604d599d40f, 2016-09-02 (annotated)

Committer:
sakanakuuun
Date:
Fri Sep 02 12:41:13 2016 +0000
Revision:
4:1604d599d40f
Child:
6:931d51a70200
fs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakanakuuun 4:1604d599d40f 1 /*
sakanakuuun 4:1604d599d40f 2 void pmovex2(int length)
sakanakuuun 4:1604d599d40f 3 {
sakanakuuun 4:1604d599d40f 4 int px,py,ptheta,dx,dy,dtheta;
sakanakuuun 4:1604d599d40f 5 int k_y=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 4:1604d599d40f 6 int k_theta=5;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 4:1604d599d40f 7
sakanakuuun 4:1604d599d40f 8 update();
sakanakuuun 4:1604d599d40f 9 px=coordinateX();
sakanakuuun 4:1604d599d40f 10 py=coordinateY();
sakanakuuun 4:1604d599d40f 11 ptheta=coordinateTheta();
sakanakuuun 4:1604d599d40f 12 move(rightspeed,leftspeed);
sakanakuuun 4:1604d599d40f 13
sakanakuuun 4:1604d599d40f 14 while(1) {
sakanakuuun 4:1604d599d40f 15 update();
sakanakuuun 4:1604d599d40f 16
sakanakuuun 4:1604d599d40f 17 dx = coordinateX() - px;
sakanakuuun 4:1604d599d40f 18 dy = coordinateY() - py;
sakanakuuun 4:1604d599d40f 19 dtheta = coordinateTheta() - ptheta;
sakanakuuun 4:1604d599d40f 20
sakanakuuun 4:1604d599d40f 21 if(dy>7) {
sakanakuuun 4:1604d599d40f 22 dy=7;
sakanakuuun 4:1604d599d40f 23 }
sakanakuuun 4:1604d599d40f 24 else if(dy<-7) {
sakanakuuun 4:1604d599d40f 25 dy=-7;
sakanakuuun 4:1604d599d40f 26 }
sakanakuuun 4:1604d599d40f 27
sakanakuuun 4:1604d599d40f 28 if(-3 < dy && dy < 3){
sakanakuuun 4:1604d599d40f 29 move(rightspeed - k_y*dy - k_theta*dtheta, leftspeed + k_y*dy + k_theta*dtheta);
sakanakuuun 4:1604d599d40f 30 }
sakanakuuun 4:1604d599d40f 31 else{
sakanakuuun 4:1604d599d40f 32 move(rightspeed -50 - k_y*dy*dy - k_theta*dtheta, leftspeed -50 + k_y*dy*dy + k_theta*dtheta);
sakanakuuun 4:1604d599d40f 33 }
sakanakuuun 4:1604d599d40f 34 if(dx>length) {
sakanakuuun 4:1604d599d40f 35 move(0,0);
sakanakuuun 4:1604d599d40f 36 break;
sakanakuuun 4:1604d599d40f 37 }
sakanakuuun 4:1604d599d40f 38 }
sakanakuuun 4:1604d599d40f 39 }
sakanakuuun 4:1604d599d40f 40 */