File content as of revision 2:1a2984dfac3e:

#include "QEI.h"

#define PERIOD (20 * 1000) //(us)

/*
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 M2cw(PB_14);
PwmOut M2ccw(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  (PH_0, PC_14, NC, 100, QEI::X2_ENCODING);

DigitalOut encordervcc1(PA_10);
DigitalOut encordervcc2(PC_15);

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

void initmotor(){

    
    M1cw.period_us(256);
    M1ccw.period_us(256);
    M2cw.period_us(256);
    M2ccw.period_us(256);
    
}

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

    
    //回さないモーターにはperiod=widthを出力
    if(right>0){
        M1cw.pulsewidth_us(256-right);
        M1ccw.pulsewidth_us(256);
    }else{
        M1cw.pulsewidth_us(256);
        M1ccw.pulsewidth_us(256+right);
    }
    if(left>0){
        M2cw.pulsewidth_us(256-left);
        M2ccw.pulsewidth_us(256);
    }else{
        M2cw.pulsewidth_us(256);
        M2ccw.pulsewidth_us(256+left);
    }
}
    
    
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;
}