main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "PID.h"
00004 
00005 QEI enc1(PA_8, PA_9, NC, 100, QEI::X4_ENCODING);
00006 DigitalOut led(LED1);
00007 PwmOut A(D5);
00008 PwmOut B(D6);
00009 DigitalOut stop(D13);
00010 Serial pc(USBTX, USBRX, 115200);
00011 
00012 PID rori(2.0,0,0,0.01);
00013 
00014 int main() {
00015     stop=1;
00016     float Angle,AngleSet,power=0.0;
00017     
00018     rori.setInputLimits(-180.0,180.0);
00019     rori.setOutputLimits(-1.0,1.0);
00020     rori.setBias(0);
00021     rori.setMode(1);
00022     rori.setSetPoint(60.0);//
00023     
00024     AngleSet = (float)enc1.getPulses();
00025     while(1){
00026         Angle = (float)enc1.getPulses()-AngleSet;
00027         rori.setProcessValue(Angle);
00028         if(Angle>=60){
00029             rori.setSetPoint(-60.0);
00030         }else if(Angle<=-60){
00031             rori.setSetPoint(60.0);
00032         }
00033         power=rori.compute();
00034         if(power>0){
00035             A=0;
00036             B=power;
00037         }else{
00038             A=-1*power;
00039             B=0;
00040         }
00041         wait(0.01);
00042         pc.printf("%.2f\r\n",Angle);
00043     }
00044 }