File content as of revision 0:df2659fd8031:

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