main.cpp

00001 #include "PID.h"
00002 #include "QEI.h"
00003 #include "TextLCD.h"
00004 
00005 
00006 
00007 #define RATE  0.01
00008 #define Kc    0.473
00009 #define Ti    0.025
00010 #define Td    0.0
00011 
00012 TextLCD lcd(p15, p16, p17, p18, p19, p20, TextLCD::LCD16x2); // rs, e, d4-d7
00013 // Motor
00014 PwmOut Motor(p21);
00015 QEI Qei(p29, p30, NC, 504);
00016 PID Controller(Kc, Ti, Td, RATE);
00017 
00018 // Definiranje konstanti
00019 
00020 
00021 Timer t;                        
00022 int startTimeUs, endTimeUs;     
00023 int deltaUs = 0;                
00024 int startPulse, endPulse;       
00025 int deltaPulse = 0;             
00026 int i = 0;
00027 
00028 
00029 
00030 int Pulses     = 0;
00031 int PrevPulses = 0;
00032 float PwmDuty  = 1.0;
00033 float Velocity = 0.0;
00034 //Velocity to reach.
00035 int goal = 21000;
00036 
00037 
00038 
00039 // Deklaracija funkcija
00040 
00041 //Set motors to go "forward".
00042 void initializeMotors(void);
00043 //Set up PID controllers with appropriate limits and biases.
00044 void initializePidControllers(void);
00045 
00046 float calcRpm(float deltaTimeSec, int deltaPulse);          
00047 
00048 float calcRpm(float deltaTimeSec, int deltaPulse)           
00049 {                                                           
00050      return 60.0 * ((deltaPulse / deltaTimeSec) / 1008 );   
00051 }                                                           
00052 
00053 
00054 void initializeMotors(void)
00055 {
00056 
00057     Motor.period_us(50);
00058     Motor = 1.0;
00059 
00060 }
00061 
00062 void initializePidControllers(void)
00063 {
00064 
00065     Controller.setInputLimits(0.0, 54500.0);
00066     Controller.setOutputLimits(0.0, 1.0);
00067     Controller.setBias(1.0);
00068     Controller.setMode(AUTO_MODE);
00069 
00070 }
00071 
00072 int main()
00073 {
00074 
00075     //Initialization.
00076     initializeMotors();
00077     initializePidControllers();
00078 //Set velocity set point.
00079     Controller.setSetPoint(goal);
00080 
00081     startTimeUs = t.read_us();              
00082     startPulse = Qei.getPulses();           
00083     t.start();                              
00084     
00085     
00086     while (1) {
00087         
00088         
00089         Pulses = Qei.getPulses();
00090         Velocity = (Pulses - PrevPulses) / RATE;
00091         PrevPulses = Pulses;
00092         Controller.setProcessValue(Velocity);
00093         PwmDuty = Controller.compute();
00094         Motor = PwmDuty;
00095         
00096         //Ispis brzine na TeraTerm
00097         if(i > 30) {
00098         endTimeUs = t.read_us();                                    
00099         // endPulse = Qei.getPulses();                                
00100         deltaUs = endTimeUs - startTimeUs;                          
00101         deltaPulse =Pulses - startPulse;
00102         lcd.locate(0,0);                         
00103         lcd.printf("N = %.0f\n",calcRpm(deltaUs/1000000.0, deltaPulse)); 
00104         lcd.printf(" okr/min"); 
00105         startPulse = Qei.getPulses();                               
00106         startTimeUs = t.read_us();
00107         i = 0;                                  
00108         }
00109         i = i+1;
00110         wait(RATE);
00111         
00112     }
00113 }