main.cpp

00001 #include "PID.h"
00002 #include "QEI.h"
00003 #define RATE  0.01
00004 #define Kc    0.473
00005 #define Ti    0.025
00006 #define Td    0.0
00007 
00008 PwmOut Motor(p21);
00009 QEI Qei(p29, p30, NC, 504);
00010 PID Controller(Kc, Ti, Td, RATE);
00011 //-------
00012 // Datoteke
00013 //-------
00014 LocalFileSystem local("local");
00015 FILE* fp;
00016 
00017 Timer endTimer;
00018 
00019 //Radne varijable
00020 
00021 volatile int Pulses     = 0;
00022 volatile int PrevPulses = 0;
00023 volatile float PwmDuty  = 0.0;
00024 volatile float Velocity = 0.0;
00025 //Zadana brzina
00026 int goal = 21000;
00027  
00028 
00029 void initializeMotors(void);
00030 void initializePidControllers(void);
00031 void initializeMotors(void){
00032  
00033     Motor.period_us(50);
00034     Motor = 1.0;
00035 }
00036  
00037 void initializePidControllers(void){
00038  
00039     Controller.setInputLimits(0.0, 54500);
00040     Controller.setOutputLimits(0.0, 1.0);
00041     Controller.setBias(1.0);
00042     Controller.setMode(AUTO_MODE);
00043 }
00044  
00045 int main() {
00046  wait(2);
00047     //Inicijalizacija
00048     initializeMotors();
00049     initializePidControllers();
00050  
00051     //Kreiranje datoteke za spremanje rezultata.
00052     fp = fopen("/local/pidtest.csv", "w");
00053     
00054     endTimer.start();
00055  
00056     Controller.setSetPoint(goal);
00057  
00058     //Vrijeme mjerenja je 3 sekunde
00059     while (endTimer.read() < 3.0){
00060         Pulses = Qei.getPulses();
00061         Velocity = (Pulses - PrevPulses) / RATE;
00062         PrevPulses = Pulses;
00063         Controller.setProcessValue(Velocity);
00064         PwmDuty = Controller.compute();
00065         Motor = PwmDuty;
00066         fprintf(fp, "%f,%f,\n", Velocity, PwmDuty);
00067         wait(RATE);
00068     }
00069     //Spremanje izmjerenih podataka
00070     fclose(fp);
00071 }