涼太郎 中村 / Mbed 2 deprecated 9_6test

a

Dependencies:   mbed

Fork of ARAI45th by 涼太郎 中村

prime.h@9:f4dbffb78eb8, 2016-09-06 (annotated)

Committer:
choutin
Date:
Tue Sep 06 08:46:22 2016 +0000
Revision:
9:f4dbffb78eb8
Parent:
3:56b034c41dc5 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
choutin 3:56b034c41dc5 1
choutin 3:56b034c41dc5 2 /*
choutin 3:56b034c41dc5 3 primeではPINを制御する関数を扱う。
choutin 3:56b034c41dc5 4
choutin 3:56b034c41dc5 5 以下一覧
choutin 3:56b034c41dc5 6 open,close hand
choutin 3:56b034c41dc5 7 open close arm
choutin 3:56b034c41dc5 8
choutin 3:56b034c41dc5 9 step
choutin 3:56b034c41dc5 10 move
choutin 3:56b034c41dc5 11 blueorred
choutin 3:56b034c41dc5 12 lrsensor
choutin 3:56b034c41dc5 13
choutin 3:56b034c41dc5 14 全関数共通で意図しない動作が起きたら基板上のLEDを点灯
choutin 3:56b034c41dc5 15 */
choutin 3:56b034c41dc5 16 PwmOut pwmarm(PC_6);
choutin 3:56b034c41dc5 17 PwmOut pwmhand(PC_8);
choutin 3:56b034c41dc5 18
choutin 3:56b034c41dc5 19 AnalogIn rightsensor(A0);
choutin 3:56b034c41dc5 20 AnalogIn leftsensor(A1);
choutin 3:56b034c41dc5 21
choutin 3:56b034c41dc5 22 AnalogIn armadj(A2);
choutin 3:56b034c41dc5 23 AnalogIn handadj(A3);
choutin 3:56b034c41dc5 24
choutin 3:56b034c41dc5 25 DigitalIn teamSW(PC_11);
choutin 3:56b034c41dc5 26 DigitalOut teamledblue(PC_10);
choutin 3:56b034c41dc5 27 DigitalOut teamledred(PC_12);
choutin 3:56b034c41dc5 28
choutin 3:56b034c41dc5 29 DigitalOut errorled(LED1);
choutin 3:56b034c41dc5 30
choutin 3:56b034c41dc5 31
choutin 3:56b034c41dc5 32 DigitalIn phase1(PB_8);
choutin 3:56b034c41dc5 33 DigitalIn phase2(PC_9);
choutin 3:56b034c41dc5 34 DigitalIn phase4(PB_9);
choutin 3:56b034c41dc5 35 DigitalIn phase8(PD_2);
choutin 3:56b034c41dc5 36
choutin 3:56b034c41dc5 37 PwmOut M1cw(PA_11);
choutin 3:56b034c41dc5 38 PwmOut M1ccw(PB_15);
choutin 3:56b034c41dc5 39 PwmOut M2ccw(PB_14);
choutin 3:56b034c41dc5 40 PwmOut M2cw(PB_13);
choutin 3:56b034c41dc5 41
choutin 3:56b034c41dc5 42 DigitalOut PINW(PA_3);
choutin 3:56b034c41dc5 43 DigitalOut PINX(PA_2);
choutin 3:56b034c41dc5 44 DigitalOut PINY(PA_10);
choutin 3:56b034c41dc5 45 DigitalOut PINZ(PB_3);
choutin 3:56b034c41dc5 46
choutin 3:56b034c41dc5 47
choutin 3:56b034c41dc5 48
choutin 3:56b034c41dc5 49
choutin 3:56b034c41dc5 50 DigitalOut encordervcc1(PA_6);
choutin 3:56b034c41dc5 51 DigitalOut encordervcc2(PA_14);
choutin 3:56b034c41dc5 52
choutin 3:56b034c41dc5 53 void initencorder(void){
choutin 3:56b034c41dc5 54 encordervcc1=1;
choutin 3:56b034c41dc5 55 encordervcc2=1;
choutin 3:56b034c41dc5 56 }
choutin 3:56b034c41dc5 57
choutin 3:56b034c41dc5 58
choutin 3:56b034c41dc5 59
choutin 3:56b034c41dc5 60 void close_hand(void) {
choutin 3:56b034c41dc5 61 int i,degree;
choutin 3:56b034c41dc5 62
choutin 3:56b034c41dc5 63 pwmhand.period_ms(20); //20ms
choutin 3:56b034c41dc5 64
choutin 3:56b034c41dc5 65 degree=175;
choutin 3:56b034c41dc5 66
choutin 3:56b034c41dc5 67 i=500+degree*1900/180;
choutin 3:56b034c41dc5 68 pwmhand.pulsewidth_us(i);
choutin 3:56b034c41dc5 69
choutin 3:56b034c41dc5 70
choutin 3:56b034c41dc5 71 }
choutin 3:56b034c41dc5 72
choutin 3:56b034c41dc5 73 void close_arm(void) {
choutin 3:56b034c41dc5 74 PwmOut mypwm(PB_3);
choutin 3:56b034c41dc5 75 int i,degree;
choutin 3:56b034c41dc5 76
choutin 3:56b034c41dc5 77 mypwm.period_ms(20); //20ms
choutin 3:56b034c41dc5 78
choutin 3:56b034c41dc5 79 degree=160;
choutin 3:56b034c41dc5 80
choutin 3:56b034c41dc5 81 i=500+degree*1900/180;
choutin 3:56b034c41dc5 82 pwmarm.pulsewidth_us(i);
choutin 3:56b034c41dc5 83
choutin 3:56b034c41dc5 84
choutin 3:56b034c41dc5 85 }
choutin 3:56b034c41dc5 86
choutin 3:56b034c41dc5 87
choutin 3:56b034c41dc5 88
choutin 3:56b034c41dc5 89 void open_hand(void) {
choutin 3:56b034c41dc5 90 PwmOut mypwm(PWM_OUT);
choutin 3:56b034c41dc5 91 int i,degree;
choutin 3:56b034c41dc5 92
choutin 3:56b034c41dc5 93 pwmhand.period_ms(20); //20ms
choutin 3:56b034c41dc5 94
choutin 3:56b034c41dc5 95 degree=90;
choutin 3:56b034c41dc5 96
choutin 3:56b034c41dc5 97 i=500+degree*1900/180;
choutin 3:56b034c41dc5 98 mypwm.pulsewidth_us(i);
choutin 3:56b034c41dc5 99
choutin 3:56b034c41dc5 100
choutin 3:56b034c41dc5 101 }
choutin 3:56b034c41dc5 102
choutin 3:56b034c41dc5 103
choutin 3:56b034c41dc5 104
choutin 3:56b034c41dc5 105 void open_arm(void) {
choutin 3:56b034c41dc5 106 PwmOut mypwm(PWM_OUT);
choutin 3:56b034c41dc5 107 int i,degree;
choutin 3:56b034c41dc5 108
choutin 3:56b034c41dc5 109 mypwm.period_ms(20); //20ms
choutin 3:56b034c41dc5 110
choutin 3:56b034c41dc5 111 degree=10;
choutin 3:56b034c41dc5 112
choutin 3:56b034c41dc5 113 i=500+degree*1900/180;
choutin 3:56b034c41dc5 114 pwmarm.pulsewidth_us(i);
choutin 3:56b034c41dc5 115
choutin 3:56b034c41dc5 116
choutin 3:56b034c41dc5 117 }
choutin 3:56b034c41dc5 118
choutin 3:56b034c41dc5 119 void step(int degree){
choutin 3:56b034c41dc5 120
choutin 3:56b034c41dc5 121
choutin 3:56b034c41dc5 122
choutin 3:56b034c41dc5 123 int puls_times=0;
choutin 3:56b034c41dc5 124
choutin 3:56b034c41dc5 125 if(degree>0){
choutin 3:56b034c41dc5 126
choutin 3:56b034c41dc5 127 puls_times=1+(int)(degree/(3.75));
choutin 3:56b034c41dc5 128
choutin 3:56b034c41dc5 129 while(1){
choutin 3:56b034c41dc5 130 PINW=1;
choutin 3:56b034c41dc5 131 PINX=1;
choutin 3:56b034c41dc5 132 PINY=0;
choutin 3:56b034c41dc5 133 PINZ=0;
choutin 3:56b034c41dc5 134 wait_ms(20);
choutin 3:56b034c41dc5 135 puls_times--;
choutin 3:56b034c41dc5 136
choutin 3:56b034c41dc5 137 if(puls_times<0){break;}
choutin 3:56b034c41dc5 138
choutin 3:56b034c41dc5 139 PINW=0;
choutin 3:56b034c41dc5 140 PINX=1;
choutin 3:56b034c41dc5 141 PINY=1;
choutin 3:56b034c41dc5 142 PINZ=0;
choutin 3:56b034c41dc5 143 wait_ms(20);
choutin 3:56b034c41dc5 144 puls_times--;
choutin 3:56b034c41dc5 145
choutin 3:56b034c41dc5 146 if(puls_times<0){break;}
choutin 3:56b034c41dc5 147
choutin 3:56b034c41dc5 148 PINW=0;
choutin 3:56b034c41dc5 149 PINX=0;
choutin 3:56b034c41dc5 150 PINY=1;
choutin 3:56b034c41dc5 151 PINZ=1;
choutin 3:56b034c41dc5 152 wait_ms(20);
choutin 3:56b034c41dc5 153 puls_times--;
choutin 3:56b034c41dc5 154
choutin 3:56b034c41dc5 155 if(puls_times<0){break;}
choutin 3:56b034c41dc5 156
choutin 3:56b034c41dc5 157 PINW=1;
choutin 3:56b034c41dc5 158 PINX=1;
choutin 3:56b034c41dc5 159 PINY=0;
choutin 3:56b034c41dc5 160 PINZ=0;
choutin 3:56b034c41dc5 161 wait_ms(20);
choutin 3:56b034c41dc5 162 puls_times--;
choutin 3:56b034c41dc5 163
choutin 3:56b034c41dc5 164 if(puls_times<0){break;}
choutin 3:56b034c41dc5 165 }
choutin 3:56b034c41dc5 166
choutin 3:56b034c41dc5 167 }
choutin 3:56b034c41dc5 168
choutin 3:56b034c41dc5 169 if(degree<0){
choutin 3:56b034c41dc5 170
choutin 3:56b034c41dc5 171 puls_times=(-1)*(1+(int)(degree/(3.75)));
choutin 3:56b034c41dc5 172
choutin 3:56b034c41dc5 173 while(1){
choutin 3:56b034c41dc5 174 PINW=1;
choutin 3:56b034c41dc5 175 PINX=1;
choutin 3:56b034c41dc5 176 PINY=0;
choutin 3:56b034c41dc5 177 PINZ=0;
choutin 3:56b034c41dc5 178 wait_ms(20);
choutin 3:56b034c41dc5 179 puls_times--;
choutin 3:56b034c41dc5 180
choutin 3:56b034c41dc5 181 if(puls_times<0){break;}
choutin 3:56b034c41dc5 182
choutin 3:56b034c41dc5 183 PINW=1;
choutin 3:56b034c41dc5 184 PINX=0;
choutin 3:56b034c41dc5 185 PINY=0;
choutin 3:56b034c41dc5 186 PINZ=1;
choutin 3:56b034c41dc5 187 wait_ms(20);
choutin 3:56b034c41dc5 188 puls_times--;
choutin 3:56b034c41dc5 189
choutin 3:56b034c41dc5 190 if(puls_times<0){break;}
choutin 3:56b034c41dc5 191
choutin 3:56b034c41dc5 192 PINW=0;
choutin 3:56b034c41dc5 193 PINX=0;
choutin 3:56b034c41dc5 194 PINY=1;
choutin 3:56b034c41dc5 195 PINZ=1;
choutin 3:56b034c41dc5 196 wait_ms(20);
choutin 3:56b034c41dc5 197 puls_times--;
choutin 3:56b034c41dc5 198
choutin 3:56b034c41dc5 199 if(puls_times<0){break;}
choutin 3:56b034c41dc5 200
choutin 3:56b034c41dc5 201 PINW=0;
choutin 3:56b034c41dc5 202 PINX=1;
choutin 3:56b034c41dc5 203 PINY=1;
choutin 3:56b034c41dc5 204 PINZ=0;
choutin 3:56b034c41dc5 205 wait_ms(20);
choutin 3:56b034c41dc5 206 puls_times--;
choutin 3:56b034c41dc5 207
choutin 3:56b034c41dc5 208 if(puls_times<0){break;}
choutin 3:56b034c41dc5 209 }
choutin 3:56b034c41dc5 210
choutin 3:56b034c41dc5 211 }
choutin 3:56b034c41dc5 212
choutin 3:56b034c41dc5 213 }
choutin 3:56b034c41dc5 214
choutin 3:56b034c41dc5 215
choutin 3:56b034c41dc5 216
choutin 3:56b034c41dc5 217 int sensor(void){
choutin 3:56b034c41dc5 218 //センサー読
choutin 3:56b034c41dc5 219 int right ,left,x,y;
choutin 3:56b034c41dc5 220
choutin 3:56b034c41dc5 221 right=rightsensor.read();
choutin 3:56b034c41dc5 222 left=leftsensor.read();
choutin 3:56b034c41dc5 223
choutin 3:56b034c41dc5 224 if(right>0.5){x=1;}
choutin 3:56b034c41dc5 225 else{right=0;}
choutin 3:56b034c41dc5 226
choutin 3:56b034c41dc5 227 if(left>0.5){y=1;}
choutin 3:56b034c41dc5 228 else{left=0;}
choutin 3:56b034c41dc5 229
choutin 3:56b034c41dc5 230 return x+2*(y);
choutin 3:56b034c41dc5 231 //(right,left)=(off,off),(on,off),(off,on),(on,on)
choutin 3:56b034c41dc5 232 // 0 1 2 3
choutin 3:56b034c41dc5 233 }
choutin 3:56b034c41dc5 234
choutin 3:56b034c41dc5 235
choutin 3:56b034c41dc5 236
choutin 3:56b034c41dc5 237 void initmotor(){
choutin 3:56b034c41dc5 238
choutin 3:56b034c41dc5 239
choutin 3:56b034c41dc5 240 M1cw.period_us(256);
choutin 3:56b034c41dc5 241 M1ccw.period_us(256);
choutin 3:56b034c41dc5 242 M2cw.period_us(256);
choutin 3:56b034c41dc5 243 M2ccw.period_us(256);
choutin 3:56b034c41dc5 244
choutin 3:56b034c41dc5 245 }
choutin 3:56b034c41dc5 246
choutin 3:56b034c41dc5 247 void move(int right,int left){
choutin 3:56b034c41dc5 248
choutin 3:56b034c41dc5 249 float rightduty,leftduty;
choutin 3:56b034c41dc5 250
choutin 3:56b034c41dc5 251 if(right>256){right=256;}
choutin 3:56b034c41dc5 252 if(left>256){left=256;}
choutin 3:56b034c41dc5 253 if(right<-256){right=-256;}
choutin 3:56b034c41dc5 254 if(left<-256){left=-256;}
choutin 3:56b034c41dc5 255
choutin 3:56b034c41dc5 256 rightduty=right/256.0;
choutin 3:56b034c41dc5 257 leftduty=left/256.0;
choutin 3:56b034c41dc5 258 if(right>0){
choutin 3:56b034c41dc5 259 M1cw.write(1-rightduty);
choutin 3:56b034c41dc5 260 M1ccw.write(1);
choutin 3:56b034c41dc5 261 }else{
choutin 3:56b034c41dc5 262 M1cw.write(1);
choutin 3:56b034c41dc5 263 M1ccw.write(1+rightduty);
choutin 3:56b034c41dc5 264 }
choutin 3:56b034c41dc5 265
choutin 3:56b034c41dc5 266 if(left>0){
choutin 3:56b034c41dc5 267 M2cw.write(1-leftduty);
choutin 3:56b034c41dc5 268 M2ccw.write(1);
choutin 3:56b034c41dc5 269 }else{
choutin 3:56b034c41dc5 270 M2cw.write(1);
choutin 3:56b034c41dc5 271 M2ccw.write(1+leftduty);
choutin 3:56b034c41dc5 272 }
choutin 3:56b034c41dc5 273 }
choutin 3:56b034c41dc5 274
choutin 3:56b034c41dc5 275
choutin 3:56b034c41dc5 276 int phase(void){
choutin 3:56b034c41dc5 277
choutin 3:56b034c41dc5 278 phase1.mode(PullUp);
choutin 3:56b034c41dc5 279 phase2.mode(PullUp);
choutin 3:56b034c41dc5 280 phase4.mode(PullUp);
choutin 3:56b034c41dc5 281 phase8.mode(PullUp);
choutin 3:56b034c41dc5 282
choutin 3:56b034c41dc5 283 int r=0;
choutin 3:56b034c41dc5 284
choutin 3:56b034c41dc5 285 r=phase1+2*phase2+4*phase4+8*phase8;
choutin 3:56b034c41dc5 286
choutin 3:56b034c41dc5 287 return r;
choutin 3:56b034c41dc5 288 }
choutin 3:56b034c41dc5 289
choutin 3:56b034c41dc5 290
choutin 3:56b034c41dc5 291 void teamLED(){
choutin 3:56b034c41dc5 292 teamSW.mode(PullUp);
choutin 3:56b034c41dc5 293 if(teamSW){
choutin 3:56b034c41dc5 294 teamledblue=1;
choutin 3:56b034c41dc5 295 teamledred=0;
choutin 3:56b034c41dc5 296 }else{
choutin 3:56b034c41dc5 297 teamledblue=0;
choutin 3:56b034c41dc5 298 teamledred=1;
choutin 3:56b034c41dc5 299
choutin 3:56b034c41dc5 300 }
choutin 3:56b034c41dc5 301 }