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@7:e14e28d8cae3, 2015-10-05 (annotated)

Committer:
ewoud
Date:
Mon Oct 05 13:28:12 2015 +0000
Revision:
7:e14e28d8cae3
Parent:
6:f58052f57505
Child:
8:55ca92c0e39d
added reversing motors using 2 potmeters as muscle input

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 7:e14e28d8cae3 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 //****************************************************************************/
ewoud 7:e14e28d8cae3 11 #define RATE 0.01
ewoud 7:e14e28d8cae3 12 #define Kc 0.35
ewoud 7:e14e28d8cae3 13 #define Ti 0.15
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);
ewoud 7:e14e28d8cae3 32 AnalogIn pot2(A1);
ewoud 7:e14e28d8cae3 33
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;
ewoud 5:8ae6d935a16a 44 volatile float leftPwmDutyPrev = 1.0;
ewoud 5:8ae6d935a16a 45 volatile float leftPwmDutyChange;
aberk 0:9bca35ae9c6b 46 volatile float leftVelocity = 0.0;
aberk 0:9bca35ae9c6b 47 //Velocity to reach.
ewoud 5:8ae6d935a16a 48 float request;
ewoud 7:e14e28d8cae3 49 float request_pos;
ewoud 7:e14e28d8cae3 50 float request_neg;
aberk 0:9bca35ae9c6b 51
aberk 0:9bca35ae9c6b 52
ewoud 7:e14e28d8cae3 53 int main() {
ewoud 7:e14e28d8cae3 54 //Initialization of motor
aberk 0:9bca35ae9c6b 55 leftMotor.period_us(50);
aberk 0:9bca35ae9c6b 56 leftMotor = 1.0;
aberk 0:9bca35ae9c6b 57 leftBrake = 0.0;
ewoud 4:be465e9a12cb 58 leftDirection = 1;
ewoud 7:e14e28d8cae3 59
ewoud 7:e14e28d8cae3 60 //Initialization of PID controller
ewoud 7:e14e28d8cae3 61 leftController.setInputLimits(-1.0, 1.0);
ewoud 7:e14e28d8cae3 62 leftController.setOutputLimits(-1.0, 1.0);
ewoud 5:8ae6d935a16a 63 leftController.setBias(0.0);
aberk 0:9bca35ae9c6b 64 leftController.setMode(AUTO_MODE);
aberk 0:9bca35ae9c6b 65
aberk 0:9bca35ae9c6b 66
aberk 0:9bca35ae9c6b 67 //Open results file.
ewoud 2:b2ccd9f044bb 68 //fp = fopen("/local/pidtest.csv", "w");
aberk 0:9bca35ae9c6b 69
aberk 0:9bca35ae9c6b 70 endTimer.start();
aberk 0:9bca35ae9c6b 71
aberk 0:9bca35ae9c6b 72 //Set velocity set point.
ewoud 3:4c93be3a9010 73
ewoud 4:be465e9a12cb 74
ewoud 2:b2ccd9f044bb 75
aberk 0:9bca35ae9c6b 76 //Run for 3 seconds.
ewoud 4:be465e9a12cb 77 while (endTimer.read() < 100){
ewoud 7:e14e28d8cae3 78 request_pos = pot1.read();
ewoud 7:e14e28d8cae3 79 request_neg = pot2.read();
ewoud 7:e14e28d8cae3 80
ewoud 7:e14e28d8cae3 81 if (request_pos > request_neg){
ewoud 7:e14e28d8cae3 82 request = request_pos;
ewoud 7:e14e28d8cae3 83 }
ewoud 7:e14e28d8cae3 84 else {
ewoud 7:e14e28d8cae3 85 request = -request_neg;
ewoud 7:e14e28d8cae3 86 }
ewoud 7:e14e28d8cae3 87 // Velocity calculation
ewoud 5:8ae6d935a16a 88 leftController.setSetPoint(request);
aberk 0:9bca35ae9c6b 89 leftPulses = leftQei.getPulses();
ewoud 7:e14e28d8cae3 90 leftVelocity = ((float)(leftPulses - leftPrevPulses))/ (30000.0* RATE);
aberk 0:9bca35ae9c6b 91 leftPrevPulses = leftPulses;
ewoud 2:b2ccd9f044bb 92
ewoud 5:8ae6d935a16a 93
ewoud 7:e14e28d8cae3 94 // PID control
ewoud 5:8ae6d935a16a 95 leftController.setProcessValue(leftVelocity);
aberk 1:ac598811dd00 96 leftPwmDuty = leftController.compute();
ewoud 7:e14e28d8cae3 97 if (leftPwmDuty < 0){
ewoud 7:e14e28d8cae3 98 leftDirection = 0;
ewoud 7:e14e28d8cae3 99 leftMotor = -leftPwmDuty;
ewoud 7:e14e28d8cae3 100 }
ewoud 7:e14e28d8cae3 101 else {
ewoud 7:e14e28d8cae3 102 leftDirection = 1;
ewoud 7:e14e28d8cae3 103 leftMotor = leftPwmDuty;
ewoud 7:e14e28d8cae3 104 }
ewoud 5:8ae6d935a16a 105
ewoud 5:8ae6d935a16a 106 scope.set(0, request);
ewoud 5:8ae6d935a16a 107 scope.set(1, leftPwmDuty);
ewoud 5:8ae6d935a16a 108 scope.set(2, leftVelocity);
ewoud 6:f58052f57505 109 scope.send();
ewoud 7:e14e28d8cae3 110 pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
ewoud 2:b2ccd9f044bb 111
ewoud 2:b2ccd9f044bb 112 //fprintf(fp, "%f,%f\n", leftVelocity, goal);
aberk 0:9bca35ae9c6b 113 wait(RATE);
aberk 0:9bca35ae9c6b 114 }
aberk 0:9bca35ae9c6b 115
aberk 0:9bca35ae9c6b 116 //Stop motors.
ewoud 2:b2ccd9f044bb 117 leftMotor = 0;
aberk 0:9bca35ae9c6b 118
aberk 0:9bca35ae9c6b 119 //Close results file.
ewoud 2:b2ccd9f044bb 120 //fclose(fp);
aberk 0:9bca35ae9c6b 121
aberk 0:9bca35ae9c6b 122 }