main.cpp

00001 //****************************************************************************/
00002 // Includes
00003 //****************************************************************************/
00004 #include "PID.h"
00005 #include "QEI.h"
00006 
00007 //****************************************************************************/
00008 // Defines
00009 //****************************************************************************/
00010 #define RATE  0.01
00011 #define Kc   -2.6
00012 #define Ti    0.0
00013 #define Td    0.0
00014 
00015 //****************************************************************************/
00016 // Globals
00017 //****************************************************************************/
00018 //--------
00019 // Motors 
00020 //--------
00021 //Left motor.
00022 PwmOut leftMotor(p23);
00023 DigitalOut leftBrake(p19);
00024 DigitalOut leftDirection(p28);
00025 QEI leftQei(p29, p30, NC, 624);
00026 PID leftController(Kc, Ti, Td, RATE);
00027 //-------
00028 // Files
00029 //-------
00030 LocalFileSystem local("local");
00031 FILE* fp;
00032 //--------
00033 // Timers
00034 //--------
00035 Timer endTimer;
00036 //--------------------
00037 // Working variables.
00038 //--------------------
00039 volatile int leftPulses     = 0;
00040 volatile int leftPrevPulses = 0;
00041 volatile float leftPwmDuty  = 1.0;
00042 volatile float leftVelocity = 0.0;
00043 //Velocity to reach.
00044 int goal = 3000;
00045 
00046 //****************************************************************************/
00047 // Prototypes
00048 //****************************************************************************/
00049 //Set motors to go "forward", brake off, not moving.
00050 void initializeMotors(void);
00051 //Set up PID controllers with appropriate limits and biases.
00052 void initializePidControllers(void);
00053 
00054 void initializeMotors(void){
00055 
00056     leftMotor.period_us(50);
00057     leftMotor = 1.0;
00058     leftBrake = 0.0;
00059     leftDirection = 0;
00060 
00061 }
00062 
00063 void initializePidControllers(void){
00064 
00065     leftController.setInputLimits(0.0, 10500.0);
00066     leftController.setOutputLimits(0.0, 1.0);
00067     leftController.setBias(1.0);
00068     leftController.setMode(AUTO_MODE);
00069 
00070 }
00071 
00072 int main() {
00073 
00074     //Initialization.
00075     initializeMotors();
00076     initializePidControllers();
00077 
00078     //Open results file.
00079     fp = fopen("/local/pidtest.csv", "w");
00080     
00081     endTimer.start();
00082 
00083     //Set velocity set point.
00084     leftController.setSetPoint(goal);
00085 
00086     //Run for 3 seconds.
00087     while (endTimer.read() < 3.0){
00088         leftPulses = leftQei.getPulses();
00089         leftVelocity = (leftPulses - leftPrevPulses) / RATE;
00090         leftPrevPulses = leftPulses;
00091         leftController.setProcessValue(leftVelocity);
00092         leftPwmDuty = leftController.compute();
00093         leftMotor = leftPwmDuty;
00094         fprintf(fp, "%f,%f\n", leftVelocity, goal);
00095         wait(RATE);
00096     }
00097 
00098     //Stop motors.
00099     leftMotor  = 1.0;
00100     
00101     //Close results file.
00102     fclose(fp);
00103 
00104 }