File content as of revision 2:b204cf2f9b60:

#include "QEI.h"
/*
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);


QEI right (PA_7, PA_5, NC, 100, QEI::X2_ENCODING);
QEI left  (PA_13, PA_15, NC, 100, QEI::X2_ENCODING);

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;
}