MotorControl.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003  
00004 extern              Serial bt;
00005 DigitalOut          Enable(A0);
00006 DigitalOut          M1(PA_1);
00007 DigitalOut          M2(PF_1);
00008 
00009 
00010 extern              QEI re;
00011 extern              Ticker      timer1;
00012 extern int          targetDis;
00013 
00014 void MotorTest(int mode)
00015 {
00016     
00017     switch (mode)
00018     
00019     {
00020         case 0 : //stop
00021             
00022             Enable = 0;
00023             timer1.detach();
00024 
00025             break;
00026     
00027         case 1 : // right
00028     
00029             Enable = 1;
00030             M1 = 1;
00031             M2 = 0;            
00032     
00033             break;
00034     
00035         case 2 : // left
00036         
00037             Enable = 1;
00038             M1 = 0;
00039             M2 = 1;
00040         
00041             break;       
00042             
00043     }
00044     
00045               
00046 }
00047 
00048 void ControlPos()
00049 {
00050         
00051         int pulse = re.getPulses();
00052         int d = pulse/200;
00053        
00054         float ref_d = (targetDis-d);
00055         float abs_ref_d = abs(ref_d);
00056 //        float s;
00057 //        s = abs_ref_d; 
00058         
00059         if(-0.8f > ref_d)
00060         {
00061                 Enable = 1;
00062                 M1 = 0;
00063                 M2 = 1; //(s<3)?s/5:1;
00064                 
00065         }
00066         
00067         else if ( 0.8f <ref_d)
00068         {
00069                     
00070                 Enable = 1;
00071                 M1 = 1; //(s<3)?s/5:1;
00072                 M2 = 0;
00073                         
00074         }
00075         
00076         if (abs_ref_d < 0.8f)
00077         {
00078             
00079                 Enable = 0;
00080                 timer1.detach();
00081                   
00082         }
00083 }