Tk A / Move

fs

Dependents:   ARAI45th 3servotest 1stcomp

move.cpp@1:405e28b64fdb, 2016-09-03 (annotated)

Committer:
sakanakuuun
Date:
Sat Sep 03 10:53:20 2016 +0000
Revision:
1:405e28b64fdb
Parent:
0:d7ff86f25eaa
Child:
2:f25a09c5e113
h;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakanakuuun 0:d7ff86f25eaa 1 #include "mbed.h"
sakanakuuun 0:d7ff86f25eaa 2 #include "move.h"
sakanakuuun 0:d7ff86f25eaa 3 #include "locate.h"
sakanakuuun 0:d7ff86f25eaa 4
sakanakuuun 0:d7ff86f25eaa 5 PwmOut M1cw(PA_11);
sakanakuuun 0:d7ff86f25eaa 6 PwmOut M1ccw(PB_15);
sakanakuuun 0:d7ff86f25eaa 7 PwmOut M2ccw(PB_14);
sakanakuuun 0:d7ff86f25eaa 8 PwmOut M2cw(PB_13);
sakanakuuun 0:d7ff86f25eaa 9
sakanakuuun 0:d7ff86f25eaa 10 //const int allowlength=5;
sakanakuuun 0:d7ff86f25eaa 11 //const float allowdegree=0.02;
sakanakuuun 0:d7ff86f25eaa 12 const int rightspeed=29*2;
sakanakuuun 0:d7ff86f25eaa 13 const int leftspeed=31*2;
sakanakuuun 0:d7ff86f25eaa 14 const int turnspeed=15*2;
sakanakuuun 1:405e28b64fdb 15 const float k_x = 0.9;
sakanakuuun 1:405e28b64fdb 16 const float k_y = 0.9;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 1:405e28b64fdb 17 const int k_theta = 500;
sakanakuuun 0:d7ff86f25eaa 18 //const float PIfact=2.89;
sakanakuuun 0:d7ff86f25eaa 19
sakanakuuun 0:d7ff86f25eaa 20
sakanakuuun 0:d7ff86f25eaa 21 void initmotor()
sakanakuuun 0:d7ff86f25eaa 22 {
sakanakuuun 0:d7ff86f25eaa 23 M1cw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 24 M1ccw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 25 M2cw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 26 M2ccw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 27
sakanakuuun 0:d7ff86f25eaa 28 }
sakanakuuun 0:d7ff86f25eaa 29
sakanakuuun 0:d7ff86f25eaa 30 void move(int left,int right)
sakanakuuun 0:d7ff86f25eaa 31 {
sakanakuuun 0:d7ff86f25eaa 32
sakanakuuun 0:d7ff86f25eaa 33 float rightduty,leftduty;
sakanakuuun 0:d7ff86f25eaa 34
sakanakuuun 0:d7ff86f25eaa 35 if(right>256) {
sakanakuuun 0:d7ff86f25eaa 36 right=256;
sakanakuuun 0:d7ff86f25eaa 37 }
sakanakuuun 0:d7ff86f25eaa 38 if(left>256) {
sakanakuuun 0:d7ff86f25eaa 39 left=256;
sakanakuuun 0:d7ff86f25eaa 40 }
sakanakuuun 0:d7ff86f25eaa 41 if(right<-256) {
sakanakuuun 0:d7ff86f25eaa 42 right=-256;
sakanakuuun 0:d7ff86f25eaa 43 }
sakanakuuun 0:d7ff86f25eaa 44 if(left<-256) {
sakanakuuun 0:d7ff86f25eaa 45 left=-256;
sakanakuuun 0:d7ff86f25eaa 46 }
sakanakuuun 0:d7ff86f25eaa 47
sakanakuuun 0:d7ff86f25eaa 48 rightduty=right/256.0;
sakanakuuun 0:d7ff86f25eaa 49 leftduty=left/256.0;
sakanakuuun 0:d7ff86f25eaa 50 if(right>0) {
sakanakuuun 0:d7ff86f25eaa 51 M1cw.write(1-rightduty);
sakanakuuun 0:d7ff86f25eaa 52 M1ccw.write(1);
sakanakuuun 0:d7ff86f25eaa 53 } else {
sakanakuuun 0:d7ff86f25eaa 54 M1cw.write(1);
sakanakuuun 0:d7ff86f25eaa 55 M1ccw.write(1+rightduty);
sakanakuuun 0:d7ff86f25eaa 56 }
sakanakuuun 0:d7ff86f25eaa 57
sakanakuuun 0:d7ff86f25eaa 58 if(left>0) {
sakanakuuun 0:d7ff86f25eaa 59 M2cw.write(1-leftduty);
sakanakuuun 0:d7ff86f25eaa 60 M2ccw.write(1);
sakanakuuun 0:d7ff86f25eaa 61 } else {
sakanakuuun 0:d7ff86f25eaa 62 M2cw.write(1);
sakanakuuun 0:d7ff86f25eaa 63 M2ccw.write(1+leftduty);
sakanakuuun 0:d7ff86f25eaa 64 }
sakanakuuun 0:d7ff86f25eaa 65 }
sakanakuuun 0:d7ff86f25eaa 66
sakanakuuun 0:d7ff86f25eaa 67
sakanakuuun 0:d7ff86f25eaa 68 void movelength(int length)
sakanakuuun 0:d7ff86f25eaa 69 {
sakanakuuun 0:d7ff86f25eaa 70 int px,py;
sakanakuuun 0:d7ff86f25eaa 71 update();
sakanakuuun 0:d7ff86f25eaa 72 px=coordinateX();
sakanakuuun 0:d7ff86f25eaa 73 py=coordinateY();
sakanakuuun 0:d7ff86f25eaa 74 //int pt=coordinateTheta();
sakanakuuun 0:d7ff86f25eaa 75
sakanakuuun 0:d7ff86f25eaa 76 move(rightspeed,leftspeed);
sakanakuuun 0:d7ff86f25eaa 77 while(1) {
sakanakuuun 0:d7ff86f25eaa 78
sakanakuuun 0:d7ff86f25eaa 79 update();
sakanakuuun 0:d7ff86f25eaa 80 //pc.printf("dx:%d, dy:%d, l:%d x:%d y:%d t:%f\n\r",px-coordinateX(),py-coordinateY(),length,coordinateX(),coordinateY(), coordinateTheta());
sakanakuuun 0:d7ff86f25eaa 81 if(((px-coordinateX())*(px-coordinateX())+(py-coordinateY())*(py-coordinateY()))>length*length) {
sakanakuuun 0:d7ff86f25eaa 82 break;
sakanakuuun 0:d7ff86f25eaa 83 }
sakanakuuun 0:d7ff86f25eaa 84
sakanakuuun 0:d7ff86f25eaa 85 }
sakanakuuun 0:d7ff86f25eaa 86 move(0,0);
sakanakuuun 0:d7ff86f25eaa 87 }
sakanakuuun 0:d7ff86f25eaa 88
sakanakuuun 0:d7ff86f25eaa 89 void turncw()
sakanakuuun 0:d7ff86f25eaa 90 {
sakanakuuun 0:d7ff86f25eaa 91 update();
sakanakuuun 1:405e28b64fdb 92 float pt = coordinateTheta();;
sakanakuuun 1:405e28b64fdb 93 move(-turnspeed, turnspeed);
sakanakuuun 0:d7ff86f25eaa 94
sakanakuuun 0:d7ff86f25eaa 95 while(1) {
sakanakuuun 0:d7ff86f25eaa 96 update();
sakanakuuun 1:405e28b64fdb 97 if(coordinateTheta() - pt > -PI/2) {
sakanakuuun 0:d7ff86f25eaa 98 move(0,0);
sakanakuuun 0:d7ff86f25eaa 99 break;
sakanakuuun 0:d7ff86f25eaa 100 }
sakanakuuun 0:d7ff86f25eaa 101 }
sakanakuuun 0:d7ff86f25eaa 102 }
sakanakuuun 0:d7ff86f25eaa 103
sakanakuuun 0:d7ff86f25eaa 104 void turnccw()
sakanakuuun 0:d7ff86f25eaa 105 {
sakanakuuun 0:d7ff86f25eaa 106 update();
sakanakuuun 1:405e28b64fdb 107 float pt = coordinateTheta();;
sakanakuuun 1:405e28b64fdb 108 move(turnspeed, -turnspeed);
sakanakuuun 0:d7ff86f25eaa 109
sakanakuuun 0:d7ff86f25eaa 110 while(1) {
sakanakuuun 0:d7ff86f25eaa 111 update();
sakanakuuun 1:405e28b64fdb 112 if(coordinateTheta() - pt < PI/2) {
sakanakuuun 0:d7ff86f25eaa 113 move(0,0);
sakanakuuun 0:d7ff86f25eaa 114 break;
sakanakuuun 0:d7ff86f25eaa 115 }
sakanakuuun 0:d7ff86f25eaa 116 }
sakanakuuun 0:d7ff86f25eaa 117 }
sakanakuuun 0:d7ff86f25eaa 118
sakanakuuun 0:d7ff86f25eaa 119 void turn_cw()
sakanakuuun 0:d7ff86f25eaa 120 {
sakanakuuun 0:d7ff86f25eaa 121 update();
sakanakuuun 1:405e28b64fdb 122 float pt = coordinateTheta();;
sakanakuuun 1:405e28b64fdb 123 move(-turnspeed, turnspeed);
sakanakuuun 0:d7ff86f25eaa 124
sakanakuuun 0:d7ff86f25eaa 125 while(1) {
sakanakuuun 0:d7ff86f25eaa 126 update();
sakanakuuun 1:405e28b64fdb 127 if(coordinateTheta() - pt < -PI/2) {
sakanakuuun 0:d7ff86f25eaa 128 move(0,0);
sakanakuuun 0:d7ff86f25eaa 129 break;
sakanakuuun 0:d7ff86f25eaa 130 }
sakanakuuun 0:d7ff86f25eaa 131 }
sakanakuuun 0:d7ff86f25eaa 132
sakanakuuun 1:405e28b64fdb 133 hosei_turn(pt, CW, PI/2);
sakanakuuun 0:d7ff86f25eaa 134 }
sakanakuuun 0:d7ff86f25eaa 135
sakanakuuun 0:d7ff86f25eaa 136 void turn_ccw()
sakanakuuun 0:d7ff86f25eaa 137 {
sakanakuuun 0:d7ff86f25eaa 138 update();
sakanakuuun 1:405e28b64fdb 139 float pt = coordinateTheta();;
sakanakuuun 1:405e28b64fdb 140 move(turnspeed, -turnspeed);
sakanakuuun 0:d7ff86f25eaa 141
sakanakuuun 0:d7ff86f25eaa 142 while(1) {
sakanakuuun 0:d7ff86f25eaa 143 update();
sakanakuuun 1:405e28b64fdb 144 if(coordinateTheta() - pt > PI/2) {
sakanakuuun 0:d7ff86f25eaa 145 move(0,0);
sakanakuuun 0:d7ff86f25eaa 146 break;
sakanakuuun 0:d7ff86f25eaa 147 }
sakanakuuun 0:d7ff86f25eaa 148 }
sakanakuuun 0:d7ff86f25eaa 149
sakanakuuun 1:405e28b64fdb 150 hosei_turn(pt, CCW, PI/2);
sakanakuuun 0:d7ff86f25eaa 151 }
sakanakuuun 0:d7ff86f25eaa 152
sakanakuuun 0:d7ff86f25eaa 153 void hosei_turn(float pt, bool cw, float rad)
sakanakuuun 0:d7ff86f25eaa 154 {
sakanakuuun 0:d7ff86f25eaa 155 int np;
sakanakuuun 0:d7ff86f25eaa 156 if(cw) np = 1;
sakanakuuun 0:d7ff86f25eaa 157 else np = -1;
sakanakuuun 0:d7ff86f25eaa 158 while(1) {
sakanakuuun 0:d7ff86f25eaa 159 update();
sakanakuuun 0:d7ff86f25eaa 160 if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
sakanakuuun 0:d7ff86f25eaa 161 move(-15, 15);
sakanakuuun 0:d7ff86f25eaa 162 } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
sakanakuuun 0:d7ff86f25eaa 163 move(15, -15);
sakanakuuun 0:d7ff86f25eaa 164 } else {
sakanakuuun 0:d7ff86f25eaa 165 move(0,0);
sakanakuuun 0:d7ff86f25eaa 166 return;
sakanakuuun 0:d7ff86f25eaa 167 }
sakanakuuun 0:d7ff86f25eaa 168 }
sakanakuuun 0:d7ff86f25eaa 169 }
sakanakuuun 0:d7ff86f25eaa 170
sakanakuuun 1:405e28b64fdb 171
sakanakuuun 1:405e28b64fdb 172 void turn_abs_rad(float rad)
sakanakuuun 0:d7ff86f25eaa 173 {
sakanakuuun 1:405e28b64fdb 174 update();
sakanakuuun 0:d7ff86f25eaa 175
sakanakuuun 1:405e28b64fdb 176 int np;
sakanakuuun 1:405e28b64fdb 177 if(coordinateTheta() > rad) np = 1;
sakanakuuun 1:405e28b64fdb 178 else if(coordinateTheta() < rad) np = -1;
sakanakuuun 1:405e28b64fdb 179 else return;
sakanakuuun 1:405e28b64fdb 180
sakanakuuun 1:405e28b64fdb 181 move((-np)*turnspeed, (np)*turnspeed);
sakanakuuun 0:d7ff86f25eaa 182
sakanakuuun 0:d7ff86f25eaa 183 while(1) {
sakanakuuun 0:d7ff86f25eaa 184 update();
sakanakuuun 1:405e28b64fdb 185 if(coordinateTheta() < rad + 0.1 && rad - 0.1 < coordinateTheta()) {
sakanakuuun 0:d7ff86f25eaa 186 move(0,0);
sakanakuuun 0:d7ff86f25eaa 187 break;
sakanakuuun 0:d7ff86f25eaa 188 }
sakanakuuun 0:d7ff86f25eaa 189 }
sakanakuuun 0:d7ff86f25eaa 190
sakanakuuun 1:405e28b64fdb 191 hosei_turn(0, false, rad);
sakanakuuun 1:405e28b64fdb 192 }
sakanakuuun 1:405e28b64fdb 193
sakanakuuun 1:405e28b64fdb 194
sakanakuuun 1:405e28b64fdb 195 void pmovex(int length)
sakanakuuun 1:405e28b64fdb 196 {
sakanakuuun 1:405e28b64fdb 197 int px,py,ptheta,dx,dy,dtheta;
sakanakuuun 1:405e28b64fdb 198
sakanakuuun 1:405e28b64fdb 199 update();
sakanakuuun 1:405e28b64fdb 200 px=coordinateX();
sakanakuuun 1:405e28b64fdb 201 py=coordinateY();
sakanakuuun 1:405e28b64fdb 202 ptheta=coordinateTheta();
sakanakuuun 1:405e28b64fdb 203 move(rightspeed,leftspeed);
sakanakuuun 1:405e28b64fdb 204
sakanakuuun 1:405e28b64fdb 205 while(1) {
sakanakuuun 1:405e28b64fdb 206 update();
sakanakuuun 1:405e28b64fdb 207 dx=coordinateX()-px;
sakanakuuun 1:405e28b64fdb 208 dy=coordinateY()-py;
sakanakuuun 1:405e28b64fdb 209 dtheta=coordinateTheta()-ptheta;
sakanakuuun 1:405e28b64fdb 210 if(dy>3) {
sakanakuuun 1:405e28b64fdb 211 dy=3;
sakanakuuun 1:405e28b64fdb 212 } else if(dy<-3) {
sakanakuuun 1:405e28b64fdb 213 dy=-3;
sakanakuuun 1:405e28b64fdb 214 }
sakanakuuun 1:405e28b64fdb 215 move(rightspeed - k_y * dy - k_theta * dtheta,leftspeed + k_y *dy + k_theta * dtheta);
sakanakuuun 1:405e28b64fdb 216
sakanakuuun 1:405e28b64fdb 217 if(dx>length) {
sakanakuuun 1:405e28b64fdb 218 move(0,0);
sakanakuuun 1:405e28b64fdb 219 break;
sakanakuuun 1:405e28b64fdb 220 }
sakanakuuun 1:405e28b64fdb 221 }
sakanakuuun 1:405e28b64fdb 222 }
sakanakuuun 1:405e28b64fdb 223
sakanakuuun 1:405e28b64fdb 224 void pmovey(int length)
sakanakuuun 1:405e28b64fdb 225 {
sakanakuuun 1:405e28b64fdb 226 int px,py,ptheta,dx,dy,dtheta;
sakanakuuun 1:405e28b64fdb 227
sakanakuuun 1:405e28b64fdb 228 update();
sakanakuuun 1:405e28b64fdb 229 px=coordinateX();
sakanakuuun 1:405e28b64fdb 230 py=coordinateY();
sakanakuuun 1:405e28b64fdb 231 ptheta=coordinateTheta();
sakanakuuun 1:405e28b64fdb 232 move(rightspeed,leftspeed);
sakanakuuun 1:405e28b64fdb 233
sakanakuuun 1:405e28b64fdb 234 while(1) {
sakanakuuun 1:405e28b64fdb 235 update();
sakanakuuun 1:405e28b64fdb 236 dx=coordinateX()-px;
sakanakuuun 1:405e28b64fdb 237 dy=coordinateY()-py;
sakanakuuun 1:405e28b64fdb 238 dtheta=coordinateTheta()-ptheta;
sakanakuuun 1:405e28b64fdb 239
sakanakuuun 1:405e28b64fdb 240 if(dx>7) {
sakanakuuun 1:405e28b64fdb 241 dx=7;
sakanakuuun 1:405e28b64fdb 242 } else if(dx<-7) {
sakanakuuun 1:405e28b64fdb 243 dx=-7;
sakanakuuun 1:405e28b64fdb 244 }
sakanakuuun 1:405e28b64fdb 245 move(rightspeed-k_x*dx-k_theta*dtheta,leftspeed+k_x*dx+k_theta*dtheta);
sakanakuuun 1:405e28b64fdb 246
sakanakuuun 1:405e28b64fdb 247 if(dy>length) {
sakanakuuun 1:405e28b64fdb 248 move(0,0);
sakanakuuun 1:405e28b64fdb 249 break;
sakanakuuun 1:405e28b64fdb 250 }
sakanakuuun 1:405e28b64fdb 251 }
sakanakuuun 1:405e28b64fdb 252 }
sakanakuuun 1:405e28b64fdb 253
sakanakuuun 1:405e28b64fdb 254 /*
sakanakuuun 1:405e28b64fdb 255 void pmovex_minus(int length)
sakanakuuun 1:405e28b64fdb 256 {
sakanakuuun 1:405e28b64fdb 257 int px,py,ptheta,dx,dy,dtheta;
sakanakuuun 1:405e28b64fdb 258
sakanakuuun 1:405e28b64fdb 259 update();
sakanakuuun 1:405e28b64fdb 260 px=coordinateX();
sakanakuuun 1:405e28b64fdb 261 py=coordinateY();
sakanakuuun 1:405e28b64fdb 262 ptheta=coordinateTheta();
sakanakuuun 1:405e28b64fdb 263 move(rightspeed,leftspeed);
sakanakuuun 1:405e28b64fdb 264
sakanakuuun 1:405e28b64fdb 265 while(1) {
sakanakuuun 1:405e28b64fdb 266 update();
sakanakuuun 1:405e28b64fdb 267 dx=coordinateX()-px;
sakanakuuun 1:405e28b64fdb 268 dy=coordinateY()-py;
sakanakuuun 1:405e28b64fdb 269 dtheta=coordinateTheta()-ptheta;
sakanakuuun 1:405e28b64fdb 270
sakanakuuun 1:405e28b64fdb 271 if(dy>7) {
sakanakuuun 1:405e28b64fdb 272 dy=7;
sakanakuuun 1:405e28b64fdb 273 }
sakanakuuun 1:405e28b64fdb 274 if(dy<-7) {
sakanakuuun 1:405e28b64fdb 275 dy=-7;
sakanakuuun 1:405e28b64fdb 276 }
sakanakuuun 1:405e28b64fdb 277 move(rightspeed - k_y * dy - k_theta * dtheta, leftspeed + k_y *dy + k_theta * dtheta);
sakanakuuun 1:405e28b64fdb 278
sakanakuuun 1:405e28b64fdb 279 if(dx < length) {
sakanakuuun 1:405e28b64fdb 280 move(0,0);
sakanakuuun 1:405e28b64fdb 281 break;
sakanakuuun 1:405e28b64fdb 282 }
sakanakuuun 1:405e28b64fdb 283 }
sakanakuuun 0:d7ff86f25eaa 284 }
sakanakuuun 1:405e28b64fdb 285
sakanakuuun 1:405e28b64fdb 286 void pmovey_minus()
sakanakuuun 1:405e28b64fdb 287 {
sakanakuuun 1:405e28b64fdb 288 int px,py,ptheta,dx,dy,dtheta;
sakanakuuun 1:405e28b64fdb 289 int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 1:405e28b64fdb 290 int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 1:405e28b64fdb 291
sakanakuuun 1:405e28b64fdb 292 update();
sakanakuuun 1:405e28b64fdb 293 px=coordinateX();
sakanakuuun 1:405e28b64fdb 294 py=coordinateY();
sakanakuuun 1:405e28b64fdb 295 ptheta=coordinateTheta();
sakanakuuun 1:405e28b64fdb 296 move(rightspeed,leftspeed);
sakanakuuun 1:405e28b64fdb 297
sakanakuuun 1:405e28b64fdb 298 while(1) {
sakanakuuun 1:405e28b64fdb 299 update();
sakanakuuun 1:405e28b64fdb 300 dx=coordinateX()-px;
sakanakuuun 1:405e28b64fdb 301 dy=coordinateY()-py;
sakanakuuun 1:405e28b64fdb 302 dtheta=coordinateTheta()-ptheta;
sakanakuuun 1:405e28b64fdb 303
sakanakuuun 1:405e28b64fdb 304 if(dx>7) {
sakanakuuun 1:405e28b64fdb 305 dx=7;
sakanakuuun 1:405e28b64fdb 306 }
sakanakuuun 1:405e28b64fdb 307 if(dx<-7) {
sakanakuuun 1:405e28b64fdb 308 dx=-7;
sakanakuuun 1:405e28b64fdb 309 }
sakanakuuun 1:405e28b64fdb 310 move(rightspeed - k_x * dx - k_theta * dtheta,leftspeed + k_x *dx + k_theta * dtheta);
sakanakuuun 1:405e28b64fdb 311
sakanakuuun 1:405e28b64fdb 312 if(dy<length) {
sakanakuuun 1:405e28b64fdb 313 move(0,0);
sakanakuuun 1:405e28b64fdb 314 break;
sakanakuuun 1:405e28b64fdb 315 }
sakanakuuun 1:405e28b64fdb 316 }
sakanakuuun 1:405e28b64fdb 317 }
sakanakuuun 1:405e28b64fdb 318 */
sakanakuuun 1:405e28b64fdb 319
sakanakuuun 1:405e28b64fdb 320 void pmovex_to(int x, int y)
sakanakuuun 1:405e28b64fdb 321 {
sakanakuuun 1:405e28b64fdb 322 int px,py,ptheta,dx,dy,dtheta;
sakanakuuun 1:405e28b64fdb 323
sakanakuuun 1:405e28b64fdb 324 if(x <= coordinateX()) return;
sakanakuuun 1:405e28b64fdb 325 update();
sakanakuuun 1:405e28b64fdb 326 py=y;
sakanakuuun 1:405e28b64fdb 327 ptheta=coordinateTheta();
sakanakuuun 1:405e28b64fdb 328 move(rightspeed,leftspeed);
sakanakuuun 1:405e28b64fdb 329
sakanakuuun 1:405e28b64fdb 330 while(1) {
sakanakuuun 1:405e28b64fdb 331 update();
sakanakuuun 1:405e28b64fdb 332 dy=coordinateY()-py;
sakanakuuun 1:405e28b64fdb 333 dtheta=coordinateTheta()-ptheta;
sakanakuuun 1:405e28b64fdb 334 if(dy>3) {
sakanakuuun 1:405e28b64fdb 335 dy=3;
sakanakuuun 1:405e28b64fdb 336 } else if(dy<-3) {
sakanakuuun 1:405e28b64fdb 337 dy=-3;
sakanakuuun 1:405e28b64fdb 338 }
sakanakuuun 1:405e28b64fdb 339 move(rightspeed - k_y * dy - k_theta * dtheta,leftspeed + k_y *dy + k_theta * dtheta);
sakanakuuun 1:405e28b64fdb 340
sakanakuuun 1:405e28b64fdb 341 if(coordinateX() > x) {
sakanakuuun 1:405e28b64fdb 342 move(0,0);
sakanakuuun 1:405e28b64fdb 343 break;
sakanakuuun 1:405e28b64fdb 344 }
sakanakuuun 1:405e28b64fdb 345 }
sakanakuuun 1:405e28b64fdb 346 }
sakanakuuun 1:405e28b64fdb 347
sakanakuuun 1:405e28b64fdb 348 /*
sakanakuuun 1:405e28b64fdb 349 void pmovey_to(int x, int y)
sakanakuuun 1:405e28b64fdb 350 {
sakanakuuun 1:405e28b64fdb 351 int px,py,ptheta,dx,dy,dtheta;
sakanakuuun 1:405e28b64fdb 352 int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 1:405e28b64fdb 353 int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 1:405e28b64fdb 354
sakanakuuun 1:405e28b64fdb 355 update();
sakanakuuun 1:405e28b64fdb 356 px=coordinateX();
sakanakuuun 1:405e28b64fdb 357 py=coordinateY();
sakanakuuun 1:405e28b64fdb 358 ptheta=coordinateTheta();
sakanakuuun 1:405e28b64fdb 359 move(rightspeed,leftspeed);
sakanakuuun 1:405e28b64fdb 360
sakanakuuun 1:405e28b64fdb 361 while(1) {
sakanakuuun 1:405e28b64fdb 362 update();
sakanakuuun 1:405e28b64fdb 363 dx=coordinateX()-px;
sakanakuuun 1:405e28b64fdb 364 dy=coordinateY()-py;
sakanakuuun 1:405e28b64fdb 365 dtheta=coordinateTheta()-ptheta;
sakanakuuun 1:405e28b64fdb 366
sakanakuuun 1:405e28b64fdb 367 if(dx>7) {
sakanakuuun 1:405e28b64fdb 368 dx=7;
sakanakuuun 1:405e28b64fdb 369 } else if(dx<-7) {
sakanakuuun 1:405e28b64fdb 370 dx=-7;
sakanakuuun 1:405e28b64fdb 371 }
sakanakuuun 1:405e28b64fdb 372 move(rightspeed-k_x*dx-k_theta*dtheta,leftspeed+k_x*dx+k_theta*dtheta);
sakanakuuun 1:405e28b64fdb 373
sakanakuuun 1:405e28b64fdb 374 if(dy>length) {
sakanakuuun 1:405e28b64fdb 375 move(0,0);
sakanakuuun 1:405e28b64fdb 376 break;
sakanakuuun 1:405e28b64fdb 377 }
sakanakuuun 1:405e28b64fdb 378 }
sakanakuuun 1:405e28b64fdb 379 }
sakanakuuun 1:405e28b64fdb 380
sakanakuuun 1:405e28b64fdb 381 void pmovex_minus_to(int x, int y)
sakanakuuun 0:d7ff86f25eaa 382 {
sakanakuuun 0:d7ff86f25eaa 383 int px,py,ptheta,dx,dy,dtheta;
sakanakuuun 0:d7ff86f25eaa 384 int k_y=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 0:d7ff86f25eaa 385 int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 0:d7ff86f25eaa 386
sakanakuuun 0:d7ff86f25eaa 387 update();
sakanakuuun 0:d7ff86f25eaa 388 px=coordinateX();
sakanakuuun 0:d7ff86f25eaa 389 py=coordinateY();
sakanakuuun 0:d7ff86f25eaa 390 ptheta=coordinateTheta();
sakanakuuun 0:d7ff86f25eaa 391 move(rightspeed,leftspeed);
sakanakuuun 0:d7ff86f25eaa 392
sakanakuuun 0:d7ff86f25eaa 393 while(1) {
sakanakuuun 0:d7ff86f25eaa 394 update();
sakanakuuun 0:d7ff86f25eaa 395 dx=coordinateX()-px;
sakanakuuun 0:d7ff86f25eaa 396 dy=coordinateY()-py;
sakanakuuun 0:d7ff86f25eaa 397 dtheta=coordinateTheta()-ptheta;
sakanakuuun 0:d7ff86f25eaa 398
sakanakuuun 0:d7ff86f25eaa 399 if(dy>7) {
sakanakuuun 0:d7ff86f25eaa 400 dy=7;
sakanakuuun 0:d7ff86f25eaa 401 }
sakanakuuun 0:d7ff86f25eaa 402 if(dy<-7) {
sakanakuuun 0:d7ff86f25eaa 403 dy=-7;
sakanakuuun 0:d7ff86f25eaa 404 }
sakanakuuun 1:405e28b64fdb 405 move(rightspeed - k_y * dy - k_theta * dtheta, leftspeed + k_y *dy + k_theta * dtheta);
sakanakuuun 0:d7ff86f25eaa 406
sakanakuuun 1:405e28b64fdb 407 if(dx < length) {
sakanakuuun 0:d7ff86f25eaa 408 move(0,0);
sakanakuuun 0:d7ff86f25eaa 409 break;
sakanakuuun 0:d7ff86f25eaa 410 }
sakanakuuun 0:d7ff86f25eaa 411 }
sakanakuuun 0:d7ff86f25eaa 412 }
sakanakuuun 0:d7ff86f25eaa 413
sakanakuuun 1:405e28b64fdb 414 void pmovey_minus_to(int x, int y)
sakanakuuun 0:d7ff86f25eaa 415 {
sakanakuuun 0:d7ff86f25eaa 416 int px,py,ptheta,dx,dy,dtheta;
sakanakuuun 0:d7ff86f25eaa 417 int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 0:d7ff86f25eaa 418 int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 0:d7ff86f25eaa 419
sakanakuuun 0:d7ff86f25eaa 420 update();
sakanakuuun 0:d7ff86f25eaa 421 px=coordinateX();
sakanakuuun 0:d7ff86f25eaa 422 py=coordinateY();
sakanakuuun 0:d7ff86f25eaa 423 ptheta=coordinateTheta();
sakanakuuun 0:d7ff86f25eaa 424 move(rightspeed,leftspeed);
sakanakuuun 0:d7ff86f25eaa 425
sakanakuuun 0:d7ff86f25eaa 426 while(1) {
sakanakuuun 0:d7ff86f25eaa 427 update();
sakanakuuun 0:d7ff86f25eaa 428 dx=coordinateX()-px;
sakanakuuun 0:d7ff86f25eaa 429 dy=coordinateY()-py;
sakanakuuun 0:d7ff86f25eaa 430 dtheta=coordinateTheta()-ptheta;
sakanakuuun 0:d7ff86f25eaa 431
sakanakuuun 0:d7ff86f25eaa 432 if(dx>7) {
sakanakuuun 0:d7ff86f25eaa 433 dx=7;
sakanakuuun 0:d7ff86f25eaa 434 }
sakanakuuun 0:d7ff86f25eaa 435 if(dx<-7) {
sakanakuuun 0:d7ff86f25eaa 436 dx=-7;
sakanakuuun 0:d7ff86f25eaa 437 }
sakanakuuun 1:405e28b64fdb 438 move(rightspeed - k_x * dx - k_theta * dtheta,leftspeed + k_x *dx + k_theta * dtheta);
sakanakuuun 0:d7ff86f25eaa 439
sakanakuuun 1:405e28b64fdb 440 if(dy<length) {
sakanakuuun 0:d7ff86f25eaa 441 move(0,0);
sakanakuuun 0:d7ff86f25eaa 442 break;
sakanakuuun 0:d7ff86f25eaa 443 }
sakanakuuun 0:d7ff86f25eaa 444 }
sakanakuuun 0:d7ff86f25eaa 445 }
sakanakuuun 1:405e28b64fdb 446 */