涼太郎 中村 / Mbed 2 deprecated 3servotest

a

Dependencies:   Locate Move Servo button mbed

Fork of ARAI45th by Tk A

_hakaba/h_move.cpp@6:931d51a70200, 2016-09-05 (annotated)

Committer:
sakanakuuun
Date:
Mon Sep 05 10:39:03 2016 +0000
Revision:
6:931d51a70200
Parent:
4:1604d599d40f 
aa

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 6:931d51a70200 40 */
sakanakuuun 6:931d51a70200 41
sakanakuuun 6:931d51a70200 42
sakanakuuun 6:931d51a70200 43 /*
sakanakuuun 6:931d51a70200 44 void pmovey_to(int x, int y)
sakanakuuun 6:931d51a70200 45 {
sakanakuuun 6:931d51a70200 46 int px,py,ptheta,dx,dy,dtheta;
sakanakuuun 6:931d51a70200 47 int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 6:931d51a70200 48 int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 6:931d51a70200 49
sakanakuuun 6:931d51a70200 50 update();
sakanakuuun 6:931d51a70200 51 px=coordinateX();
sakanakuuun 6:931d51a70200 52 py=coordinateY();
sakanakuuun 6:931d51a70200 53 ptheta=coordinateTheta();
sakanakuuun 6:931d51a70200 54 move(rightspeed,leftspeed);
sakanakuuun 6:931d51a70200 55
sakanakuuun 6:931d51a70200 56 while(1) {
sakanakuuun 6:931d51a70200 57 update();
sakanakuuun 6:931d51a70200 58 dx=coordinateX()-px;
sakanakuuun 6:931d51a70200 59 dy=coordinateY()-py;
sakanakuuun 6:931d51a70200 60 dtheta=coordinateTheta()-ptheta;
sakanakuuun 6:931d51a70200 61
sakanakuuun 6:931d51a70200 62 if(dx>7) {
sakanakuuun 6:931d51a70200 63 dx=7;
sakanakuuun 6:931d51a70200 64 } else if(dx<-7) {
sakanakuuun 6:931d51a70200 65 dx=-7;
sakanakuuun 6:931d51a70200 66 }
sakanakuuun 6:931d51a70200 67 move(rightspeed-k_x*dx-k_theta*dtheta,leftspeed+k_x*dx+k_theta*dtheta);
sakanakuuun 6:931d51a70200 68
sakanakuuun 6:931d51a70200 69 if(dy>length) {
sakanakuuun 6:931d51a70200 70 move(0,0);
sakanakuuun 6:931d51a70200 71 break;
sakanakuuun 6:931d51a70200 72 }
sakanakuuun 6:931d51a70200 73 }
sakanakuuun 6:931d51a70200 74 }
sakanakuuun 6:931d51a70200 75
sakanakuuun 6:931d51a70200 76 void pmovex_minus_to(int x, int y)
sakanakuuun 6:931d51a70200 77 {
sakanakuuun 6:931d51a70200 78 int px,py,ptheta,dx,dy,dtheta;
sakanakuuun 6:931d51a70200 79 int k_y=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 6:931d51a70200 80 int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 6:931d51a70200 81
sakanakuuun 6:931d51a70200 82 update();
sakanakuuun 6:931d51a70200 83 px=coordinateX();
sakanakuuun 6:931d51a70200 84 py=coordinateY();
sakanakuuun 6:931d51a70200 85 ptheta=coordinateTheta();
sakanakuuun 6:931d51a70200 86 move(rightspeed,leftspeed);
sakanakuuun 6:931d51a70200 87
sakanakuuun 6:931d51a70200 88 while(1) {
sakanakuuun 6:931d51a70200 89 update();
sakanakuuun 6:931d51a70200 90 dx=coordinateX()-px;
sakanakuuun 6:931d51a70200 91 dy=coordinateY()-py;
sakanakuuun 6:931d51a70200 92 dtheta=coordinateTheta()-ptheta;
sakanakuuun 6:931d51a70200 93
sakanakuuun 6:931d51a70200 94 if(dy>7) {
sakanakuuun 6:931d51a70200 95 dy=7;
sakanakuuun 6:931d51a70200 96 }
sakanakuuun 6:931d51a70200 97 if(dy<-7) {
sakanakuuun 6:931d51a70200 98 dy=-7;
sakanakuuun 6:931d51a70200 99 }
sakanakuuun 6:931d51a70200 100 move(rightspeed - k_y * dy - k_theta * dtheta, leftspeed + k_y *dy + k_theta * dtheta);
sakanakuuun 6:931d51a70200 101
sakanakuuun 6:931d51a70200 102 if(dx < length) {
sakanakuuun 6:931d51a70200 103 move(0,0);
sakanakuuun 6:931d51a70200 104 break;
sakanakuuun 6:931d51a70200 105 }
sakanakuuun 6:931d51a70200 106 }
sakanakuuun 6:931d51a70200 107 }
sakanakuuun 6:931d51a70200 108
sakanakuuun 6:931d51a70200 109 void pmovey_minus_to(int x, int y)
sakanakuuun 6:931d51a70200 110 {
sakanakuuun 6:931d51a70200 111 int px,py,ptheta,dx,dy,dtheta;
sakanakuuun 6:931d51a70200 112 int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 6:931d51a70200 113 int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 6:931d51a70200 114
sakanakuuun 6:931d51a70200 115 update();
sakanakuuun 6:931d51a70200 116 px=coordinateX();
sakanakuuun 6:931d51a70200 117 py=coordinateY();
sakanakuuun 6:931d51a70200 118 ptheta=coordinateTheta();
sakanakuuun 6:931d51a70200 119 move(rightspeed,leftspeed);
sakanakuuun 6:931d51a70200 120
sakanakuuun 6:931d51a70200 121 while(1) {
sakanakuuun 6:931d51a70200 122 update();
sakanakuuun 6:931d51a70200 123 dx=coordinateX()-px;
sakanakuuun 6:931d51a70200 124 dy=coordinateY()-py;
sakanakuuun 6:931d51a70200 125 dtheta=coordinateTheta()-ptheta;
sakanakuuun 6:931d51a70200 126
sakanakuuun 6:931d51a70200 127 if(dx>7) {
sakanakuuun 6:931d51a70200 128 dx=7;
sakanakuuun 6:931d51a70200 129 }
sakanakuuun 6:931d51a70200 130 if(dx<-7) {
sakanakuuun 6:931d51a70200 131 dx=-7;
sakanakuuun 6:931d51a70200 132 }
sakanakuuun 6:931d51a70200 133 move(rightspeed - k_x * dx - k_theta * dtheta,leftspeed + k_x *dx + k_theta * dtheta);
sakanakuuun 6:931d51a70200 134
sakanakuuun 6:931d51a70200 135 if(dy<length) {
sakanakuuun 6:931d51a70200 136 move(0,0);
sakanakuuun 6:931d51a70200 137 break;
sakanakuuun 6:931d51a70200 138 }
sakanakuuun 6:931d51a70200 139 }
sakanakuuun 6:931d51a70200 140 }
sakanakuuun 4:1604d599d40f 141 */