Tk A / Move2

aaa

Fork of Move by 涼太郎 中村

move.cpp@5:0e18cf25291a, 2016-09-06 (annotated)

Committer:
sakanakuuun
Date:
Tue Sep 06 09:31:07 2016 +0000
Revision:
5:0e18cf25291a
Parent:
4:4c574be6325c
Child:
6:0aa97a99c9cb
yhh

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 5:0e18cf25291a 20 const int rightspeed=70;
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 5:0e18cf25291a 125 void pmove(int x, int y)
sakanakuuun 5:0e18cf25291a 126 {
sakanakuuun 5:0e18cf25291a 127 float k = 0.9;//ズレ(mm)を回転数に反映させる比例定数
sakanakuuun 5:0e18cf25291a 128
sakanakuuun 5:0e18cf25291a 129 int k_theta = 2;//ズレ(rad)を回転数に反映させる比例定数
sakanakuuun 5:0e18cf25291a 130
sakanakuuun 4:4c574be6325c 131 int length, dx, dy;
sakanakuuun 2:f25a09c5e113 132 int *d_length, *disorder;
sakanakuuun 5:0e18cf25291a 133 int absd_length;
sakanakuuun 4:4c574be6325c 134 float dtheta, ptheta;
sakanakuuun 2:f25a09c5e113 135 float daikei;
sakanakuuun 1:405e28b64fdb 136
sakanakuuun 5:0e18cf25291a 137 char direction;
sakanakuuun 5:0e18cf25291a 138 if(abs(x - coordinateX()) > abs(y - coordinateY())) {
sakanakuuun 5:0e18cf25291a 139 y = coordinateY();
sakanakuuun 5:0e18cf25291a 140 direction = X_PLUS;
sakanakuuun 5:0e18cf25291a 141 length = abs(x - coordinateX());
sakanakuuun 5:0e18cf25291a 142 d_length = &dx;
sakanakuuun 5:0e18cf25291a 143 disorder = &dy;
sakanakuuun 5:0e18cf25291a 144 }
sakanakuuun 5:0e18cf25291a 145 else {
sakanakuuun 5:0e18cf25291a 146 x = coordinateX();
sakanakuuun 5:0e18cf25291a 147 direction = Y_PLUS;
sakanakuuun 5:0e18cf25291a 148 length = abs(y - coordinateY());
sakanakuuun 5:0e18cf25291a 149 d_length = &dy;
sakanakuuun 5:0e18cf25291a 150 disorder = &dx;
sakanakuuun 5:0e18cf25291a 151 }
sakanakuuun 5:0e18cf25291a 152
sakanakuuun 5:0e18cf25291a 153 if(*d_length < 0)
sakanakuuun 5:0e18cf25291a 154 direction *= -1;
sakanakuuun 1:405e28b64fdb 155
sakanakuuun 2:f25a09c5e113 156 switch(direction) {
sakanakuuun 2:f25a09c5e113 157 case X_PLUS:
sakanakuuun 2:f25a09c5e113 158 ptheta = 0;
sakanakuuun 2:f25a09c5e113 159 break;
sakanakuuun 2:f25a09c5e113 160 case Y_PLUS:
sakanakuuun 2:f25a09c5e113 161 k *= -1;
sakanakuuun 2:f25a09c5e113 162 ptheta = PI/2;
sakanakuuun 2:f25a09c5e113 163 break;
sakanakuuun 2:f25a09c5e113 164 case X_MINUS:
sakanakuuun 2:f25a09c5e113 165 k *= -1;
sakanakuuun 2:f25a09c5e113 166 ptheta = PI;
sakanakuuun 2:f25a09c5e113 167 break;
sakanakuuun 2:f25a09c5e113 168 case Y_MINUS:
sakanakuuun 2:f25a09c5e113 169 ptheta = PI*3/2;
sakanakuuun 2:f25a09c5e113 170 break;
sakanakuuun 2:f25a09c5e113 171 default:
sakanakuuun 2:f25a09c5e113 172 return;
sakanakuuun 2:f25a09c5e113 173 }
sakanakuuun 1:405e28b64fdb 174
sakanakuuun 5:0e18cf25291a 175 turn_abs_rad(ptheta);
sakanakuuun 5:0e18cf25291a 176
sakanakuuun 2:f25a09c5e113 177 if(length == 0) return;
sakanakuuun 1:405e28b64fdb 178
sakanakuuun 1:405e28b64fdb 179 while(1) {
sakanakuuun 1:405e28b64fdb 180 update();
sakanakuuun 5:0e18cf25291a 181 dx = x - coordinateX();
sakanakuuun 5:0e18cf25291a 182 dy = y - coordinateY();
sakanakuuun 2:f25a09c5e113 183 dtheta = coordinateTheta() - ptheta;
sakanakuuun 2:f25a09c5e113 184
sakanakuuun 2:f25a09c5e113 185 if(*disorder>1) {
sakanakuuun 2:f25a09c5e113 186 *disorder = 1;
sakanakuuun 2:f25a09c5e113 187 } else if(*disorder<-1) {
sakanakuuun 2:f25a09c5e113 188 *disorder = -1;
sakanakuuun 1:405e28b64fdb 189 }
sakanakuuun 1:405e28b64fdb 190
sakanakuuun 5:0e18cf25291a 191 absd_length = abs(*d_length);
sakanakuuun 1:405e28b64fdb 192
sakanakuuun 5:0e18cf25291a 193 if(absd_length > length - 30) {
sakanakuuun 5:0e18cf25291a 194 daikei = (length - absd_length) / 30.0;
sakanakuuun 5:0e18cf25291a 195 } else if(absd_length < 150) {
sakanakuuun 5:0e18cf25291a 196 daikei = absd_length / 150.0;
sakanakuuun 4:4c574be6325c 197 } else
sakanakuuun 4:4c574be6325c 198 daikei = 1;
sakanakuuun 4:4c574be6325c 199
sakanakuuun 5:0e18cf25291a 200 move(daikei * (rightspeed*6/7.0 + k*(*disorder) - k_theta*dtheta) + rightspeed/7.0,
sakanakuuun 5:0e18cf25291a 201 daikei * (leftspeed*6/7.0 - k*(*disorder) + k_theta*dtheta) + leftspeed/7.0);
sakanakuuun 4:4c574be6325c 202
sakanakuuun 5:0e18cf25291a 203 if((direction > 0 && *d_length <= 0) || (direction < 0 && *d_length >= 0)) {
sakanakuuun 2:f25a09c5e113 204 move(0, 0);
sakanakuuun 1:405e28b64fdb 205 break;
sakanakuuun 1:405e28b64fdb 206 }
sakanakuuun 0:d7ff86f25eaa 207
sakanakuuun 2:f25a09c5e113 208 //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 209 }
sakanakuuun 0:d7ff86f25eaa 210
sakanakuuun 2:f25a09c5e113 211 wait(0.5);
sakanakuuun 0:d7ff86f25eaa 212 }
sakanakuuun 3:cecaa0154f92 213
sakanakuuun 5:0e18cf25291a 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 5:0e18cf25291a 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 5:0e18cf25291a 247 ptheta = 0;
sakanakuuun 4:4c574be6325c 248 break;
sakanakuuun 4:4c574be6325c 249 case Y_PLUS:
sakanakuuun 4:4c574be6325c 250 k *= -1;
sakanakuuun 5:0e18cf25291a 251 ptheta = PI/2;
sakanakuuun 4:4c574be6325c 252 break;
sakanakuuun 4:4c574be6325c 253 case X_MINUS:
sakanakuuun 4:4c574be6325c 254 k *= -1;
sakanakuuun 5:0e18cf25291a 255 ptheta = PI;
sakanakuuun 4:4c574be6325c 256 break;
sakanakuuun 4:4c574be6325c 257 case Y_MINUS:
sakanakuuun 5:0e18cf25291a 258 ptheta = PI*3/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 }