koko

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 
00004 
00005 PwmOut MRA (PB_15);
00006 PwmOut MRB (PB_14);
00007 //Right
00008 PwmOut MLA (PB_13);
00009 PwmOut MLB (PA_11);
00010 //Left
00011 
00012 PwmOut s1 (PB_8);//包容力
00013 PwmOut s2 (PB_9);//raise
00014 
00015 BusIn Encoder(PA_6,PA_7,PA_8,PA_9);
00016 
00017 QEI Encoder_R(PA_6,PA_7,NC,1000,QEI::X2_ENCODING);
00018 QEI Encoder_L(PA_8,PA_9,NC,1000,QEI::X2_ENCODING);
00019 
00020 int main()
00021 {
00022     MRA.period(0.002);
00023     MRB.period(0.002);
00024     MLA.period(0.002);
00025     MLB.period(0.002);
00026 
00027     s1.period_us(20000);
00028     s2.period_us(20000);
00029     
00030     int OldEnc=0;
00031 
00032 
00033     while(1) {
00034 
00035         Encoder.mode(PullUp);
00036         //printf("%d %d\r\n",Encoder_R.getPulses(),-1*Encoder_L.getPulses());
00037 
00038 
00039         if(Encoder_R.getPulses() < 42024) {
00040             MRA = 0.4;
00041             MLA = 0.407;
00042         } else if (Encoder_R.getPulses() >= 42024) {
00043             MRA = 0;
00044             MLA = 0;
00045 
00046             wait (1.0);
00047 
00048             s1.pulsewidth_us(1700);
00049             wait(1);
00050 
00051             /*MRA=0;
00052             MLA=0;*/
00053             MRB=0.4;
00054             MLB=0.407;
00055 
00056         if(Encoder_R.getPulses() <= 42024-1200) {
00057 
00058             s2.pulsewidth_us(1300);
00059             MRB=0;
00060             MLB=0;
00061             wait(1);
00062             MRA=0;
00063             MLA=0;
00064             break;
00065             }
00066 
00067         } else;
00068 
00069 
00070     }
00071 }