Tk A / Move2

aaa

Fork of Move by 涼太郎 中村

move.cpp@4:4c574be6325c, 2016-09-06 (annotated)

Committer:
sakanakuuun
Date:
Tue Sep 06 06:47:25 2016 +0000
Revision:
4:4c574be6325c
Parent:
3:cecaa0154f92
Child:
5:0e18cf25291a
hj

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