Tk A / Move2

aaa

Fork of Move by 涼太郎 中村

move.cpp@14:db58d3b0ecde, 2016-09-10 (annotated)

Committer:
sakanakuuun
Date:
Sat Sep 10 03:15:19 2016 +0000
Revision:
14:db58d3b0ecde
Parent:
13:176e543e6d64
Child:
15:403e9c57c1a1
a

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