涼太郎 中村 / Mbed 2 deprecated 9_6test

a

Dependencies:   mbed

Fork of ARAI45th by 涼太郎 中村

main.cpp@9:f4dbffb78eb8, 2016-09-06 (annotated)

Committer:
choutin
Date:
Tue Sep 06 08:46:22 2016 +0000
Revision:
9:f4dbffb78eb8
Parent:
8:e2dc708fc3e6 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakanakuuun 0:f12d257b587e 1 #include "mbed.h"
sakanakuuun 0:f12d257b587e 2 #include "locate.h"
choutin 3:56b034c41dc5 3 #include "math.h"
choutin 3:56b034c41dc5 4 PwmOut M1cw(PA_11);
choutin 3:56b034c41dc5 5 PwmOut M1ccw(PB_15);
choutin 3:56b034c41dc5 6 PwmOut M2ccw(PB_14);
choutin 3:56b034c41dc5 7 PwmOut M2cw(PB_13);
choutin 3:56b034c41dc5 8
choutin 9:f4dbffb78eb8 9 DigitalIn start(USER_BUTTON);
choutin 9:f4dbffb78eb8 10
choutin 7:2b63f0a1b679 11 DigitalIn teamSW(PC_11);
choutin 7:2b63f0a1b679 12 DigitalOut teamledblue(PC_10);
choutin 7:2b63f0a1b679 13 DigitalOut teamledred(PC_12);
choutin 7:2b63f0a1b679 14
choutin 7:2b63f0a1b679 15 DigitalIn phase1(PA_5);
choutin 7:2b63f0a1b679 16 DigitalIn phase2(PA_6);
choutin 9:f4dbffb78eb8 17 DigitalIn phase4(PA_7);
choutin 9:f4dbffb78eb8 18 DigitalIn phase8(PB_12);
choutin 7:2b63f0a1b679 19
choutin 3:56b034c41dc5 20 const int allowlength=5;
choutin 3:56b034c41dc5 21 const float allowdegree=0.02;
choutin 9:f4dbffb78eb8 22 const int rightspeed=30*2-2;
choutin 9:f4dbffb78eb8 23 const int leftspeed=30*2+1;
choutin 3:56b034c41dc5 24 const int turnspeed=15*2;
choutin 3:56b034c41dc5 25
choutin 9:f4dbffb78eb8 26 const float PIfact=2.8;
choutin 9:f4dbffb78eb8 27 const float PIblue=2.8;
choutin 7:2b63f0a1b679 28 int phaseSW(void){
choutin 7:2b63f0a1b679 29
choutin 7:2b63f0a1b679 30
choutin 7:2b63f0a1b679 31 phase1.mode(PullUp);
choutin 7:2b63f0a1b679 32 phase2.mode(PullUp);
choutin 9:f4dbffb78eb8 33 phase4.mode(PullUp);
choutin 9:f4dbffb78eb8 34 phase8.mode(PullUp);
choutin 9:f4dbffb78eb8 35
choutin 9:f4dbffb78eb8 36 int SW=phase1+2*phase2+4*phase4+8*phase8;
choutin 7:2b63f0a1b679 37 pc.printf("%d\n\r",SW);
choutin 7:2b63f0a1b679 38 return SW;
choutin 7:2b63f0a1b679 39 }
choutin 3:56b034c41dc5 40 void initmotor()
choutin 3:56b034c41dc5 41 {
choutin 3:56b034c41dc5 42
choutin 3:56b034c41dc5 43
choutin 3:56b034c41dc5 44 M1cw.period_us(256);
choutin 3:56b034c41dc5 45 M1ccw.period_us(256);
choutin 3:56b034c41dc5 46 M2cw.period_us(256);
choutin 3:56b034c41dc5 47 M2ccw.period_us(256);
choutin 3:56b034c41dc5 48
choutin 3:56b034c41dc5 49 }
choutin 3:56b034c41dc5 50
choutin 3:56b034c41dc5 51 void move(int left,int right)
choutin 3:56b034c41dc5 52 {
choutin 3:56b034c41dc5 53
choutin 3:56b034c41dc5 54 float rightduty,leftduty;
choutin 3:56b034c41dc5 55
choutin 3:56b034c41dc5 56 if(right>256) {
choutin 3:56b034c41dc5 57 right=256;
choutin 3:56b034c41dc5 58 }
choutin 3:56b034c41dc5 59 if(left>256) {
choutin 3:56b034c41dc5 60 left=256;
choutin 3:56b034c41dc5 61 }
choutin 3:56b034c41dc5 62 if(right<-256) {
choutin 3:56b034c41dc5 63 right=-256;
choutin 3:56b034c41dc5 64 }
choutin 3:56b034c41dc5 65 if(left<-256) {
choutin 3:56b034c41dc5 66 left=-256;
choutin 3:56b034c41dc5 67 }
choutin 3:56b034c41dc5 68
choutin 3:56b034c41dc5 69 rightduty=right/256.0;
choutin 3:56b034c41dc5 70 leftduty=left/256.0;
choutin 3:56b034c41dc5 71 if(right>0) {
choutin 3:56b034c41dc5 72 M1cw.write(1-rightduty);
choutin 3:56b034c41dc5 73 M1ccw.write(1);
choutin 3:56b034c41dc5 74 } else {
choutin 3:56b034c41dc5 75 M1cw.write(1);
choutin 3:56b034c41dc5 76 M1ccw.write(1+rightduty);
choutin 3:56b034c41dc5 77 }
choutin 3:56b034c41dc5 78
choutin 3:56b034c41dc5 79 if(left>0) {
choutin 3:56b034c41dc5 80 M2cw.write(1-leftduty);
choutin 3:56b034c41dc5 81 M2ccw.write(1);
choutin 3:56b034c41dc5 82 } else {
choutin 3:56b034c41dc5 83 M2cw.write(1);
choutin 3:56b034c41dc5 84 M2ccw.write(1+leftduty);
choutin 3:56b034c41dc5 85 }
choutin 3:56b034c41dc5 86 }
choutin 3:56b034c41dc5 87
choutin 3:56b034c41dc5 88
choutin 3:56b034c41dc5 89 void movelength(int length)
choutin 3:56b034c41dc5 90 {
choutin 3:56b034c41dc5 91 int px,py,pt;
choutin 3:56b034c41dc5 92 update();
choutin 3:56b034c41dc5 93 px=coordinateX();
choutin 3:56b034c41dc5 94 py=coordinateY();
choutin 3:56b034c41dc5 95 pt=coordinateTheta();
choutin 3:56b034c41dc5 96
choutin 3:56b034c41dc5 97 move(rightspeed,leftspeed);
choutin 3:56b034c41dc5 98 while(1) {
choutin 3:56b034c41dc5 99
choutin 3:56b034c41dc5 100 update();
choutin 3:56b034c41dc5 101 //pc.printf("dx:%d, dy:%d, l:%d x:%d y:%d t:%f\n\r",px-coordinateX(),py-coordinateY(),length,coordinateX(),coordinateY(), coordinateTheta());
choutin 3:56b034c41dc5 102 if(((px-coordinateX())*(px-coordinateX())+(py-coordinateY())*(py-coordinateY()))>length*length) {
choutin 3:56b034c41dc5 103 break;
choutin 3:56b034c41dc5 104 }
choutin 3:56b034c41dc5 105
choutin 3:56b034c41dc5 106 }
choutin 3:56b034c41dc5 107 move(0,0);
choutin 3:56b034c41dc5 108 }
choutin 3:56b034c41dc5 109
choutin 3:56b034c41dc5 110 void turncw()
choutin 3:56b034c41dc5 111 {
choutin 3:56b034c41dc5 112 float pt;
choutin 3:56b034c41dc5 113 move((-1)*turnspeed,turnspeed);
choutin 3:56b034c41dc5 114
choutin 3:56b034c41dc5 115 update();
choutin 3:56b034c41dc5 116 pt=coordinateTheta();
choutin 3:56b034c41dc5 117
choutin 3:56b034c41dc5 118 while(1) {
choutin 3:56b034c41dc5 119 update();
choutin 3:56b034c41dc5 120 if(pt-coordinateTheta()>PIfact/2) {
choutin 3:56b034c41dc5 121 move(0,0);
choutin 3:56b034c41dc5 122 break;
choutin 3:56b034c41dc5 123 }
choutin 3:56b034c41dc5 124 }
choutin 3:56b034c41dc5 125 }
choutin 3:56b034c41dc5 126
choutin 3:56b034c41dc5 127 void turnccw()
choutin 3:56b034c41dc5 128 {
choutin 3:56b034c41dc5 129 float pt;
choutin 3:56b034c41dc5 130 move(turnspeed,(-1)*turnspeed);
choutin 3:56b034c41dc5 131
choutin 3:56b034c41dc5 132 update();
choutin 3:56b034c41dc5 133 pt=coordinateTheta();
choutin 3:56b034c41dc5 134
choutin 3:56b034c41dc5 135 while(1) {
choutin 3:56b034c41dc5 136 update();
choutin 3:56b034c41dc5 137 if(pt-coordinateTheta()<(-1)*PIfact/2) {
choutin 3:56b034c41dc5 138 move(0,0);
choutin 3:56b034c41dc5 139 break;
choutin 3:56b034c41dc5 140 }
choutin 3:56b034c41dc5 141 }
choutin 3:56b034c41dc5 142 }
choutin 7:2b63f0a1b679 143
choutin 7:2b63f0a1b679 144 void turnccw80()
choutin 7:2b63f0a1b679 145 { float GODRAD=1.2;
choutin 7:2b63f0a1b679 146 float pt;
choutin 7:2b63f0a1b679 147 move(turnspeed,(-1)*turnspeed);
choutin 7:2b63f0a1b679 148
choutin 7:2b63f0a1b679 149 update();
choutin 7:2b63f0a1b679 150 pt=coordinateTheta();
choutin 3:56b034c41dc5 151
choutin 7:2b63f0a1b679 152 while(1) {
choutin 7:2b63f0a1b679 153 update();
choutin 7:2b63f0a1b679 154 if(pt-coordinateTheta()<(-1)*GODRAD) {
choutin 7:2b63f0a1b679 155 move(0,0);
choutin 7:2b63f0a1b679 156 break;
choutin 7:2b63f0a1b679 157 }
choutin 3:56b034c41dc5 158 }
choutin 7:2b63f0a1b679 159 }
choutin 7:2b63f0a1b679 160
choutin 7:2b63f0a1b679 161 void turncw80()
choutin 7:2b63f0a1b679 162 { float GODRAD=1.2;
choutin 7:2b63f0a1b679 163 float pt;
choutin 7:2b63f0a1b679 164 move((-1)*turnspeed,turnspeed);
choutin 7:2b63f0a1b679 165
choutin 7:2b63f0a1b679 166 update();
choutin 7:2b63f0a1b679 167 pt=coordinateTheta();
choutin 7:2b63f0a1b679 168
choutin 7:2b63f0a1b679 169 while(1) {
choutin 7:2b63f0a1b679 170 update();
choutin 7:2b63f0a1b679 171 if(pt-coordinateTheta()>GODRAD) {
choutin 7:2b63f0a1b679 172 move(0,0);
choutin 7:2b63f0a1b679 173 break;
choutin 7:2b63f0a1b679 174 }
choutin 7:2b63f0a1b679 175 }
choutin 7:2b63f0a1b679 176 }
choutin 3:56b034c41dc5 177
choutin 3:56b034c41dc5 178 void pmovex(int length){
choutin 3:56b034c41dc5 179 int px,py,dx,dy;
choutin 3:56b034c41dc5 180 int k=1;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
choutin 3:56b034c41dc5 181 update();
choutin 3:56b034c41dc5 182 px=coordinateX();
choutin 3:56b034c41dc5 183 py=coordinateY();
choutin 3:56b034c41dc5 184 move(rightspeed,leftspeed);
choutin 3:56b034c41dc5 185
choutin 3:56b034c41dc5 186 while(1){
choutin 3:56b034c41dc5 187 update();
choutin 3:56b034c41dc5 188 dx=coordinateX()-px;
choutin 3:56b034c41dc5 189 dy=coordinateY()-py;
choutin 3:56b034c41dc5 190
choutin 3:56b034c41dc5 191 if(dy>7){dy=7;}
choutin 3:56b034c41dc5 192 if(dy<-7){dy=-7;}
choutin 3:56b034c41dc5 193 move(rightspeed-k*dy,leftspeed+k*dy);
choutin 3:56b034c41dc5 194
choutin 3:56b034c41dc5 195 if(dx>length){
choutin 3:56b034c41dc5 196 move(0,0);
choutin 3:56b034c41dc5 197 break;
choutin 3:56b034c41dc5 198 }
choutin 3:56b034c41dc5 199 }
choutin 3:56b034c41dc5 200 }
sakanakuuun 0:f12d257b587e 201
choutin 7:2b63f0a1b679 202 int teamLED()
sakanakuuun 0:f12d257b587e 203 {
choutin 7:2b63f0a1b679 204 teamSW.mode(PullUp);
choutin 7:2b63f0a1b679 205 if(teamSW) {
choutin 7:2b63f0a1b679 206 teamledblue=1;
choutin 7:2b63f0a1b679 207 teamledred=0;
choutin 7:2b63f0a1b679 208 return -1;
choutin 7:2b63f0a1b679 209 } else {
choutin 7:2b63f0a1b679 210 teamledblue=0;
choutin 7:2b63f0a1b679 211 teamledred=1;
choutin 7:2b63f0a1b679 212 return 1;
choutin 7:2b63f0a1b679 213 }
choutin 7:2b63f0a1b679 214 }
choutin 7:2b63f0a1b679 215
choutin 8:e2dc708fc3e6 216
choutin 8:e2dc708fc3e6 217
choutin 8:e2dc708fc3e6 218
choutin 7:2b63f0a1b679 219 int main(){
choutin 8:e2dc708fc3e6 220 int team=1,phase=0,comand,buf;
sakanakuuun 0:f12d257b587e 221 setup();
choutin 3:56b034c41dc5 222 initmotor();
choutin 7:2b63f0a1b679 223 team=teamLED();
choutin 7:2b63f0a1b679 224 phase=phaseSW();
choutin 9:f4dbffb78eb8 225 while(start);
choutin 7:2b63f0a1b679 226 if(team>0){
choutin 7:2b63f0a1b679 227
choutin 7:2b63f0a1b679 228 if(phase==0){
choutin 7:2b63f0a1b679 229 movelength(600);
choutin 7:2b63f0a1b679 230 turnccw();
choutin 7:2b63f0a1b679 231 movelength(900);
choutin 7:2b63f0a1b679 232 turnccw80();
choutin 7:2b63f0a1b679 233 movelength(600-50);
choutin 7:2b63f0a1b679 234 wait(30);
choutin 7:2b63f0a1b679 235 }
choutin 7:2b63f0a1b679 236
choutin 7:2b63f0a1b679 237 if(phase==1){
choutin 7:2b63f0a1b679 238 movelength(900);
choutin 7:2b63f0a1b679 239 turnccw();
choutin 7:2b63f0a1b679 240 movelength(900);
choutin 7:2b63f0a1b679 241 turnccw80();
choutin 7:2b63f0a1b679 242 movelength(900-50);
choutin 7:2b63f0a1b679 243 wait(30);
choutin 7:2b63f0a1b679 244 }
choutin 7:2b63f0a1b679 245
choutin 7:2b63f0a1b679 246 if(phase==2){
choutin 7:2b63f0a1b679 247 movelength(1200);
choutin 7:2b63f0a1b679 248 turnccw();
choutin 9:f4dbffb78eb8 249 movelength(700);
choutin 9:f4dbffb78eb8 250 turnccw80();
choutin 9:f4dbffb78eb8 251 movelength(1200-100);
choutin 9:f4dbffb78eb8 252 wait(30);
choutin 9:f4dbffb78eb8 253 }
choutin 9:f4dbffb78eb8 254
choutin 9:f4dbffb78eb8 255 if(phase==3){
choutin 9:f4dbffb78eb8 256 movelength(1200);
choutin 8:e2dc708fc3e6 257 turnccw();
choutin 9:f4dbffb78eb8 258 movelength(1000);
choutin 9:f4dbffb78eb8 259 turnccw80();
choutin 8:e2dc708fc3e6 260 movelength(1200-100);
choutin 8:e2dc708fc3e6 261 wait(30);
choutin 8:e2dc708fc3e6 262 }
choutin 8:e2dc708fc3e6 263
choutin 8:e2dc708fc3e6 264 if(phase==15){
choutin 8:e2dc708fc3e6 265 buf=15;
choutin 9:f4dbffb78eb8 266 while(buf>=phaseSW()){
choutin 8:e2dc708fc3e6 267 buf=phaseSW();
choutin 8:e2dc708fc3e6 268 }
choutin 8:e2dc708fc3e6 269 //スイッチを上昇させると、上昇前で確定
choutin 8:e2dc708fc3e6 270 //この時点でbufに現在のスイッチの状態がはいる。
choutin 9:f4dbffb78eb8 271 pc.printf("Hello World !%d\n",buf);
choutin 8:e2dc708fc3e6 272 movelength(300+75*buf);
choutin 8:e2dc708fc3e6 273 turnccw();
choutin 7:2b63f0a1b679 274 movelength(1200);
choutin 7:2b63f0a1b679 275 turnccw80();
choutin 8:e2dc708fc3e6 276 movelength(300+75*buf);
choutin 7:2b63f0a1b679 277 wait(30);
choutin 7:2b63f0a1b679 278 }
choutin 7:2b63f0a1b679 279 }
choutin 3:56b034c41dc5 280
choutin 9:f4dbffb78eb8 281 if(team<0){
choutin 7:2b63f0a1b679 282
choutin 7:2b63f0a1b679 283 if(phase==0){
choutin 7:2b63f0a1b679 284 movelength(600);
choutin 9:f4dbffb78eb8 285 turncw80();
choutin 7:2b63f0a1b679 286 movelength(900);
choutin 9:f4dbffb78eb8 287 turncw80();
choutin 9:f4dbffb78eb8 288 movelength(600-50);
choutin 7:2b63f0a1b679 289 wait(30);
choutin 7:2b63f0a1b679 290 }
choutin 7:2b63f0a1b679 291
choutin 7:2b63f0a1b679 292 if(phase==1){
choutin 7:2b63f0a1b679 293 movelength(900);
choutin 9:f4dbffb78eb8 294 turncw80();
choutin 7:2b63f0a1b679 295 movelength(900);
choutin 9:f4dbffb78eb8 296 turncw80();
choutin 9:f4dbffb78eb8 297 movelength(900-50);
choutin 9:f4dbffb78eb8 298 wait(30);
choutin 9:f4dbffb78eb8 299 }
choutin 9:f4dbffb78eb8 300
choutin 7:2b63f0a1b679 301 if(phase==2){
choutin 9:f4dbffb78eb8 302 movelength(1200-50);
choutin 9:f4dbffb78eb8 303 turncw80();
choutin 7:2b63f0a1b679 304 movelength(600);
choutin 9:f4dbffb78eb8 305 turncw80();
choutin 9:f4dbffb78eb8 306 movelength(1200-100);
choutin 9:f4dbffb78eb8 307 wait(30);
choutin 9:f4dbffb78eb8 308 }
choutin 9:f4dbffb78eb8 309
choutin 9:f4dbffb78eb8 310 if(phase==3){
choutin 9:f4dbffb78eb8 311 movelength(1200-50);
choutin 9:f4dbffb78eb8 312 turncw80();
choutin 9:f4dbffb78eb8 313 movelength(1000);
choutin 9:f4dbffb78eb8 314 turncw80();
choutin 9:f4dbffb78eb8 315 movelength(1200-100);
choutin 9:f4dbffb78eb8 316 wait(30);
choutin 9:f4dbffb78eb8 317 }
choutin 9:f4dbffb78eb8 318
choutin 9:f4dbffb78eb8 319 if(phase==15){
choutin 9:f4dbffb78eb8 320 buf=15;
choutin 9:f4dbffb78eb8 321 while(buf>=phaseSW()){
choutin 9:f4dbffb78eb8 322 buf=phaseSW();
choutin 9:f4dbffb78eb8 323 }
choutin 9:f4dbffb78eb8 324
choutin 9:f4dbffb78eb8 325 //スイッチを上昇させると、上昇前で確定
choutin 9:f4dbffb78eb8 326 //この時点でbufに現在のスイッチの状態がはいる。
choutin 9:f4dbffb78eb8 327
choutin 9:f4dbffb78eb8 328 movelength(300+75*buf);
choutin 7:2b63f0a1b679 329 turncw();
choutin 7:2b63f0a1b679 330 movelength(1200);
choutin 9:f4dbffb78eb8 331 turncw80();
choutin 9:f4dbffb78eb8 332 movelength(300+75*buf);
choutin 7:2b63f0a1b679 333 wait(30);
choutin 7:2b63f0a1b679 334 }
choutin 7:2b63f0a1b679 335 }
choutin 9:f4dbffb78eb8 336
choutin 3:56b034c41dc5 337 }