motor controller with P velocity control

Dependencies:   HIDScope PID QEI mbed EMG

Fork of PID_VelocityExample by Aaron Berk

Committer:
ewoud
Date:
Thu Sep 24 11:25:06 2015 +0000
Revision:
3:4c93be3a9010
Parent:
2:b2ccd9f044bb
Child:
4:be465e9a12cb
included potmeter

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 //****************************************************************************/
ewoud 2:b2ccd9f044bb 18 Serial pc(USBTX, USBRX);
aberk 0:9bca35ae9c6b 19 //--------
aberk 0:9bca35ae9c6b 20 // Motors
aberk 0:9bca35ae9c6b 21 //--------
aberk 0:9bca35ae9c6b 22 //Left motor.
ewoud 2:b2ccd9f044bb 23 PwmOut leftMotor(D5);
ewoud 2:b2ccd9f044bb 24 DigitalOut leftBrake(D3);
ewoud 2:b2ccd9f044bb 25 DigitalOut leftDirection(D4);
ewoud 2:b2ccd9f044bb 26 QEI leftQei(D12, D13, NC, 624);
aberk 0:9bca35ae9c6b 27 PID leftController(Kc, Ti, Td, RATE);
ewoud 3:4c93be3a9010 28 AnalogIn pot1(A0);
aberk 0:9bca35ae9c6b 29 //-------
aberk 0:9bca35ae9c6b 30 // Files
aberk 0:9bca35ae9c6b 31 //-------
ewoud 2:b2ccd9f044bb 32 //LocalFileSystem local("local");
ewoud 2:b2ccd9f044bb 33 //FILE* fp;
aberk 0:9bca35ae9c6b 34 //--------
aberk 0:9bca35ae9c6b 35 // Timers
aberk 0:9bca35ae9c6b 36 //--------
aberk 0:9bca35ae9c6b 37 Timer endTimer;
aberk 0:9bca35ae9c6b 38 //--------------------
aberk 0:9bca35ae9c6b 39 // Working variables.
aberk 0:9bca35ae9c6b 40 //--------------------
aberk 0:9bca35ae9c6b 41 volatile int leftPulses = 0;
aberk 0:9bca35ae9c6b 42 volatile int leftPrevPulses = 0;
aberk 0:9bca35ae9c6b 43 volatile float leftPwmDuty = 1.0;
aberk 0:9bca35ae9c6b 44 volatile float leftVelocity = 0.0;
aberk 0:9bca35ae9c6b 45 //Velocity to reach.
aberk 0:9bca35ae9c6b 46 int goal = 3000;
ewoud 3:4c93be3a9010 47 float measure;
aberk 0:9bca35ae9c6b 48
aberk 0:9bca35ae9c6b 49 //****************************************************************************/
aberk 0:9bca35ae9c6b 50 // Prototypes
aberk 0:9bca35ae9c6b 51 //****************************************************************************/
aberk 0:9bca35ae9c6b 52 //Set motors to go "forward", brake off, not moving.
aberk 0:9bca35ae9c6b 53 void initializeMotors(void);
aberk 0:9bca35ae9c6b 54 //Set up PID controllers with appropriate limits and biases.
aberk 0:9bca35ae9c6b 55 void initializePidControllers(void);
aberk 0:9bca35ae9c6b 56
aberk 0:9bca35ae9c6b 57 void initializeMotors(void){
aberk 0:9bca35ae9c6b 58
aberk 0:9bca35ae9c6b 59 leftMotor.period_us(50);
aberk 0:9bca35ae9c6b 60 leftMotor = 1.0;
aberk 0:9bca35ae9c6b 61 leftBrake = 0.0;
aberk 0:9bca35ae9c6b 62 leftDirection = 0;
aberk 0:9bca35ae9c6b 63
aberk 0:9bca35ae9c6b 64 }
aberk 0:9bca35ae9c6b 65
aberk 0:9bca35ae9c6b 66 void initializePidControllers(void){
aberk 0:9bca35ae9c6b 67
aberk 0:9bca35ae9c6b 68 leftController.setInputLimits(0.0, 10500.0);
aberk 0:9bca35ae9c6b 69 leftController.setOutputLimits(0.0, 1.0);
aberk 0:9bca35ae9c6b 70 leftController.setBias(1.0);
aberk 0:9bca35ae9c6b 71 leftController.setMode(AUTO_MODE);
aberk 0:9bca35ae9c6b 72
aberk 0:9bca35ae9c6b 73 }
aberk 0:9bca35ae9c6b 74
aberk 0:9bca35ae9c6b 75 int main() {
aberk 0:9bca35ae9c6b 76
aberk 0:9bca35ae9c6b 77 //Initialization.
aberk 0:9bca35ae9c6b 78 initializeMotors();
aberk 0:9bca35ae9c6b 79 initializePidControllers();
aberk 0:9bca35ae9c6b 80
aberk 0:9bca35ae9c6b 81 //Open results file.
ewoud 2:b2ccd9f044bb 82 //fp = fopen("/local/pidtest.csv", "w");
aberk 0:9bca35ae9c6b 83
aberk 0:9bca35ae9c6b 84 endTimer.start();
aberk 0:9bca35ae9c6b 85
aberk 0:9bca35ae9c6b 86 //Set velocity set point.
ewoud 3:4c93be3a9010 87
ewoud 2:b2ccd9f044bb 88 leftMotor.period_ms(0.1);
ewoud 2:b2ccd9f044bb 89
aberk 0:9bca35ae9c6b 90 //Run for 3 seconds.
ewoud 3:4c93be3a9010 91 while (endTimer.read() < 10){
ewoud 3:4c93be3a9010 92 measure = pot1.read()*10000;
ewoud 3:4c93be3a9010 93 leftController.setSetPoint(measure);
aberk 0:9bca35ae9c6b 94 leftPulses = leftQei.getPulses();
aberk 0:9bca35ae9c6b 95 leftVelocity = (leftPulses - leftPrevPulses) / RATE;
aberk 0:9bca35ae9c6b 96 leftPrevPulses = leftPulses;
ewoud 2:b2ccd9f044bb 97
aberk 0:9bca35ae9c6b 98 leftController.setProcessValue(leftVelocity);
aberk 1:ac598811dd00 99 leftPwmDuty = leftController.compute();
aberk 0:9bca35ae9c6b 100 leftMotor = leftPwmDuty;
ewoud 2:b2ccd9f044bb 101 pc.printf("leftpusles: %d, lefVelocity: %f, leftPwmDuty: %f \n\r",leftPulses,leftVelocity,leftPwmDuty);
ewoud 2:b2ccd9f044bb 102
ewoud 2:b2ccd9f044bb 103 //fprintf(fp, "%f,%f\n", leftVelocity, goal);
aberk 0:9bca35ae9c6b 104 wait(RATE);
aberk 0:9bca35ae9c6b 105 }
aberk 0:9bca35ae9c6b 106
aberk 0:9bca35ae9c6b 107 //Stop motors.
ewoud 2:b2ccd9f044bb 108 leftMotor = 0;
aberk 0:9bca35ae9c6b 109
aberk 0:9bca35ae9c6b 110 //Close results file.
ewoud 2:b2ccd9f044bb 111 //fclose(fp);
aberk 0:9bca35ae9c6b 112
aberk 0:9bca35ae9c6b 113 }