涼太郎 中村 / Mbed 2 deprecated 9_6test

a

Dependencies:   mbed

Fork of ARAI45th by 涼太郎 中村

main.cpp@7:2b63f0a1b679, 2016-09-05 (annotated)

Committer:
choutin
Date:
Mon Sep 05 07:32:38 2016 +0000
Revision:
7:2b63f0a1b679
Parent:
6:26ac9f539e8b
Child:
8:e2dc708fc3e6
96;

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 7:2b63f0a1b679 209 int main(){
choutin 7:2b63f0a1b679 210 int team=1,phase=0;
sakanakuuun 0:f12d257b587e 211 setup();
choutin 3:56b034c41dc5 212 initmotor();
choutin 7:2b63f0a1b679 213 team=teamLED();
choutin 7:2b63f0a1b679 214 phase=phaseSW();
choutin 6:26ac9f539e8b 215
choutin 7:2b63f0a1b679 216 if(team>0){
choutin 7:2b63f0a1b679 217
choutin 7:2b63f0a1b679 218 if(phase==0){
choutin 7:2b63f0a1b679 219 movelength(600);
choutin 7:2b63f0a1b679 220 turnccw();
choutin 7:2b63f0a1b679 221 movelength(900);
choutin 7:2b63f0a1b679 222 turnccw80();
choutin 7:2b63f0a1b679 223 movelength(600-50);
choutin 7:2b63f0a1b679 224 wait(30);
choutin 7:2b63f0a1b679 225 }
choutin 7:2b63f0a1b679 226
choutin 7:2b63f0a1b679 227 if(phase==1){
choutin 7:2b63f0a1b679 228 movelength(900);
choutin 7:2b63f0a1b679 229 turnccw();
choutin 7:2b63f0a1b679 230 movelength(900);
choutin 7:2b63f0a1b679 231 turnccw80();
choutin 7:2b63f0a1b679 232 movelength(900-50);
choutin 7:2b63f0a1b679 233 wait(30);
choutin 7:2b63f0a1b679 234 }
choutin 7:2b63f0a1b679 235
choutin 7:2b63f0a1b679 236 if(phase==2){
choutin 7:2b63f0a1b679 237 movelength(1200);
choutin 7:2b63f0a1b679 238 turnccw();
choutin 7:2b63f0a1b679 239 movelength(1200);
choutin 7:2b63f0a1b679 240 turnccw80();
choutin 7:2b63f0a1b679 241 movelength(1200-50);
choutin 7:2b63f0a1b679 242 wait(30);
choutin 7:2b63f0a1b679 243 }
choutin 7:2b63f0a1b679 244 }
choutin 3:56b034c41dc5 245
choutin 7:2b63f0a1b679 246 if(team<0){
choutin 7:2b63f0a1b679 247
choutin 7:2b63f0a1b679 248 if(phase==0){
choutin 7:2b63f0a1b679 249 movelength(600);
choutin 7:2b63f0a1b679 250 turncw();
choutin 7:2b63f0a1b679 251 movelength(900);
choutin 7:2b63f0a1b679 252 turncw();
choutin 7:2b63f0a1b679 253 movelength(600);
choutin 7:2b63f0a1b679 254 wait(30);
choutin 7:2b63f0a1b679 255 }
choutin 7:2b63f0a1b679 256
choutin 7:2b63f0a1b679 257 if(phase==1){
choutin 7:2b63f0a1b679 258 movelength(900);
choutin 7:2b63f0a1b679 259 turncw();
choutin 7:2b63f0a1b679 260 movelength(900);
choutin 7:2b63f0a1b679 261 turncw();
choutin 7:2b63f0a1b679 262 movelength(900);
choutin 7:2b63f0a1b679 263 wait(30);
choutin 7:2b63f0a1b679 264 }
choutin 7:2b63f0a1b679 265
choutin 7:2b63f0a1b679 266 if(phase==2){
choutin 7:2b63f0a1b679 267 movelength(1200);
choutin 7:2b63f0a1b679 268 turncw();
choutin 7:2b63f0a1b679 269 movelength(600);
choutin 7:2b63f0a1b679 270 turncw();
choutin 7:2b63f0a1b679 271 movelength(1200);
choutin 7:2b63f0a1b679 272 wait(30);
choutin 7:2b63f0a1b679 273 }
choutin 7:2b63f0a1b679 274 }
choutin 3:56b034c41dc5 275 }