Tk A / Move

fs

Dependents:   ARAI45th 3servotest 1stcomp

move.cpp@0:d7ff86f25eaa, 2016-09-02 (annotated)

Committer:
sakanakuuun
Date:
Fri Sep 02 12:40:45 2016 +0000
Revision:
0:d7ff86f25eaa
Child:
1:405e28b64fdb
fs

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 0:d7ff86f25eaa 4
sakanakuuun 0:d7ff86f25eaa 5 PwmOut M1cw(PA_11);
sakanakuuun 0:d7ff86f25eaa 6 PwmOut M1ccw(PB_15);
sakanakuuun 0:d7ff86f25eaa 7 PwmOut M2ccw(PB_14);
sakanakuuun 0:d7ff86f25eaa 8 PwmOut M2cw(PB_13);
sakanakuuun 0:d7ff86f25eaa 9
sakanakuuun 0:d7ff86f25eaa 10 //const int allowlength=5;
sakanakuuun 0:d7ff86f25eaa 11 //const float allowdegree=0.02;
sakanakuuun 0:d7ff86f25eaa 12 const int rightspeed=29*2;
sakanakuuun 0:d7ff86f25eaa 13 const int leftspeed=31*2;
sakanakuuun 0:d7ff86f25eaa 14 const int turnspeed=15*2;
sakanakuuun 0:d7ff86f25eaa 15
sakanakuuun 0:d7ff86f25eaa 16 //const float PIfact=2.89;
sakanakuuun 0:d7ff86f25eaa 17
sakanakuuun 0:d7ff86f25eaa 18
sakanakuuun 0:d7ff86f25eaa 19 void initmotor()
sakanakuuun 0:d7ff86f25eaa 20 {
sakanakuuun 0:d7ff86f25eaa 21 M1cw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 22 M1ccw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 23 M2cw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 24 M2ccw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 25
sakanakuuun 0:d7ff86f25eaa 26 }
sakanakuuun 0:d7ff86f25eaa 27
sakanakuuun 0:d7ff86f25eaa 28 void move(int left,int right)
sakanakuuun 0:d7ff86f25eaa 29 {
sakanakuuun 0:d7ff86f25eaa 30
sakanakuuun 0:d7ff86f25eaa 31 float rightduty,leftduty;
sakanakuuun 0:d7ff86f25eaa 32
sakanakuuun 0:d7ff86f25eaa 33 if(right>256) {
sakanakuuun 0:d7ff86f25eaa 34 right=256;
sakanakuuun 0:d7ff86f25eaa 35 }
sakanakuuun 0:d7ff86f25eaa 36 if(left>256) {
sakanakuuun 0:d7ff86f25eaa 37 left=256;
sakanakuuun 0:d7ff86f25eaa 38 }
sakanakuuun 0:d7ff86f25eaa 39 if(right<-256) {
sakanakuuun 0:d7ff86f25eaa 40 right=-256;
sakanakuuun 0:d7ff86f25eaa 41 }
sakanakuuun 0:d7ff86f25eaa 42 if(left<-256) {
sakanakuuun 0:d7ff86f25eaa 43 left=-256;
sakanakuuun 0:d7ff86f25eaa 44 }
sakanakuuun 0:d7ff86f25eaa 45
sakanakuuun 0:d7ff86f25eaa 46 rightduty=right/256.0;
sakanakuuun 0:d7ff86f25eaa 47 leftduty=left/256.0;
sakanakuuun 0:d7ff86f25eaa 48 if(right>0) {
sakanakuuun 0:d7ff86f25eaa 49 M1cw.write(1-rightduty);
sakanakuuun 0:d7ff86f25eaa 50 M1ccw.write(1);
sakanakuuun 0:d7ff86f25eaa 51 } else {
sakanakuuun 0:d7ff86f25eaa 52 M1cw.write(1);
sakanakuuun 0:d7ff86f25eaa 53 M1ccw.write(1+rightduty);
sakanakuuun 0:d7ff86f25eaa 54 }
sakanakuuun 0:d7ff86f25eaa 55
sakanakuuun 0:d7ff86f25eaa 56 if(left>0) {
sakanakuuun 0:d7ff86f25eaa 57 M2cw.write(1-leftduty);
sakanakuuun 0:d7ff86f25eaa 58 M2ccw.write(1);
sakanakuuun 0:d7ff86f25eaa 59 } else {
sakanakuuun 0:d7ff86f25eaa 60 M2cw.write(1);
sakanakuuun 0:d7ff86f25eaa 61 M2ccw.write(1+leftduty);
sakanakuuun 0:d7ff86f25eaa 62 }
sakanakuuun 0:d7ff86f25eaa 63 }
sakanakuuun 0:d7ff86f25eaa 64
sakanakuuun 0:d7ff86f25eaa 65
sakanakuuun 0:d7ff86f25eaa 66 void movelength(int length)
sakanakuuun 0:d7ff86f25eaa 67 {
sakanakuuun 0:d7ff86f25eaa 68 int px,py;
sakanakuuun 0:d7ff86f25eaa 69 update();
sakanakuuun 0:d7ff86f25eaa 70 px=coordinateX();
sakanakuuun 0:d7ff86f25eaa 71 py=coordinateY();
sakanakuuun 0:d7ff86f25eaa 72 //int pt=coordinateTheta();
sakanakuuun 0:d7ff86f25eaa 73
sakanakuuun 0:d7ff86f25eaa 74 move(rightspeed,leftspeed);
sakanakuuun 0:d7ff86f25eaa 75 while(1) {
sakanakuuun 0:d7ff86f25eaa 76
sakanakuuun 0:d7ff86f25eaa 77 update();
sakanakuuun 0:d7ff86f25eaa 78 //pc.printf("dx:%d, dy:%d, l:%d x:%d y:%d t:%f\n\r",px-coordinateX(),py-coordinateY(),length,coordinateX(),coordinateY(), coordinateTheta());
sakanakuuun 0:d7ff86f25eaa 79 if(((px-coordinateX())*(px-coordinateX())+(py-coordinateY())*(py-coordinateY()))>length*length) {
sakanakuuun 0:d7ff86f25eaa 80 break;
sakanakuuun 0:d7ff86f25eaa 81 }
sakanakuuun 0:d7ff86f25eaa 82
sakanakuuun 0:d7ff86f25eaa 83 }
sakanakuuun 0:d7ff86f25eaa 84 move(0,0);
sakanakuuun 0:d7ff86f25eaa 85 }
sakanakuuun 0:d7ff86f25eaa 86
sakanakuuun 0:d7ff86f25eaa 87 void turncw()
sakanakuuun 0:d7ff86f25eaa 88 {
sakanakuuun 0:d7ff86f25eaa 89 float pt;
sakanakuuun 0:d7ff86f25eaa 90 move((-1)*turnspeed,turnspeed);
sakanakuuun 0:d7ff86f25eaa 91
sakanakuuun 0:d7ff86f25eaa 92 update();
sakanakuuun 0:d7ff86f25eaa 93 pt=coordinateTheta();
sakanakuuun 0:d7ff86f25eaa 94
sakanakuuun 0:d7ff86f25eaa 95 while(1) {
sakanakuuun 0:d7ff86f25eaa 96 update();
sakanakuuun 0:d7ff86f25eaa 97 if(pt-coordinateTheta()>PI/2) {
sakanakuuun 0:d7ff86f25eaa 98 move(0,0);
sakanakuuun 0:d7ff86f25eaa 99 break;
sakanakuuun 0:d7ff86f25eaa 100 }
sakanakuuun 0:d7ff86f25eaa 101 }
sakanakuuun 0:d7ff86f25eaa 102 }
sakanakuuun 0:d7ff86f25eaa 103
sakanakuuun 0:d7ff86f25eaa 104 void turnccw()
sakanakuuun 0:d7ff86f25eaa 105 {
sakanakuuun 0:d7ff86f25eaa 106 float pt;
sakanakuuun 0:d7ff86f25eaa 107 move(turnspeed,(-1)*turnspeed);
sakanakuuun 0:d7ff86f25eaa 108
sakanakuuun 0:d7ff86f25eaa 109 update();
sakanakuuun 0:d7ff86f25eaa 110 pt=coordinateTheta();
sakanakuuun 0:d7ff86f25eaa 111
sakanakuuun 0:d7ff86f25eaa 112 while(1) {
sakanakuuun 0:d7ff86f25eaa 113 update();
sakanakuuun 0:d7ff86f25eaa 114 if(pt-coordinateTheta()<(-1)*PI/2) {
sakanakuuun 0:d7ff86f25eaa 115 move(0,0);
sakanakuuun 0:d7ff86f25eaa 116 break;
sakanakuuun 0:d7ff86f25eaa 117 }
sakanakuuun 0:d7ff86f25eaa 118 }
sakanakuuun 0:d7ff86f25eaa 119 }
sakanakuuun 0:d7ff86f25eaa 120
sakanakuuun 0:d7ff86f25eaa 121 void turn_cw()
sakanakuuun 0:d7ff86f25eaa 122 {
sakanakuuun 0:d7ff86f25eaa 123 float pt;
sakanakuuun 0:d7ff86f25eaa 124 move((-1)*turnspeed,turnspeed);
sakanakuuun 0:d7ff86f25eaa 125
sakanakuuun 0:d7ff86f25eaa 126 update();
sakanakuuun 0:d7ff86f25eaa 127 pt=coordinateTheta();
sakanakuuun 0:d7ff86f25eaa 128
sakanakuuun 0:d7ff86f25eaa 129 while(1) {
sakanakuuun 0:d7ff86f25eaa 130 update();
sakanakuuun 0:d7ff86f25eaa 131 if(pt-coordinateTheta()>PI/2) {
sakanakuuun 0:d7ff86f25eaa 132 move(0,0);
sakanakuuun 0:d7ff86f25eaa 133 break;
sakanakuuun 0:d7ff86f25eaa 134 }
sakanakuuun 0:d7ff86f25eaa 135 }
sakanakuuun 0:d7ff86f25eaa 136
sakanakuuun 0:d7ff86f25eaa 137 hosei_turn(pt, true, PI/2);
sakanakuuun 0:d7ff86f25eaa 138 }
sakanakuuun 0:d7ff86f25eaa 139
sakanakuuun 0:d7ff86f25eaa 140 void turn_ccw()
sakanakuuun 0:d7ff86f25eaa 141 {
sakanakuuun 0:d7ff86f25eaa 142 float pt;
sakanakuuun 0:d7ff86f25eaa 143 move(turnspeed,(-1)*turnspeed);
sakanakuuun 0:d7ff86f25eaa 144
sakanakuuun 0:d7ff86f25eaa 145 update();
sakanakuuun 0:d7ff86f25eaa 146 pt=coordinateTheta();
sakanakuuun 0:d7ff86f25eaa 147
sakanakuuun 0:d7ff86f25eaa 148 while(1) {
sakanakuuun 0:d7ff86f25eaa 149 update();
sakanakuuun 0:d7ff86f25eaa 150 if(pt-coordinateTheta()<(-1)*PI/2) {
sakanakuuun 0:d7ff86f25eaa 151 move(0,0);
sakanakuuun 0:d7ff86f25eaa 152 break;
sakanakuuun 0:d7ff86f25eaa 153 }
sakanakuuun 0:d7ff86f25eaa 154 }
sakanakuuun 0:d7ff86f25eaa 155
sakanakuuun 0:d7ff86f25eaa 156 hosei_turn(pt, false, PI/2);
sakanakuuun 0:d7ff86f25eaa 157 }
sakanakuuun 0:d7ff86f25eaa 158
sakanakuuun 0:d7ff86f25eaa 159 void hosei_turn(float pt, bool cw, float rad)
sakanakuuun 0:d7ff86f25eaa 160 {
sakanakuuun 0:d7ff86f25eaa 161 int np;
sakanakuuun 0:d7ff86f25eaa 162 if(cw) np = 1;
sakanakuuun 0:d7ff86f25eaa 163 else np = -1;
sakanakuuun 0:d7ff86f25eaa 164 while(1) {
sakanakuuun 0:d7ff86f25eaa 165 update();
sakanakuuun 0:d7ff86f25eaa 166 if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
sakanakuuun 0:d7ff86f25eaa 167 move(-15, 15);
sakanakuuun 0:d7ff86f25eaa 168 } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
sakanakuuun 0:d7ff86f25eaa 169 move(15, -15);
sakanakuuun 0:d7ff86f25eaa 170 } else {
sakanakuuun 0:d7ff86f25eaa 171 move(0,0);
sakanakuuun 0:d7ff86f25eaa 172 return;
sakanakuuun 0:d7ff86f25eaa 173 }
sakanakuuun 0:d7ff86f25eaa 174 }
sakanakuuun 0:d7ff86f25eaa 175 }
sakanakuuun 0:d7ff86f25eaa 176
sakanakuuun 0:d7ff86f25eaa 177 void turn_direction(int rad)
sakanakuuun 0:d7ff86f25eaa 178 {
sakanakuuun 0:d7ff86f25eaa 179 move(turnspeed, (-1)*turnspeed);
sakanakuuun 0:d7ff86f25eaa 180
sakanakuuun 0:d7ff86f25eaa 181 update();
sakanakuuun 0:d7ff86f25eaa 182
sakanakuuun 0:d7ff86f25eaa 183 while(1) {
sakanakuuun 0:d7ff86f25eaa 184 update();
sakanakuuun 0:d7ff86f25eaa 185 if(coordinateTheta() < rad + 0.1 && coordinateTheta() > rad - 0.1) {
sakanakuuun 0:d7ff86f25eaa 186 move(0,0);
sakanakuuun 0:d7ff86f25eaa 187 break;
sakanakuuun 0:d7ff86f25eaa 188 }
sakanakuuun 0:d7ff86f25eaa 189 }
sakanakuuun 0:d7ff86f25eaa 190
sakanakuuun 0:d7ff86f25eaa 191 hosei_turn(0, false, rad);
sakanakuuun 0:d7ff86f25eaa 192 }
sakanakuuun 0:d7ff86f25eaa 193 void pmovex(int length)
sakanakuuun 0:d7ff86f25eaa 194 {
sakanakuuun 0:d7ff86f25eaa 195 int px,py,ptheta,dx,dy,dtheta;
sakanakuuun 0:d7ff86f25eaa 196 int k_y=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 0:d7ff86f25eaa 197 int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 0:d7ff86f25eaa 198
sakanakuuun 0:d7ff86f25eaa 199 update();
sakanakuuun 0:d7ff86f25eaa 200 px=coordinateX();
sakanakuuun 0:d7ff86f25eaa 201 py=coordinateY();
sakanakuuun 0:d7ff86f25eaa 202 ptheta=coordinateTheta();
sakanakuuun 0:d7ff86f25eaa 203 move(rightspeed,leftspeed);
sakanakuuun 0:d7ff86f25eaa 204
sakanakuuun 0:d7ff86f25eaa 205 while(1) {
sakanakuuun 0:d7ff86f25eaa 206 update();
sakanakuuun 0:d7ff86f25eaa 207 dx=coordinateX()-px;
sakanakuuun 0:d7ff86f25eaa 208 dy=coordinateY()-py;
sakanakuuun 0:d7ff86f25eaa 209 dtheta=coordinateTheta()-ptheta;
sakanakuuun 0:d7ff86f25eaa 210
sakanakuuun 0:d7ff86f25eaa 211 if(dy>7) {
sakanakuuun 0:d7ff86f25eaa 212 dy=7;
sakanakuuun 0:d7ff86f25eaa 213 }
sakanakuuun 0:d7ff86f25eaa 214 if(dy<-7) {
sakanakuuun 0:d7ff86f25eaa 215 dy=-7;
sakanakuuun 0:d7ff86f25eaa 216 }
sakanakuuun 0:d7ff86f25eaa 217 move(rightspeed-k_y*dy-k_theta*dtheta,leftspeed+k_y*dy+k_theta*dtheta);
sakanakuuun 0:d7ff86f25eaa 218
sakanakuuun 0:d7ff86f25eaa 219 if(dx>length) {
sakanakuuun 0:d7ff86f25eaa 220 move(0,0);
sakanakuuun 0:d7ff86f25eaa 221 break;
sakanakuuun 0:d7ff86f25eaa 222 }
sakanakuuun 0:d7ff86f25eaa 223 }
sakanakuuun 0:d7ff86f25eaa 224 }
sakanakuuun 0:d7ff86f25eaa 225
sakanakuuun 0:d7ff86f25eaa 226 void pmovey(int length)
sakanakuuun 0:d7ff86f25eaa 227 {
sakanakuuun 0:d7ff86f25eaa 228 int px,py,ptheta,dx,dy,dtheta;
sakanakuuun 0:d7ff86f25eaa 229 int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 0:d7ff86f25eaa 230 int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 0:d7ff86f25eaa 231
sakanakuuun 0:d7ff86f25eaa 232 update();
sakanakuuun 0:d7ff86f25eaa 233 px=coordinateX();
sakanakuuun 0:d7ff86f25eaa 234 py=coordinateY();
sakanakuuun 0:d7ff86f25eaa 235 ptheta=coordinateTheta();
sakanakuuun 0:d7ff86f25eaa 236 move(rightspeed,leftspeed);
sakanakuuun 0:d7ff86f25eaa 237
sakanakuuun 0:d7ff86f25eaa 238 while(1) {
sakanakuuun 0:d7ff86f25eaa 239 update();
sakanakuuun 0:d7ff86f25eaa 240 dx=coordinateX()-px;
sakanakuuun 0:d7ff86f25eaa 241 dy=coordinateY()-py;
sakanakuuun 0:d7ff86f25eaa 242 dtheta=coordinateTheta()-ptheta;
sakanakuuun 0:d7ff86f25eaa 243
sakanakuuun 0:d7ff86f25eaa 244 if(dx>7) {
sakanakuuun 0:d7ff86f25eaa 245 dx=7;
sakanakuuun 0:d7ff86f25eaa 246 }
sakanakuuun 0:d7ff86f25eaa 247 if(dx<-7) {
sakanakuuun 0:d7ff86f25eaa 248 dx=-7;
sakanakuuun 0:d7ff86f25eaa 249 }
sakanakuuun 0:d7ff86f25eaa 250 move(rightspeed-k_x*dx-k_theta*dtheta,leftspeed+k_x*dx+k_theta*dtheta);
sakanakuuun 0:d7ff86f25eaa 251
sakanakuuun 0:d7ff86f25eaa 252 if(dy>length) {
sakanakuuun 0:d7ff86f25eaa 253 move(0,0);
sakanakuuun 0:d7ff86f25eaa 254 break;
sakanakuuun 0:d7ff86f25eaa 255 }
sakanakuuun 0:d7ff86f25eaa 256 }
sakanakuuun 0:d7ff86f25eaa 257 }