涼太郎 中村
めいん
Diff: prime.h
- Revision:
- 0:df2659fd8031
- Child:
- 1:a1e592eca305
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/prime.h Sun Aug 28 17:03:34 2016 +0000
@@ -0,0 +1,297 @@
+/*
+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 team(PC_3);
+DigitalOut errorled(LED1);
+
+
+DigitalIn phase1(PC_14);
+DigitalIn phase2(PC_15);
+DigitalIn phase4(PH_0);
+DigitalIn phase8(PH_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){
+
+
+ DigitalOut PINW(PA_3);
+ DigitalOut PINX(PA_2);
+ DigitalOut PINY(PA_10);
+ DigitalOut PINZ(PB_3);
+
+ 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 move(int right,int left){
+
+ if(right>256){right=256;}
+ if(left>256){left=256;}
+ if(right<-256){right=-256;}
+ if(left<-256){left=-256;}
+
+ PwmOut M1cw(PA_11);
+ PwmOut M1ccw(PB_15);
+ PwmOut M2cw(PB_14);
+ PwmOut M2ccw(PB_13);
+
+ M1cw.period_us(256);
+ M1ccw.period_us(256);
+ M2cw.period_us(256);
+ M2ccw.period_us(256);
+
+
+ right=256-right;
+ left=256-left;//トランジスタ挿入によってON,OFFが逆転したための措置
+
+ //回さないモーターにはperiod=widthを出力
+ if(right>0&&left>0){
+
+ M1cw.pulsewidth_us(right);
+ M2cw.pulsewidth_us(left);
+ M1ccw.pulsewidth_us(256);
+ M2ccw.pulsewidth_us(256);
+ }
+
+ else if(right<0&&left>0){
+
+
+ M1cw.pulsewidth_us(256);
+ M2cw.pulsewidth_us(left);
+ M1ccw.pulsewidth_us(right);
+ M2ccw.pulsewidth_us(256);
+ }
+ else if(right>0&&left<0){
+
+
+ M1cw.pulsewidth_us(right);
+ M2cw.pulsewidth_us(256);
+ M1ccw.pulsewidth_us(256);
+ M2ccw.pulsewidth_us(left);
+ }
+ else if(right<0&&left<0){
+
+
+ M1cw.pulsewidth_us(256);
+ M2cw.pulsewidth_us(256);
+ M1ccw.pulsewidth_us(right);
+ M2ccw.pulsewidth_us(left);
+ }
+ else if(right==0&&left==0){
+
+ M1cw.pulsewidth_us(256);
+ M2cw.pulsewidth_us(256);
+ M1ccw.pulsewidth_us(256);
+ M2ccw.pulsewidth_us(256);
+ }
+}
+
+int blue(){
+ int x;
+ x=(-1)*team;
+ return x;
+ }
+
+
+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;
+}
+