Diff: prime.h

Revision:

0:1c8034b7f565

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/prime.h	Thu Sep 01 06:38:13 2016 +0000
@@ -0,0 +1,292 @@
+
+/*
+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);
+
+const int PERIOD=20000;
+
+
+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.write(i/PERIOD); 
+       
+       
+}
+
+void close_arm(void) {
+   PwmOut mypwm(PB_3);
+    int i,degree;
+
+   mypwm.period_ms(20);        //20ms
+   
+       degree=160;
+
+       i=500+degree*1900/180;
+       pwmarm.write(i/PERIOD); 
+       
+       
+}
+
+
+
+void open_hand(void) {
+   PwmOut mypwm(PWM_OUT);
+    int i,degree;
+
+   pwmhand.period_ms(20);        //20ms
+   
+       degree=90;
+
+       i=500+degree*1900/180;
+       mypwm.write(i/PERIOD); 
+       
+       
+}
+
+
+
+void open_arm(void) {
+   PwmOut mypwm(PWM_OUT);
+    int i,degree;
+
+   mypwm.period_ms(20);        //20ms
+   
+       degree=10;
+
+       i=500+degree*1900/180;
+       pwmarm.write(i/PERIOD); 
+       
+       
+}
+
+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;
+}
+