涼太郎 中村 / Mbed 2 deprecated 9_6test

a

Dependencies:   mbed

Fork of ARAI45th by 涼太郎 中村

main.cpp@8:e2dc708fc3e6, 2016-09-05 (annotated)

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