Tk A / Move

fs

Dependents:   ARAI45th 3servotest 1stcomp

move.cpp@10:5079a169b752, 2016-09-09 (annotated)

Committer:
sakanakuuun
Date:
Fri Sep 09 11:44:04 2016 +0000
Revision:
10:5079a169b752
Parent:
9:d48b89437333 
afs;

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 2:f25a09c5e113 4 #include "stdlib.h"
sakanakuuun 0:d7ff86f25eaa 5
sakanakuuun 0:d7ff86f25eaa 6 PwmOut M1cw(PA_11);
sakanakuuun 0:d7ff86f25eaa 7 PwmOut M1ccw(PB_15);
sakanakuuun 0:d7ff86f25eaa 8 PwmOut M2ccw(PB_14);
sakanakuuun 0:d7ff86f25eaa 9 PwmOut M2cw(PB_13);
sakanakuuun 7:7f1721542753 10
sakanakuuun 7:7f1721542753 11 DigitalOut green (PC_2);
sakanakuuun 7:7f1721542753 12 DigitalOut yellow(PC_3);
sakanakuuun 7:7f1721542753 13 DigitalOut red (PC_0);
sakanakuuun 7:7f1721542753 14
sakanakuuun 2:f25a09c5e113 15 /*
sakanakuuun 2:f25a09c5e113 16 DigitalOut teamledblue(PC_10);
sakanakuuun 2:f25a09c5e113 17 DigitalOut teamledred(PC_12);
sakanakuuun 2:f25a09c5e113 18 */
sakanakuuun 2:f25a09c5e113 19
sakanakuuun 7:7f1721542753 20 Serial pc2(SERIAL_TX, SERIAL_RX); //pcと通信
sakanakuuun 2:f25a09c5e113 21
sakanakuuun 0:d7ff86f25eaa 22
sakanakuuun 0:d7ff86f25eaa 23 //const int allowlength=5;
sakanakuuun 0:d7ff86f25eaa 24 //const float allowdegree=0.02;
sakanakuuun 5:0e18cf25291a 25 const int rightspeed=70;
sakanakuuun 10:5079a169b752 26 const int leftspeed=rightspeed + 3;
sakanakuuun 4:4c574be6325c 27 const int turnspeed=30*2;
sakanakuuun 10:5079a169b752 28 const int hosei_turnspeed = 13;
sakanakuuun 10:5079a169b752 29 const float k = 1;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 2:f25a09c5e113 30 const int k_theta = 2;
sakanakuuun 10:5079a169b752 31 const int max_disorder = 4;
sakanakuuun 0:d7ff86f25eaa 32 //const float PIfact=2.89;
sakanakuuun 0:d7ff86f25eaa 33
sakanakuuun 0:d7ff86f25eaa 34
sakanakuuun 0:d7ff86f25eaa 35 void initmotor()
sakanakuuun 0:d7ff86f25eaa 36 {
sakanakuuun 0:d7ff86f25eaa 37 M1cw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 38 M1ccw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 39 M2cw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 40 M2ccw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 41
sakanakuuun 0:d7ff86f25eaa 42 }
sakanakuuun 0:d7ff86f25eaa 43
sakanakuuun 0:d7ff86f25eaa 44 void move(int left,int right)
sakanakuuun 0:d7ff86f25eaa 45 {
sakanakuuun 0:d7ff86f25eaa 46
sakanakuuun 0:d7ff86f25eaa 47 float rightduty,leftduty;
sakanakuuun 0:d7ff86f25eaa 48
sakanakuuun 0:d7ff86f25eaa 49 if(right>256) {
sakanakuuun 0:d7ff86f25eaa 50 right=256;
sakanakuuun 0:d7ff86f25eaa 51 }
sakanakuuun 0:d7ff86f25eaa 52 if(left>256) {
sakanakuuun 0:d7ff86f25eaa 53 left=256;
sakanakuuun 0:d7ff86f25eaa 54 }
sakanakuuun 0:d7ff86f25eaa 55 if(right<-256) {
sakanakuuun 0:d7ff86f25eaa 56 right=-256;
sakanakuuun 0:d7ff86f25eaa 57 }
sakanakuuun 0:d7ff86f25eaa 58 if(left<-256) {
sakanakuuun 0:d7ff86f25eaa 59 left=-256;
sakanakuuun 0:d7ff86f25eaa 60 }
sakanakuuun 0:d7ff86f25eaa 61
sakanakuuun 0:d7ff86f25eaa 62 rightduty=right/256.0;
sakanakuuun 0:d7ff86f25eaa 63 leftduty=left/256.0;
sakanakuuun 0:d7ff86f25eaa 64 if(right>0) {
sakanakuuun 0:d7ff86f25eaa 65 M1cw.write(1-rightduty);
sakanakuuun 0:d7ff86f25eaa 66 M1ccw.write(1);
sakanakuuun 0:d7ff86f25eaa 67 } else {
sakanakuuun 0:d7ff86f25eaa 68 M1cw.write(1);
sakanakuuun 0:d7ff86f25eaa 69 M1ccw.write(1+rightduty);
sakanakuuun 0:d7ff86f25eaa 70 }
sakanakuuun 0:d7ff86f25eaa 71
sakanakuuun 0:d7ff86f25eaa 72 if(left>0) {
sakanakuuun 0:d7ff86f25eaa 73 M2cw.write(1-leftduty);
sakanakuuun 0:d7ff86f25eaa 74 M2ccw.write(1);
sakanakuuun 0:d7ff86f25eaa 75 } else {
sakanakuuun 0:d7ff86f25eaa 76 M2cw.write(1);
sakanakuuun 0:d7ff86f25eaa 77 M2ccw.write(1+leftduty);
sakanakuuun 0:d7ff86f25eaa 78 }
sakanakuuun 0:d7ff86f25eaa 79 }
sakanakuuun 0:d7ff86f25eaa 80
sakanakuuun 0:d7ff86f25eaa 81 void hosei_turn(float pt, bool cw, float rad)
sakanakuuun 0:d7ff86f25eaa 82 {
sakanakuuun 0:d7ff86f25eaa 83 int np;
sakanakuuun 0:d7ff86f25eaa 84 if(cw) np = 1;
sakanakuuun 0:d7ff86f25eaa 85 else np = -1;
sakanakuuun 0:d7ff86f25eaa 86 while(1) {
sakanakuuun 0:d7ff86f25eaa 87 update();
sakanakuuun 2:f25a09c5e113 88 //pc.printf("t:%f\n\r", coordinateTheta());
sakanakuuun 0:d7ff86f25eaa 89 if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
sakanakuuun 9:d48b89437333 90 move(-hosei_turnspeed, hosei_turnspeed);
sakanakuuun 0:d7ff86f25eaa 91 } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
sakanakuuun 9:d48b89437333 92 move(hosei_turnspeed, -hosei_turnspeed);
sakanakuuun 0:d7ff86f25eaa 93 } else {
sakanakuuun 0:d7ff86f25eaa 94 move(0,0);
sakanakuuun 0:d7ff86f25eaa 95 return;
sakanakuuun 0:d7ff86f25eaa 96 }
sakanakuuun 0:d7ff86f25eaa 97 }
sakanakuuun 2:f25a09c5e113 98
sakanakuuun 0:d7ff86f25eaa 99 }
sakanakuuun 0:d7ff86f25eaa 100
sakanakuuun 1:405e28b64fdb 101 void turn_abs_rad(float rad)
sakanakuuun 0:d7ff86f25eaa 102 {
sakanakuuun 7:7f1721542753 103 green = 1;
sakanakuuun 7:7f1721542753 104
sakanakuuun 1:405e28b64fdb 105 update();
sakanakuuun 0:d7ff86f25eaa 106
sakanakuuun 1:405e28b64fdb 107 int np;
sakanakuuun 1:405e28b64fdb 108 if(coordinateTheta() > rad) np = 1;
sakanakuuun 1:405e28b64fdb 109 else if(coordinateTheta() < rad) np = -1;
sakanakuuun 1:405e28b64fdb 110 else return;
sakanakuuun 1:405e28b64fdb 111
sakanakuuun 8:3bf4addaaedd 112 move((-np)*(turnspeed+2), (np)*turnspeed);
sakanakuuun 0:d7ff86f25eaa 113
sakanakuuun 0:d7ff86f25eaa 114 while(1) {
sakanakuuun 0:d7ff86f25eaa 115 update();
sakanakuuun 2:f25a09c5e113 116 //pc.printf("t:%f\n\r", coordinateTheta());
sakanakuuun 2:f25a09c5e113 117 if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
sakanakuuun 0:d7ff86f25eaa 118 move(0,0);
sakanakuuun 0:d7ff86f25eaa 119 break;
sakanakuuun 0:d7ff86f25eaa 120 }
sakanakuuun 0:d7ff86f25eaa 121 }
sakanakuuun 0:d7ff86f25eaa 122
sakanakuuun 1:405e28b64fdb 123
sakanakuuun 1:405e28b64fdb 124
sakanakuuun 2:f25a09c5e113 125 hosei_turn(0, false, rad);
sakanakuuun 1:405e28b64fdb 126
sakanakuuun 2:f25a09c5e113 127 wait(0.5);
sakanakuuun 1:405e28b64fdb 128
sakanakuuun 2:f25a09c5e113 129 hosei_turn(0, false, rad);
sakanakuuun 1:405e28b64fdb 130
sakanakuuun 2:f25a09c5e113 131 wait(0.5);
sakanakuuun 7:7f1721542753 132 green = 0;
sakanakuuun 7:7f1721542753 133
sakanakuuun 1:405e28b64fdb 134 }
sakanakuuun 1:405e28b64fdb 135
sakanakuuun 5:0e18cf25291a 136 void pmove(int x, int y)
sakanakuuun 7:7f1721542753 137 {
sakanakuuun 7:7f1721542753 138 yellow = 1;
sakanakuuun 7:7f1721542753 139
sakanakuuun 5:0e18cf25291a 140 float k = 0.9;//ズレ(mm)を回転数に反映させる比例定数
sakanakuuun 7:7f1721542753 141
sakanakuuun 5:0e18cf25291a 142 int k_theta = 2;//ズレ(rad)を回転数に反映させる比例定数
sakanakuuun 7:7f1721542753 143
sakanakuuun 4:4c574be6325c 144 int length, dx, dy;
sakanakuuun 2:f25a09c5e113 145 int *d_length, *disorder;
sakanakuuun 5:0e18cf25291a 146 int absd_length;
sakanakuuun 4:4c574be6325c 147 float dtheta, ptheta;
sakanakuuun 2:f25a09c5e113 148 float daikei;
sakanakuuun 1:405e28b64fdb 149
sakanakuuun 7:7f1721542753 150 int direction;
sakanakuuun 7:7f1721542753 151
sakanakuuun 5:0e18cf25291a 152 if(abs(x - coordinateX()) > abs(y - coordinateY())) {
sakanakuuun 5:0e18cf25291a 153 y = coordinateY();
sakanakuuun 5:0e18cf25291a 154 direction = X_PLUS;
sakanakuuun 5:0e18cf25291a 155 length = abs(x - coordinateX());
sakanakuuun 5:0e18cf25291a 156 d_length = &dx;
sakanakuuun 5:0e18cf25291a 157 disorder = &dy;
sakanakuuun 7:7f1721542753 158 } else {
sakanakuuun 5:0e18cf25291a 159 x = coordinateX();
sakanakuuun 5:0e18cf25291a 160 direction = Y_PLUS;
sakanakuuun 5:0e18cf25291a 161 length = abs(y - coordinateY());
sakanakuuun 5:0e18cf25291a 162 d_length = &dy;
sakanakuuun 5:0e18cf25291a 163 disorder = &dx;
sakanakuuun 5:0e18cf25291a 164 }
sakanakuuun 7:7f1721542753 165
sakanakuuun 7:7f1721542753 166 update();
sakanakuuun 7:7f1721542753 167 dx = x - coordinateX();
sakanakuuun 7:7f1721542753 168 dy = y - coordinateY();
sakanakuuun 9:d48b89437333 169
sakanakuuun 7:7f1721542753 170 if(*d_length < 0) //x,y減少方向なら、*d_length<0
sakanakuuun 5:0e18cf25291a 171 direction *= -1;
sakanakuuun 1:405e28b64fdb 172
sakanakuuun 7:7f1721542753 173 pc2.printf("direction:%d", direction);
sakanakuuun 7:7f1721542753 174
sakanakuuun 2:f25a09c5e113 175 switch(direction) {
sakanakuuun 2:f25a09c5e113 176 case X_PLUS:
sakanakuuun 2:f25a09c5e113 177 ptheta = 0;
sakanakuuun 2:f25a09c5e113 178 break;
sakanakuuun 2:f25a09c5e113 179 case Y_PLUS:
sakanakuuun 2:f25a09c5e113 180 k *= -1;
sakanakuuun 2:f25a09c5e113 181 ptheta = PI/2;
sakanakuuun 2:f25a09c5e113 182 break;
sakanakuuun 2:f25a09c5e113 183 case X_MINUS:
sakanakuuun 2:f25a09c5e113 184 k *= -1;
sakanakuuun 2:f25a09c5e113 185 ptheta = PI;
sakanakuuun 2:f25a09c5e113 186 break;
sakanakuuun 2:f25a09c5e113 187 case Y_MINUS:
sakanakuuun 9:d48b89437333 188 ptheta = -PI/2;
sakanakuuun 2:f25a09c5e113 189 break;
sakanakuuun 2:f25a09c5e113 190 default:
sakanakuuun 2:f25a09c5e113 191 return;
sakanakuuun 2:f25a09c5e113 192 }
sakanakuuun 9:d48b89437333 193
sakanakuuun 10:5079a169b752 194 ptheta += nearPi(coordinateTheta() - ptheta);
sakanakuuun 9:d48b89437333 195
sakanakuuun 5:0e18cf25291a 196 turn_abs_rad(ptheta);
sakanakuuun 5:0e18cf25291a 197
sakanakuuun 2:f25a09c5e113 198 if(length == 0) return;
sakanakuuun 1:405e28b64fdb 199
sakanakuuun 1:405e28b64fdb 200 while(1) {
sakanakuuun 9:d48b89437333 201 update();
sakanakuuun 5:0e18cf25291a 202 dx = x - coordinateX();
sakanakuuun 5:0e18cf25291a 203 dy = y - coordinateY();
sakanakuuun 2:f25a09c5e113 204 dtheta = coordinateTheta() - ptheta;
sakanakuuun 2:f25a09c5e113 205
sakanakuuun 9:d48b89437333 206 if(*disorder>max_disorder) {
sakanakuuun 9:d48b89437333 207 *disorder = max_disorder;
sakanakuuun 9:d48b89437333 208 } else if(*disorder<-max_disorder) {
sakanakuuun 9:d48b89437333 209 *disorder = -max_disorder;
sakanakuuun 1:405e28b64fdb 210 }
sakanakuuun 1:405e28b64fdb 211
sakanakuuun 5:0e18cf25291a 212 absd_length = abs(*d_length);
sakanakuuun 1:405e28b64fdb 213
sakanakuuun 10:5079a169b752 214 if(absd_length < 300) {
sakanakuuun 10:5079a169b752 215 daikei = absd_length / 300.0;
sakanakuuun 10:5079a169b752 216 } else if(absd_length > length - 30) {
sakanakuuun 7:7f1721542753 217 daikei = abs(length - absd_length) / 30.0;
sakanakuuun 4:4c574be6325c 218 } else
sakanakuuun 4:4c574be6325c 219 daikei = 1;
sakanakuuun 4:4c574be6325c 220
sakanakuuun 10:5079a169b752 221 move(daikei * (rightspeed*6/7.0 + k*(*disorder) - k_theta*dtheta) + rightspeed/7.0,
sakanakuuun 10:5079a169b752 222 daikei * (leftspeed*6/7.0 - k*(*disorder) + k_theta*dtheta) + leftspeed/7.0);
sakanakuuun 4:4c574be6325c 223
sakanakuuun 9:d48b89437333 224 //pc2.printf("d_length:%d disorder:%d rs:%f ls:%f daikei:%f\n\r", *d_length, *disorder, k*(*disorder) - k_theta*dtheta, -k*(*disorder) + k_theta*dtheta, daikei);
sakanakuuun 5:0e18cf25291a 225 if((direction > 0 && *d_length <= 0) || (direction < 0 && *d_length >= 0)) {
sakanakuuun 2:f25a09c5e113 226 move(0, 0);
sakanakuuun 1:405e28b64fdb 227 break;
sakanakuuun 1:405e28b64fdb 228 }
sakanakuuun 0:d7ff86f25eaa 229
sakanakuuun 0:d7ff86f25eaa 230 }
sakanakuuun 0:d7ff86f25eaa 231
sakanakuuun 2:f25a09c5e113 232 wait(0.5);
sakanakuuun 7:7f1721542753 233
sakanakuuun 7:7f1721542753 234 yellow = 0;
sakanakuuun 0:d7ff86f25eaa 235 }
sakanakuuun 3:cecaa0154f92 236
sakanakuuun 5:0e18cf25291a 237
sakanakuuun 6:0aa97a99c9cb 238 void back300()
sakanakuuun 7:7f1721542753 239 {
sakanakuuun 7:7f1721542753 240 red = 1;
sakanakuuun 7:7f1721542753 241
sakanakuuun 4:4c574be6325c 242 float k = 0.9;
sakanakuuun 4:4c574be6325c 243 int k_theta = 2;
sakanakuuun 4:4c574be6325c 244
sakanakuuun 6:0aa97a99c9cb 245 int length, px, py, dx, dy;
sakanakuuun 4:4c574be6325c 246 float daikei;
sakanakuuun 7:7f1721542753 247
sakanakuuun 6:0aa97a99c9cb 248 update();
sakanakuuun 7:7f1721542753 249
sakanakuuun 7:7f1721542753 250 px = coordinateX();
sakanakuuun 7:7f1721542753 251 py = coordinateY();
sakanakuuun 7:7f1721542753 252
sakanakuuun 6:0aa97a99c9cb 253 length = 300;
sakanakuuun 7:7f1721542753 254
sakanakuuun 4:4c574be6325c 255 if(length == 0) return;
sakanakuuun 4:4c574be6325c 256
sakanakuuun 6:0aa97a99c9cb 257 turn_abs_rad(PI);
sakanakuuun 7:7f1721542753 258
sakanakuuun 4:4c574be6325c 259 while(1) {
sakanakuuun 4:4c574be6325c 260 update();
sakanakuuun 6:0aa97a99c9cb 261 dx = coordinateX() - px;
sakanakuuun 6:0aa97a99c9cb 262 dy = coordinateY() - py;
sakanakuuun 4:4c574be6325c 263
sakanakuuun 9:d48b89437333 264 if(dy>max_disorder) {
sakanakuuun 9:d48b89437333 265 dy = max_disorder;
sakanakuuun 9:d48b89437333 266 } else if(dy<-max_disorder) {
sakanakuuun 9:d48b89437333 267 dy = -max_disorder;
sakanakuuun 6:0aa97a99c9cb 268 }
sakanakuuun 7:7f1721542753 269
sakanakuuun 4:4c574be6325c 270
sakanakuuun 8:3bf4addaaedd 271 move(-(30 + k*dy), -(32 - k*dy));
sakanakuuun 4:4c574be6325c 272
sakanakuuun 7:7f1721542753 273
sakanakuuun 6:0aa97a99c9cb 274 if(dx>length) {
sakanakuuun 4:4c574be6325c 275 move(0, 0);
sakanakuuun 4:4c574be6325c 276 break;
sakanakuuun 4:4c574be6325c 277 }
sakanakuuun 4:4c574be6325c 278
sakanakuuun 4:4c574be6325c 279 //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
sakanakuuun 4:4c574be6325c 280 }
sakanakuuun 4:4c574be6325c 281
sakanakuuun 4:4c574be6325c 282 wait(0.5);
sakanakuuun 7:7f1721542753 283
sakanakuuun 7:7f1721542753 284 red = 0;
sakanakuuun 4:4c574be6325c 285 }
sakanakuuun 9:d48b89437333 286
sakanakuuun 9:d48b89437333 287 float nearPi(float rad)
sakanakuuun 9:d48b89437333 288 {
sakanakuuun 9:d48b89437333 289 float npi = 0;
sakanakuuun 9:d48b89437333 290
sakanakuuun 9:d48b89437333 291 while(1)
sakanakuuun 9:d48b89437333 292 {
sakanakuuun 9:d48b89437333 293 if(rad > npi + PI)
sakanakuuun 9:d48b89437333 294 npi += 2*PI;
sakanakuuun 9:d48b89437333 295 else if(rad < npi - PI)
sakanakuuun 9:d48b89437333 296 npi -= 2*PI;
sakanakuuun 9:d48b89437333 297 else
sakanakuuun 9:d48b89437333 298 return npi;
sakanakuuun 9:d48b89437333 299 }
sakanakuuun 9:d48b89437333 300 }