Numero Uno / Mbed 2 deprecated PID_VelocityExample

motor controller with P velocity control

Dependencies:   HIDScope PID QEI mbed EMG

Fork of PID_VelocityExample by Aaron Berk

main.cpp@6:f58052f57505, 2015-09-25 (annotated)

Committer:
ewoud
Date:
Fri Sep 25 13:41:59 2015 +0000
Revision:
6:f58052f57505
Parent:
5:8ae6d935a16a
Child:
7:e14e28d8cae3
improved graph detail and control values

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"
ewoud 4:be465e9a12cb 6 #include "HIDScope.h" // Require the HIDScope library
aberk 0:9bca35ae9c6b 7
aberk 0:9bca35ae9c6b 8 //****************************************************************************/
aberk 0:9bca35ae9c6b 9 // Defines
aberk 0:9bca35ae9c6b 10 //****************************************************************************/
aberk 0:9bca35ae9c6b 11 #define RATE 0.01
ewoud 6:f58052f57505 12 #define Kc 0.2
aberk 0:9bca35ae9c6b 13 #define Ti 0.0
aberk 0:9bca35ae9c6b 14 #define Td 0.0
aberk 0:9bca35ae9c6b 15
aberk 0:9bca35ae9c6b 16 //****************************************************************************/
aberk 0:9bca35ae9c6b 17 // Globals
aberk 0:9bca35ae9c6b 18 //****************************************************************************/
ewoud 2:b2ccd9f044bb 19 Serial pc(USBTX, USBRX);
ewoud 5:8ae6d935a16a 20 HIDScope scope(3); // Instantize a 2-channel HIDScope object
ewoud 4:be465e9a12cb 21 Ticker scopeTimer; // Instantize the timer for sending data to the PC
aberk 0:9bca35ae9c6b 22 //--------
aberk 0:9bca35ae9c6b 23 // Motors
aberk 0:9bca35ae9c6b 24 //--------
aberk 0:9bca35ae9c6b 25 //Left motor.
ewoud 2:b2ccd9f044bb 26 PwmOut leftMotor(D5);
ewoud 2:b2ccd9f044bb 27 DigitalOut leftBrake(D3);
ewoud 2:b2ccd9f044bb 28 DigitalOut leftDirection(D4);
ewoud 2:b2ccd9f044bb 29 QEI leftQei(D12, D13, NC, 624);
aberk 0:9bca35ae9c6b 30 PID leftController(Kc, Ti, Td, RATE);
ewoud 3:4c93be3a9010 31 AnalogIn pot1(A0);
aberk 0:9bca35ae9c6b 32 //-------
aberk 0:9bca35ae9c6b 33 // Files
aberk 0:9bca35ae9c6b 34 //-------
ewoud 2:b2ccd9f044bb 35 //LocalFileSystem local("local");
ewoud 2:b2ccd9f044bb 36 //FILE* fp;
aberk 0:9bca35ae9c6b 37 //--------
aberk 0:9bca35ae9c6b 38 // Timers
aberk 0:9bca35ae9c6b 39 //--------
aberk 0:9bca35ae9c6b 40 Timer endTimer;
aberk 0:9bca35ae9c6b 41 //--------------------
aberk 0:9bca35ae9c6b 42 // Working variables.
aberk 0:9bca35ae9c6b 43 //--------------------
aberk 0:9bca35ae9c6b 44 volatile int leftPulses = 0;
aberk 0:9bca35ae9c6b 45 volatile int leftPrevPulses = 0;
aberk 0:9bca35ae9c6b 46 volatile float leftPwmDuty = 1.0;
ewoud 5:8ae6d935a16a 47 volatile float leftPwmDutyPrev = 1.0;
ewoud 5:8ae6d935a16a 48 volatile float leftPwmDutyChange;
aberk 0:9bca35ae9c6b 49 volatile float leftVelocity = 0.0;
aberk 0:9bca35ae9c6b 50 //Velocity to reach.
aberk 0:9bca35ae9c6b 51 int goal = 3000;
ewoud 5:8ae6d935a16a 52 float request;
aberk 0:9bca35ae9c6b 53
aberk 0:9bca35ae9c6b 54 //****************************************************************************/
aberk 0:9bca35ae9c6b 55 // Prototypes
aberk 0:9bca35ae9c6b 56 //****************************************************************************/
aberk 0:9bca35ae9c6b 57 //Set motors to go "forward", brake off, not moving.
aberk 0:9bca35ae9c6b 58 void initializeMotors(void);
aberk 0:9bca35ae9c6b 59 //Set up PID controllers with appropriate limits and biases.
aberk 0:9bca35ae9c6b 60 void initializePidControllers(void);
aberk 0:9bca35ae9c6b 61
aberk 0:9bca35ae9c6b 62 void initializeMotors(void){
aberk 0:9bca35ae9c6b 63
aberk 0:9bca35ae9c6b 64 leftMotor.period_us(50);
aberk 0:9bca35ae9c6b 65 leftMotor = 1.0;
aberk 0:9bca35ae9c6b 66 leftBrake = 0.0;
ewoud 4:be465e9a12cb 67 leftDirection = 1;
aberk 0:9bca35ae9c6b 68
aberk 0:9bca35ae9c6b 69 }
aberk 0:9bca35ae9c6b 70
aberk 0:9bca35ae9c6b 71 void initializePidControllers(void){
aberk 0:9bca35ae9c6b 72
ewoud 5:8ae6d935a16a 73 leftController.setInputLimits(0.0, 30000.0);
aberk 0:9bca35ae9c6b 74 leftController.setOutputLimits(0.0, 1.0);
ewoud 5:8ae6d935a16a 75 leftController.setBias(0.0);
aberk 0:9bca35ae9c6b 76 leftController.setMode(AUTO_MODE);
aberk 0:9bca35ae9c6b 77
aberk 0:9bca35ae9c6b 78 }
aberk 0:9bca35ae9c6b 79
aberk 0:9bca35ae9c6b 80 int main() {
ewoud 6:f58052f57505 81 //scopeTimer.attach_us(&scope, &HIDScope::send, 1e4);
aberk 0:9bca35ae9c6b 82 //Initialization.
aberk 0:9bca35ae9c6b 83 initializeMotors();
aberk 0:9bca35ae9c6b 84 initializePidControllers();
aberk 0:9bca35ae9c6b 85
aberk 0:9bca35ae9c6b 86 //Open results file.
ewoud 2:b2ccd9f044bb 87 //fp = fopen("/local/pidtest.csv", "w");
aberk 0:9bca35ae9c6b 88
aberk 0:9bca35ae9c6b 89 endTimer.start();
aberk 0:9bca35ae9c6b 90
aberk 0:9bca35ae9c6b 91 //Set velocity set point.
ewoud 3:4c93be3a9010 92
ewoud 4:be465e9a12cb 93
ewoud 2:b2ccd9f044bb 94
aberk 0:9bca35ae9c6b 95 //Run for 3 seconds.
ewoud 4:be465e9a12cb 96 while (endTimer.read() < 100){
ewoud 5:8ae6d935a16a 97 request = pot1.read()*30000;
ewoud 5:8ae6d935a16a 98 leftController.setSetPoint(request);
aberk 0:9bca35ae9c6b 99 leftPulses = leftQei.getPulses();
aberk 0:9bca35ae9c6b 100 leftVelocity = (leftPulses - leftPrevPulses) / RATE;
aberk 0:9bca35ae9c6b 101 leftPrevPulses = leftPulses;
ewoud 2:b2ccd9f044bb 102
ewoud 5:8ae6d935a16a 103
ewoud 4:be465e9a12cb 104
ewoud 5:8ae6d935a16a 105 leftController.setProcessValue(leftVelocity);
aberk 1:ac598811dd00 106 leftPwmDuty = leftController.compute();
aberk 0:9bca35ae9c6b 107 leftMotor = leftPwmDuty;
ewoud 5:8ae6d935a16a 108
ewoud 5:8ae6d935a16a 109 scope.set(0, request);
ewoud 5:8ae6d935a16a 110 scope.set(1, leftPwmDuty);
ewoud 5:8ae6d935a16a 111 scope.set(2, leftVelocity);
ewoud 6:f58052f57505 112 scope.send();
ewoud 5:8ae6d935a16a 113 pc.printf("request: %f, lefVelocity: %f, output: %f \n\r",request,leftVelocity,leftPwmDuty);
ewoud 2:b2ccd9f044bb 114
ewoud 2:b2ccd9f044bb 115 //fprintf(fp, "%f,%f\n", leftVelocity, goal);
aberk 0:9bca35ae9c6b 116 wait(RATE);
aberk 0:9bca35ae9c6b 117 }
aberk 0:9bca35ae9c6b 118
aberk 0:9bca35ae9c6b 119 //Stop motors.
ewoud 2:b2ccd9f044bb 120 leftMotor = 0;
aberk 0:9bca35ae9c6b 121
aberk 0:9bca35ae9c6b 122 //Close results file.
ewoud 2:b2ccd9f044bb 123 //fclose(fp);
aberk 0:9bca35ae9c6b 124
aberk 0:9bca35ae9c6b 125 }