涼太郎 中村
a
Diff: prime.h
- Revision:
- 3:56b034c41dc5
diff -r 4b2594dd86be -r 56b034c41dc5 prime.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/prime.h Fri Sep 02 06:50:14 2016 +0000
@@ -0,0 +1,301 @@
+
+/*
+primeではPINを制御する関数を扱う。
+
+以下一覧
+open,close hand
+open close arm
+
+step
+move
+blueorred
+lrsensor
+
+全関数共通で意図しない動作が起きたら基板上のLEDを点灯
+*/
+PwmOut pwmarm(PC_6);
+PwmOut pwmhand(PC_8);
+
+AnalogIn rightsensor(A0);
+AnalogIn leftsensor(A1);
+
+AnalogIn armadj(A2);
+AnalogIn handadj(A3);
+
+DigitalIn teamSW(PC_11);
+DigitalOut teamledblue(PC_10);
+DigitalOut teamledred(PC_12);
+
+DigitalOut errorled(LED1);
+
+
+DigitalIn phase1(PB_8);
+DigitalIn phase2(PC_9);
+DigitalIn phase4(PB_9);
+DigitalIn phase8(PD_2);
+
+PwmOut M1cw(PA_11);
+PwmOut M1ccw(PB_15);
+PwmOut M2ccw(PB_14);
+PwmOut M2cw(PB_13);
+
+DigitalOut PINW(PA_3);
+DigitalOut PINX(PA_2);
+DigitalOut PINY(PA_10);
+DigitalOut PINZ(PB_3);
+
+
+
+
+DigitalOut encordervcc1(PA_6);
+DigitalOut encordervcc2(PA_14);
+
+void initencorder(void){
+ encordervcc1=1;
+ encordervcc2=1;
+ }
+
+
+
+void close_hand(void) {
+ int i,degree;
+
+ pwmhand.period_ms(20); //20ms
+
+ degree=175;
+
+ i=500+degree*1900/180;
+ pwmhand.pulsewidth_us(i);
+
+
+}
+
+void close_arm(void) {
+ PwmOut mypwm(PB_3);
+ int i,degree;
+
+ mypwm.period_ms(20); //20ms
+
+ degree=160;
+
+ i=500+degree*1900/180;
+ pwmarm.pulsewidth_us(i);
+
+
+}
+
+
+
+void open_hand(void) {
+ PwmOut mypwm(PWM_OUT);
+ int i,degree;
+
+ pwmhand.period_ms(20); //20ms
+
+ degree=90;
+
+ i=500+degree*1900/180;
+ mypwm.pulsewidth_us(i);
+
+
+}
+
+
+
+void open_arm(void) {
+ PwmOut mypwm(PWM_OUT);
+ int i,degree;
+
+ mypwm.period_ms(20); //20ms
+
+ degree=10;
+
+ i=500+degree*1900/180;
+ pwmarm.pulsewidth_us(i);
+
+
+}
+
+void step(int degree){
+
+
+
+ int puls_times=0;
+
+ if(degree>0){
+
+ puls_times=1+(int)(degree/(3.75));
+
+ while(1){
+ PINW=1;
+ PINX=1;
+ PINY=0;
+ PINZ=0;
+ wait_ms(20);
+ puls_times--;
+
+ if(puls_times<0){break;}
+
+ PINW=0;
+ PINX=1;
+ PINY=1;
+ PINZ=0;
+ wait_ms(20);
+ puls_times--;
+
+ if(puls_times<0){break;}
+
+ PINW=0;
+ PINX=0;
+ PINY=1;
+ PINZ=1;
+ wait_ms(20);
+ puls_times--;
+
+ if(puls_times<0){break;}
+
+ PINW=1;
+ PINX=1;
+ PINY=0;
+ PINZ=0;
+ wait_ms(20);
+ puls_times--;
+
+ if(puls_times<0){break;}
+ }
+
+ }
+
+ if(degree<0){
+
+ puls_times=(-1)*(1+(int)(degree/(3.75)));
+
+ while(1){
+ PINW=1;
+ PINX=1;
+ PINY=0;
+ PINZ=0;
+ wait_ms(20);
+ puls_times--;
+
+ if(puls_times<0){break;}
+
+ PINW=1;
+ PINX=0;
+ PINY=0;
+ PINZ=1;
+ wait_ms(20);
+ puls_times--;
+
+ if(puls_times<0){break;}
+
+ PINW=0;
+ PINX=0;
+ PINY=1;
+ PINZ=1;
+ wait_ms(20);
+ puls_times--;
+
+ if(puls_times<0){break;}
+
+ PINW=0;
+ PINX=1;
+ PINY=1;
+ PINZ=0;
+ wait_ms(20);
+ puls_times--;
+
+ if(puls_times<0){break;}
+ }
+
+ }
+
+}
+
+
+
+int sensor(void){
+ //センサー読
+ int right ,left,x,y;
+
+ right=rightsensor.read();
+ left=leftsensor.read();
+
+ if(right>0.5){x=1;}
+ else{right=0;}
+
+ if(left>0.5){y=1;}
+ else{left=0;}
+
+ return x+2*(y);
+ //(right,left)=(off,off),(on,off),(off,on),(on,on)
+ // 0 1 2 3
+}
+
+
+
+void initmotor(){
+
+
+ M1cw.period_us(256);
+ M1ccw.period_us(256);
+ M2cw.period_us(256);
+ M2ccw.period_us(256);
+
+}
+
+void move(int right,int left){
+
+ float rightduty,leftduty;
+
+ if(right>256){right=256;}
+ if(left>256){left=256;}
+ if(right<-256){right=-256;}
+ if(left<-256){left=-256;}
+
+ rightduty=right/256.0;
+ leftduty=left/256.0;
+ if(right>0){
+ M1cw.write(1-rightduty);
+ M1ccw.write(1);
+ }else{
+ M1cw.write(1);
+ M1ccw.write(1+rightduty);
+ }
+
+ if(left>0){
+ M2cw.write(1-leftduty);
+ M2ccw.write(1);
+ }else{
+ M2cw.write(1);
+ M2ccw.write(1+leftduty);
+ }
+}
+
+
+int phase(void){
+
+ phase1.mode(PullUp);
+ phase2.mode(PullUp);
+ phase4.mode(PullUp);
+ phase8.mode(PullUp);
+
+ int r=0;
+
+ r=phase1+2*phase2+4*phase4+8*phase8;
+
+ return r;
+}
+
+
+void teamLED(){
+ teamSW.mode(PullUp);
+ if(teamSW){
+ teamledblue=1;
+ teamledred=0;
+ }else{
+ teamledblue=0;
+ teamledred=1;
+
+ }
+}
\ No newline at end of file