Aaron Berk / Mbed 2 deprecated PID_VelocityExample

Proportional, integral, derivative velocity control of a motor.

Dependencies:   mbed QEI PID

main.cpp@0:9bca35ae9c6b, 2010-08-03 (annotated)

Committer:
aberk
Date:
Tue Aug 03 09:24:06 2010 +0000
Revision:
0:9bca35ae9c6b
Child:
1:ac598811dd00
Version 1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aberk 0:9bca35ae9c6b 1 //****************************************************************************/
aberk 0:9bca35ae9c6b 2 // Includes
aberk 0:9bca35ae9c6b 3 //****************************************************************************/
aberk 0:9bca35ae9c6b 4 #include "PID.h"
aberk 0:9bca35ae9c6b 5 #include "QEI.h"
aberk 0:9bca35ae9c6b 6
aberk 0:9bca35ae9c6b 7 //****************************************************************************/
aberk 0:9bca35ae9c6b 8 // Defines
aberk 0:9bca35ae9c6b 9 //****************************************************************************/
aberk 0:9bca35ae9c6b 10 #define RATE 0.01
aberk 0:9bca35ae9c6b 11 #define Kc -2.6
aberk 0:9bca35ae9c6b 12 #define Ti 0.0
aberk 0:9bca35ae9c6b 13 #define Td 0.0
aberk 0:9bca35ae9c6b 14
aberk 0:9bca35ae9c6b 15 //****************************************************************************/
aberk 0:9bca35ae9c6b 16 // Globals
aberk 0:9bca35ae9c6b 17 //****************************************************************************/
aberk 0:9bca35ae9c6b 18 //--------
aberk 0:9bca35ae9c6b 19 // Motors
aberk 0:9bca35ae9c6b 20 //--------
aberk 0:9bca35ae9c6b 21 //Left motor.
aberk 0:9bca35ae9c6b 22 PwmOut leftMotor(p23);
aberk 0:9bca35ae9c6b 23 DigitalOut leftBrake(p19);
aberk 0:9bca35ae9c6b 24 DigitalOut leftDirection(p28);
aberk 0:9bca35ae9c6b 25 QEI leftQei(p29, p30, NC, 624);
aberk 0:9bca35ae9c6b 26 PID leftController(Kc, Ti, Td, RATE);
aberk 0:9bca35ae9c6b 27 //-------
aberk 0:9bca35ae9c6b 28 // Files
aberk 0:9bca35ae9c6b 29 //-------
aberk 0:9bca35ae9c6b 30 LocalFileSystem local("local");
aberk 0:9bca35ae9c6b 31 FILE* fp;
aberk 0:9bca35ae9c6b 32 //--------
aberk 0:9bca35ae9c6b 33 // Timers
aberk 0:9bca35ae9c6b 34 //--------
aberk 0:9bca35ae9c6b 35 Timer endTimer;
aberk 0:9bca35ae9c6b 36 //--------------------
aberk 0:9bca35ae9c6b 37 // Working variables.
aberk 0:9bca35ae9c6b 38 //--------------------
aberk 0:9bca35ae9c6b 39 volatile int leftPulses = 0;
aberk 0:9bca35ae9c6b 40 volatile int leftPrevPulses = 0;
aberk 0:9bca35ae9c6b 41 volatile float leftPwmDuty = 1.0;
aberk 0:9bca35ae9c6b 42 volatile float leftVelocity = 0.0;
aberk 0:9bca35ae9c6b 43 //Velocity to reach.
aberk 0:9bca35ae9c6b 44 int goal = 3000;
aberk 0:9bca35ae9c6b 45
aberk 0:9bca35ae9c6b 46 //****************************************************************************/
aberk 0:9bca35ae9c6b 47 // Prototypes
aberk 0:9bca35ae9c6b 48 //****************************************************************************/
aberk 0:9bca35ae9c6b 49 //Set motors to go "forward", brake off, not moving.
aberk 0:9bca35ae9c6b 50 void initializeMotors(void);
aberk 0:9bca35ae9c6b 51 //Set up PID controllers with appropriate limits and biases.
aberk 0:9bca35ae9c6b 52 void initializePidControllers(void);
aberk 0:9bca35ae9c6b 53
aberk 0:9bca35ae9c6b 54 void initializeMotors(void){
aberk 0:9bca35ae9c6b 55
aberk 0:9bca35ae9c6b 56 leftMotor.period_us(50);
aberk 0:9bca35ae9c6b 57 leftMotor = 1.0;
aberk 0:9bca35ae9c6b 58 leftBrake = 0.0;
aberk 0:9bca35ae9c6b 59 leftDirection = 0;
aberk 0:9bca35ae9c6b 60
aberk 0:9bca35ae9c6b 61 }
aberk 0:9bca35ae9c6b 62
aberk 0:9bca35ae9c6b 63 void initializePidControllers(void){
aberk 0:9bca35ae9c6b 64
aberk 0:9bca35ae9c6b 65 leftController.setInputLimits(0.0, 10500.0);
aberk 0:9bca35ae9c6b 66 leftController.setOutputLimits(0.0, 1.0);
aberk 0:9bca35ae9c6b 67 leftController.setBias(1.0);
aberk 0:9bca35ae9c6b 68 leftController.setMode(AUTO_MODE);
aberk 0:9bca35ae9c6b 69
aberk 0:9bca35ae9c6b 70 }
aberk 0:9bca35ae9c6b 71
aberk 0:9bca35ae9c6b 72 int main() {
aberk 0:9bca35ae9c6b 73
aberk 0:9bca35ae9c6b 74 //Initialization.
aberk 0:9bca35ae9c6b 75 initializeMotors();
aberk 0:9bca35ae9c6b 76 initializePidControllers();
aberk 0:9bca35ae9c6b 77
aberk 0:9bca35ae9c6b 78 //Open results file.
aberk 0:9bca35ae9c6b 79 fp = fopen("/local/pidtest.csv", "w");
aberk 0:9bca35ae9c6b 80
aberk 0:9bca35ae9c6b 81 endTimer.start();
aberk 0:9bca35ae9c6b 82
aberk 0:9bca35ae9c6b 83 //Set velocity set point.
aberk 0:9bca35ae9c6b 84 leftController.setSetPoint(goal);
aberk 0:9bca35ae9c6b 85
aberk 0:9bca35ae9c6b 86 //Run for 3 seconds.
aberk 0:9bca35ae9c6b 87 while (endTimer.read() < 3.0){
aberk 0:9bca35ae9c6b 88 leftPulses = leftQei.getPulses();
aberk 0:9bca35ae9c6b 89 leftVelocity = (leftPulses - leftPrevPulses) / RATE;
aberk 0:9bca35ae9c6b 90 leftPrevPulses = leftPulses;
aberk 0:9bca35ae9c6b 91 leftController.setProcessValue(leftVelocity);
aberk 0:9bca35ae9c6b 92 leftPwmDuty = leftController.getRealOutput();
aberk 0:9bca35ae9c6b 93 leftMotor = leftPwmDuty;
aberk 0:9bca35ae9c6b 94 fprintf(fp, "%f,%f\n", leftVelocity, goal);
aberk 0:9bca35ae9c6b 95 wait(RATE);
aberk 0:9bca35ae9c6b 96 }
aberk 0:9bca35ae9c6b 97
aberk 0:9bca35ae9c6b 98 //Stop motors.
aberk 0:9bca35ae9c6b 99 leftMotor = 1.0;
aberk 0:9bca35ae9c6b 100
aberk 0:9bca35ae9c6b 101 //Close results file.
aberk 0:9bca35ae9c6b 102 fclose(fp);
aberk 0:9bca35ae9c6b 103
aberk 0:9bca35ae9c6b 104 }