prime.h

00001 #include "QEI.h"
00002 
00003 #define PERIOD (20 * 1000) //(us)
00004 
00005 /*
00006 primeではPINを制御する関数を扱う。
00007 
00008 以下一覧
00009 open,close hand
00010 open close arm
00011 
00012 step 
00013 move
00014 blueorred
00015 lrsensor
00016 
00017 全関数共通で意図しない動作が起きたら基板上のLEDを点灯
00018 */
00019 PwmOut pwmarm(PC_6);
00020 PwmOut pwmhand(PC_8);
00021 
00022 AnalogIn rightsensor(A0);
00023 AnalogIn leftsensor(A1);
00024 
00025 AnalogIn armadj(A2);
00026 AnalogIn handadj(A3);
00027 
00028 DigitalIn teamSW(PC_11);
00029 DigitalOut teamledblue(PC_10);
00030 DigitalOut teamledred(PC_12);
00031 
00032 DigitalOut errorled(LED1);
00033 
00034 
00035 DigitalIn phase1(PB_8);
00036 DigitalIn phase2(PC_9);
00037 DigitalIn phase4(PB_9);
00038 DigitalIn phase8(PD_2);
00039 
00040 PwmOut M1cw(PA_11);
00041 PwmOut M1ccw(PB_15);
00042 PwmOut M2cw(PB_14);
00043 PwmOut M2ccw(PB_13);
00044 
00045 DigitalOut PINW(PA_3);
00046 DigitalOut PINX(PA_2);
00047 DigitalOut PINY(PA_10);
00048 DigitalOut PINZ(PB_3);
00049 
00050 QEI right (PA_7, PA_5, NC, 100, QEI::X2_ENCODING);
00051 QEI left  (PH_0, PC_14, NC, 100, QEI::X2_ENCODING);
00052 
00053 DigitalOut encordervcc1(PA_10);
00054 DigitalOut encordervcc2(PC_15);
00055 
00056 void initencorder(void){
00057     encordervcc1=1;
00058     encordervcc2=1;
00059     }
00060 
00061 
00062 
00063 void close_hand(void) {
00064      int i,degree;
00065 
00066     pwmhand.period_ms(20);        //20ms
00067     
00068         degree=175;
00069 
00070         i=500+degree*1900/180;
00071         pwmhand.write(i/PERIOD); 
00072         
00073         
00074 }
00075 
00076 void close_arm(void) {
00077     PwmOut mypwm(PB_3);
00078      int i,degree;
00079 
00080     mypwm.period_ms(20);        //20ms
00081     
00082         degree=160;
00083 
00084         i=500+degree*1900/180;
00085         pwmarm.write(i/PERIOD); 
00086         
00087         
00088 }
00089 
00090 
00091 
00092 void open_hand(void) {
00093     PwmOut mypwm(PWM_OUT);
00094      int i,degree;
00095 
00096     pwmhand.period_ms(20);        //20ms
00097     
00098         degree=90;
00099 
00100         i=500+degree*1900/180;
00101         mypwm.write(i/PERIOD); 
00102         
00103         
00104 }
00105 
00106 
00107 
00108 void open_arm(void) {
00109     PwmOut mypwm(PWM_OUT);
00110      int i,degree;
00111 
00112     mypwm.period_ms(20);        //20ms
00113     
00114         degree=10;
00115 
00116         i=500+degree*1900/180;
00117         pwmarm.write(i/PERIOD); 
00118         
00119         
00120 }
00121 
00122 void step(int degree){
00123     
00124     
00125 
00126     int puls_times=0;
00127 
00128     if(degree>0){
00129 
00130         puls_times=1+(int)(degree/(3.75));
00131         
00132         while(1){
00133             PINW=1;
00134             PINX=1;
00135             PINY=0;
00136             PINZ=0;
00137             wait_ms(20);
00138             puls_times--;
00139             
00140             if(puls_times<0){break;}
00141             
00142             PINW=0;
00143             PINX=1;
00144             PINY=1;
00145             PINZ=0;
00146             wait_ms(20);
00147             puls_times--;
00148             
00149             if(puls_times<0){break;}
00150             
00151             PINW=0;
00152             PINX=0;
00153             PINY=1;
00154             PINZ=1;
00155             wait_ms(20);
00156             puls_times--;
00157             
00158             if(puls_times<0){break;}
00159             
00160             PINW=1;
00161             PINX=1;
00162             PINY=0;
00163             PINZ=0;
00164             wait_ms(20);
00165             puls_times--;
00166             
00167             if(puls_times<0){break;}
00168         }
00169         
00170     }    
00171 
00172     if(degree<0){
00173 
00174         puls_times=(-1)*(1+(int)(degree/(3.75)));
00175         
00176         while(1){
00177             PINW=1;
00178             PINX=1;
00179             PINY=0;
00180             PINZ=0;
00181             wait_ms(20);
00182             puls_times--;
00183             
00184             if(puls_times<0){break;}
00185             
00186             PINW=1;
00187             PINX=0;
00188             PINY=0;
00189             PINZ=1;
00190             wait_ms(20);
00191             puls_times--;
00192             
00193             if(puls_times<0){break;}
00194             
00195             PINW=0;
00196             PINX=0;
00197             PINY=1;
00198             PINZ=1;
00199             wait_ms(20);
00200             puls_times--;
00201             
00202             if(puls_times<0){break;}
00203             
00204             PINW=0;
00205             PINX=1;
00206             PINY=1;
00207             PINZ=0;
00208             wait_ms(20);
00209             puls_times--;
00210             
00211             if(puls_times<0){break;}
00212         }
00213         
00214     }    
00215 
00216 }
00217 
00218 
00219 
00220 int sensor(void){
00221     //センサー読
00222     int right ,left,x,y;
00223     
00224     right=rightsensor.read();
00225     left=leftsensor.read();
00226     
00227     if(right>0.5){x=1;}
00228     else{right=0;}
00229     
00230     if(left>0.5){y=1;}
00231     else{left=0;}
00232     
00233     return x+2*(y);
00234             //(right,left)=(off,off),(on,off),(off,on),(on,on)
00235              //                 0        1         2       3
00236 }
00237 
00238 /*
00239 void move(int right,int left){
00240     
00241     if(right>256){right=256;}
00242     if(left>256){left=256;}
00243     if(right<-256){right=-256;}
00244     if(left<-256){left=-256;}
00245     
00246     PwmOut M1cw(PA_11);
00247     PwmOut M1ccw(PB_15);
00248     PwmOut M2cw(PB_14);
00249     PwmOut M2ccw(PB_13);
00250 
00251     M1cw.period_us(256);
00252     M1ccw.period_us(256);
00253     M2cw.period_us(256);
00254     M2ccw.period_us(256);
00255 
00256     
00257     right=256-right;
00258     left=256-left;//トランジスタ挿入によってON,OFFが逆転したための措置
00259     
00260     //回さないモーターにはperiod=widthを出力
00261         if(right>0&&left>0){
00262         
00263         M1cw.pulsewidth_us(right);
00264         M2cw.pulsewidth_us(left);
00265         M1ccw.pulsewidth_us(256);
00266         M2ccw.pulsewidth_us(256);        
00267     }
00268         
00269         else if(right<0&&left>0){
00270         
00271                 
00272         M1cw.pulsewidth_us(256);
00273         M2cw.pulsewidth_us(left);
00274         M1ccw.pulsewidth_us(right);
00275         M2ccw.pulsewidth_us(256);      
00276     }
00277         else if(right>0&&left<0){
00278         
00279         
00280         M1cw.pulsewidth_us(right);
00281         M2cw.pulsewidth_us(256);
00282         M1ccw.pulsewidth_us(256);
00283         M2ccw.pulsewidth_us(left);      
00284     }
00285         else if(right<0&&left<0){
00286         
00287         
00288         M1cw.pulsewidth_us(256);
00289         M2cw.pulsewidth_us(256);
00290         M1ccw.pulsewidth_us(right);
00291         M2ccw.pulsewidth_us(left);      
00292     }
00293         else if(right==0&&left==0){
00294         
00295         M1cw.pulsewidth_us(256);
00296         M2cw.pulsewidth_us(256);      
00297         M1ccw.pulsewidth_us(256);
00298         M2ccw.pulsewidth_us(256);
00299     }
00300 }
00301 */
00302 
00303 void initmotor(){
00304 
00305     
00306     M1cw.period_us(256);
00307     M1ccw.period_us(256);
00308     M2cw.period_us(256);
00309     M2ccw.period_us(256);
00310     
00311 }
00312 
00313 void move(int right,int left){
00314     
00315     if(right>256){right=256;}
00316     if(left>256){left=256;}
00317     if(right<-256){right=-256;}
00318     if(left<-256){left=-256;}
00319 
00320     
00321     //回さないモーターにはperiod=widthを出力
00322     if(right>0){
00323         M1cw.pulsewidth_us(256-right);
00324         M1ccw.pulsewidth_us(256);
00325     }else{
00326         M1cw.pulsewidth_us(256);
00327         M1ccw.pulsewidth_us(256+right);
00328     }
00329     if(left>0){
00330         M2cw.pulsewidth_us(256-left);
00331         M2ccw.pulsewidth_us(256);
00332     }else{
00333         M2cw.pulsewidth_us(256);
00334         M2ccw.pulsewidth_us(256+left);
00335     }
00336 }
00337     
00338     
00339 int phase(void){
00340     
00341     phase1.mode(PullUp);
00342     phase2.mode(PullUp);
00343     phase4.mode(PullUp);
00344     phase8.mode(PullUp);
00345 
00346     int r=0;
00347     
00348     r=phase1+2*phase2+4*phase4+8*phase8;
00349     
00350     return r;
00351 }
00352