fs
Dependencies: Locate Move mbed Servo
Fork of ARAI45th by
_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?
User | Revision | Line number | New 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制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はkはP制御の感度を表す係数です。 |
sakanakuuun | 4:1604d599d40f | 6 | int k_theta=5;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はkは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 | */ |