Aaron Berk
/
PID_VelocityExample
Proportional, integral, derivative velocity control of a motor.
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Tue Jul 12 2022 18:57:26 by 1.7.2