Tk A / Move

fs

Dependents:   ARAI45th 3servotest 1stcomp

move.cpp@8:3bf4addaaedd, 2016-09-07 (annotated)

Committer:
sakanakuuun
Date:
Wed Sep 07 07:36:49 2016 +0000
Revision:
8:3bf4addaaedd
Parent:
7:7f1721542753
Child:
9:d48b89437333
s

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 2:f25a09c5e113 26 const int leftspeed=rightspeed + 2;
sakanakuuun 4:4c574be6325c 27 const int turnspeed=30*2;
sakanakuuun 2:f25a09c5e113 28 const float k = 0.9;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 2:f25a09c5e113 29 const int k_theta = 2;
sakanakuuun 0:d7ff86f25eaa 30 //const float PIfact=2.89;
sakanakuuun 0:d7ff86f25eaa 31
sakanakuuun 0:d7ff86f25eaa 32
sakanakuuun 0:d7ff86f25eaa 33 void initmotor()
sakanakuuun 0:d7ff86f25eaa 34 {
sakanakuuun 0:d7ff86f25eaa 35 M1cw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 36 M1ccw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 37 M2cw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 38 M2ccw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 39
sakanakuuun 0:d7ff86f25eaa 40 }
sakanakuuun 0:d7ff86f25eaa 41
sakanakuuun 0:d7ff86f25eaa 42 void move(int left,int right)
sakanakuuun 0:d7ff86f25eaa 43 {
sakanakuuun 0:d7ff86f25eaa 44
sakanakuuun 0:d7ff86f25eaa 45 float rightduty,leftduty;
sakanakuuun 0:d7ff86f25eaa 46
sakanakuuun 0:d7ff86f25eaa 47 if(right>256) {
sakanakuuun 0:d7ff86f25eaa 48 right=256;
sakanakuuun 0:d7ff86f25eaa 49 }
sakanakuuun 0:d7ff86f25eaa 50 if(left>256) {
sakanakuuun 0:d7ff86f25eaa 51 left=256;
sakanakuuun 0:d7ff86f25eaa 52 }
sakanakuuun 0:d7ff86f25eaa 53 if(right<-256) {
sakanakuuun 0:d7ff86f25eaa 54 right=-256;
sakanakuuun 0:d7ff86f25eaa 55 }
sakanakuuun 0:d7ff86f25eaa 56 if(left<-256) {
sakanakuuun 0:d7ff86f25eaa 57 left=-256;
sakanakuuun 0:d7ff86f25eaa 58 }
sakanakuuun 0:d7ff86f25eaa 59
sakanakuuun 0:d7ff86f25eaa 60 rightduty=right/256.0;
sakanakuuun 0:d7ff86f25eaa 61 leftduty=left/256.0;
sakanakuuun 0:d7ff86f25eaa 62 if(right>0) {
sakanakuuun 0:d7ff86f25eaa 63 M1cw.write(1-rightduty);
sakanakuuun 0:d7ff86f25eaa 64 M1ccw.write(1);
sakanakuuun 0:d7ff86f25eaa 65 } else {
sakanakuuun 0:d7ff86f25eaa 66 M1cw.write(1);
sakanakuuun 0:d7ff86f25eaa 67 M1ccw.write(1+rightduty);
sakanakuuun 0:d7ff86f25eaa 68 }
sakanakuuun 0:d7ff86f25eaa 69
sakanakuuun 0:d7ff86f25eaa 70 if(left>0) {
sakanakuuun 0:d7ff86f25eaa 71 M2cw.write(1-leftduty);
sakanakuuun 0:d7ff86f25eaa 72 M2ccw.write(1);
sakanakuuun 0:d7ff86f25eaa 73 } else {
sakanakuuun 0:d7ff86f25eaa 74 M2cw.write(1);
sakanakuuun 0:d7ff86f25eaa 75 M2ccw.write(1+leftduty);
sakanakuuun 0:d7ff86f25eaa 76 }
sakanakuuun 0:d7ff86f25eaa 77 }
sakanakuuun 0:d7ff86f25eaa 78
sakanakuuun 0:d7ff86f25eaa 79 void hosei_turn(float pt, bool cw, float rad)
sakanakuuun 0:d7ff86f25eaa 80 {
sakanakuuun 0:d7ff86f25eaa 81 int np;
sakanakuuun 0:d7ff86f25eaa 82 if(cw) np = 1;
sakanakuuun 0:d7ff86f25eaa 83 else np = -1;
sakanakuuun 0:d7ff86f25eaa 84 while(1) {
sakanakuuun 0:d7ff86f25eaa 85 update();
sakanakuuun 2:f25a09c5e113 86 //pc.printf("t:%f\n\r", coordinateTheta());
sakanakuuun 0:d7ff86f25eaa 87 if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
sakanakuuun 8:3bf4addaaedd 88 move(-12, 12);
sakanakuuun 0:d7ff86f25eaa 89 } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
sakanakuuun 8:3bf4addaaedd 90 move(12, -12);
sakanakuuun 0:d7ff86f25eaa 91 } else {
sakanakuuun 0:d7ff86f25eaa 92 move(0,0);
sakanakuuun 0:d7ff86f25eaa 93 return;
sakanakuuun 0:d7ff86f25eaa 94 }
sakanakuuun 0:d7ff86f25eaa 95 }
sakanakuuun 2:f25a09c5e113 96
sakanakuuun 0:d7ff86f25eaa 97 }
sakanakuuun 0:d7ff86f25eaa 98
sakanakuuun 1:405e28b64fdb 99 void turn_abs_rad(float rad)
sakanakuuun 0:d7ff86f25eaa 100 {
sakanakuuun 7:7f1721542753 101 green = 1;
sakanakuuun 7:7f1721542753 102
sakanakuuun 1:405e28b64fdb 103 update();
sakanakuuun 0:d7ff86f25eaa 104
sakanakuuun 1:405e28b64fdb 105 int np;
sakanakuuun 1:405e28b64fdb 106 if(coordinateTheta() > rad) np = 1;
sakanakuuun 1:405e28b64fdb 107 else if(coordinateTheta() < rad) np = -1;
sakanakuuun 1:405e28b64fdb 108 else return;
sakanakuuun 1:405e28b64fdb 109
sakanakuuun 8:3bf4addaaedd 110 move((-np)*(turnspeed+2), (np)*turnspeed);
sakanakuuun 0:d7ff86f25eaa 111
sakanakuuun 0:d7ff86f25eaa 112 while(1) {
sakanakuuun 0:d7ff86f25eaa 113 update();
sakanakuuun 2:f25a09c5e113 114 //pc.printf("t:%f\n\r", coordinateTheta());
sakanakuuun 2:f25a09c5e113 115 if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
sakanakuuun 0:d7ff86f25eaa 116 move(0,0);
sakanakuuun 0:d7ff86f25eaa 117 break;
sakanakuuun 0:d7ff86f25eaa 118 }
sakanakuuun 0:d7ff86f25eaa 119 }
sakanakuuun 0:d7ff86f25eaa 120
sakanakuuun 1:405e28b64fdb 121
sakanakuuun 1:405e28b64fdb 122
sakanakuuun 2:f25a09c5e113 123 hosei_turn(0, false, rad);
sakanakuuun 1:405e28b64fdb 124
sakanakuuun 2:f25a09c5e113 125 wait(0.5);
sakanakuuun 1:405e28b64fdb 126
sakanakuuun 2:f25a09c5e113 127 hosei_turn(0, false, rad);
sakanakuuun 1:405e28b64fdb 128
sakanakuuun 2:f25a09c5e113 129 wait(0.5);
sakanakuuun 7:7f1721542753 130 green = 0;
sakanakuuun 7:7f1721542753 131
sakanakuuun 1:405e28b64fdb 132 }
sakanakuuun 1:405e28b64fdb 133
sakanakuuun 5:0e18cf25291a 134 void pmove(int x, int y)
sakanakuuun 7:7f1721542753 135 {
sakanakuuun 7:7f1721542753 136 yellow = 1;
sakanakuuun 7:7f1721542753 137
sakanakuuun 5:0e18cf25291a 138 float k = 0.9;//ズレ(mm)を回転数に反映させる比例定数
sakanakuuun 7:7f1721542753 139
sakanakuuun 5:0e18cf25291a 140 int k_theta = 2;//ズレ(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 5:0e18cf25291a 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:
sakanakuuun 2:f25a09c5e113 186 ptheta = PI*3/2;
sakanakuuun 2:f25a09c5e113 187 break;
sakanakuuun 2:f25a09c5e113 188 default:
sakanakuuun 2:f25a09c5e113 189 return;
sakanakuuun 2:f25a09c5e113 190 }
sakanakuuun 1:405e28b64fdb 191
sakanakuuun 5:0e18cf25291a 192 turn_abs_rad(ptheta);
sakanakuuun 5:0e18cf25291a 193
sakanakuuun 2:f25a09c5e113 194 if(length == 0) return;
sakanakuuun 1:405e28b64fdb 195
sakanakuuun 1:405e28b64fdb 196 while(1) {
sakanakuuun 7:7f1721542753 197 update_np();
sakanakuuun 5:0e18cf25291a 198 dx = x - coordinateX();
sakanakuuun 5:0e18cf25291a 199 dy = y - coordinateY();
sakanakuuun 2:f25a09c5e113 200 dtheta = coordinateTheta() - ptheta;
sakanakuuun 2:f25a09c5e113 201
sakanakuuun 7:7f1721542753 202 if(*disorder>2) {
sakanakuuun 7:7f1721542753 203 *disorder = 2;
sakanakuuun 7:7f1721542753 204 } else if(*disorder<-2) {
sakanakuuun 7:7f1721542753 205 *disorder = -2;
sakanakuuun 1:405e28b64fdb 206 }
sakanakuuun 1:405e28b64fdb 207
sakanakuuun 5:0e18cf25291a 208 absd_length = abs(*d_length);
sakanakuuun 1:405e28b64fdb 209
sakanakuuun 5:0e18cf25291a 210 if(absd_length > length - 30) {
sakanakuuun 7:7f1721542753 211 daikei = abs(length - absd_length) / 30.0;
sakanakuuun 5:0e18cf25291a 212 } else if(absd_length < 150) {
sakanakuuun 5:0e18cf25291a 213 daikei = absd_length / 150.0;
sakanakuuun 4:4c574be6325c 214 } else
sakanakuuun 4:4c574be6325c 215 daikei = 1;
sakanakuuun 4:4c574be6325c 216
sakanakuuun 5:0e18cf25291a 217 move(daikei * (rightspeed*6/7.0 + k*(*disorder) - k_theta*dtheta) + rightspeed/7.0,
sakanakuuun 5:0e18cf25291a 218 daikei * (leftspeed*6/7.0 - k*(*disorder) + k_theta*dtheta) + leftspeed/7.0);
sakanakuuun 4:4c574be6325c 219
sakanakuuun 7:7f1721542753 220 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 221 if((direction > 0 && *d_length <= 0) || (direction < 0 && *d_length >= 0)) {
sakanakuuun 2:f25a09c5e113 222 move(0, 0);
sakanakuuun 1:405e28b64fdb 223 break;
sakanakuuun 1:405e28b64fdb 224 }
sakanakuuun 0:d7ff86f25eaa 225
sakanakuuun 0:d7ff86f25eaa 226 }
sakanakuuun 0:d7ff86f25eaa 227
sakanakuuun 2:f25a09c5e113 228 wait(0.5);
sakanakuuun 7:7f1721542753 229
sakanakuuun 7:7f1721542753 230 yellow = 0;
sakanakuuun 0:d7ff86f25eaa 231 }
sakanakuuun 3:cecaa0154f92 232
sakanakuuun 5:0e18cf25291a 233
sakanakuuun 6:0aa97a99c9cb 234 void back300()
sakanakuuun 7:7f1721542753 235 {
sakanakuuun 7:7f1721542753 236 red = 1;
sakanakuuun 7:7f1721542753 237
sakanakuuun 4:4c574be6325c 238 float k = 0.9;
sakanakuuun 4:4c574be6325c 239 int k_theta = 2;
sakanakuuun 4:4c574be6325c 240
sakanakuuun 6:0aa97a99c9cb 241 int length, px, py, dx, dy;
sakanakuuun 4:4c574be6325c 242 float daikei;
sakanakuuun 7:7f1721542753 243
sakanakuuun 6:0aa97a99c9cb 244 update();
sakanakuuun 7:7f1721542753 245
sakanakuuun 7:7f1721542753 246 px = coordinateX();
sakanakuuun 7:7f1721542753 247 py = coordinateY();
sakanakuuun 7:7f1721542753 248
sakanakuuun 6:0aa97a99c9cb 249 length = 300;
sakanakuuun 7:7f1721542753 250
sakanakuuun 4:4c574be6325c 251 if(length == 0) return;
sakanakuuun 4:4c574be6325c 252
sakanakuuun 6:0aa97a99c9cb 253 turn_abs_rad(PI);
sakanakuuun 7:7f1721542753 254
sakanakuuun 4:4c574be6325c 255 while(1) {
sakanakuuun 4:4c574be6325c 256 update();
sakanakuuun 6:0aa97a99c9cb 257 dx = coordinateX() - px;
sakanakuuun 6:0aa97a99c9cb 258 dy = coordinateY() - py;
sakanakuuun 4:4c574be6325c 259
sakanakuuun 6:0aa97a99c9cb 260 if(dy>2) {
sakanakuuun 6:0aa97a99c9cb 261 dy = 2;
sakanakuuun 6:0aa97a99c9cb 262 } else if(dy<-2) {
sakanakuuun 6:0aa97a99c9cb 263 dy = -2;
sakanakuuun 6:0aa97a99c9cb 264 }
sakanakuuun 7:7f1721542753 265
sakanakuuun 4:4c574be6325c 266
sakanakuuun 8:3bf4addaaedd 267 move(-(30 + k*dy), -(32 - k*dy));
sakanakuuun 4:4c574be6325c 268
sakanakuuun 7:7f1721542753 269
sakanakuuun 6:0aa97a99c9cb 270 if(dx>length) {
sakanakuuun 4:4c574be6325c 271 move(0, 0);
sakanakuuun 4:4c574be6325c 272 break;
sakanakuuun 4:4c574be6325c 273 }
sakanakuuun 4:4c574be6325c 274
sakanakuuun 4:4c574be6325c 275 //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
sakanakuuun 4:4c574be6325c 276 }
sakanakuuun 4:4c574be6325c 277
sakanakuuun 4:4c574be6325c 278 wait(0.5);
sakanakuuun 7:7f1721542753 279
sakanakuuun 7:7f1721542753 280 red = 0;
sakanakuuun 4:4c574be6325c 281 }