motor controller with P velocity control
Dependencies: HIDScope PID QEI mbed EMG
Fork of PID_VelocityExample by
main.cpp@8:55ca92c0e39d, 2015-10-05 (annotated)
- Committer:
- ewoud
- Date:
- Mon Oct 05 13:35:20 2015 +0000
- Revision:
- 8:55ca92c0e39d
- Parent:
- 7:e14e28d8cae3
- Child:
- 9:07189a75e979
cleaning up code, added commenting
Who changed what in which revision?
User | Revision | Line number | New 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; |
ewoud | 8:55ca92c0e39d | 47 | |
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 | |
ewoud | 8:55ca92c0e39d | 67 | endTimer.start(); //Run for 100 seconds. |
ewoud | 4:be465e9a12cb | 68 | while (endTimer.read() < 100){ |
ewoud | 8:55ca92c0e39d | 69 | // get 'emg' signal |
ewoud | 7:e14e28d8cae3 | 70 | request_pos = pot1.read(); |
ewoud | 7:e14e28d8cae3 | 71 | request_neg = pot2.read(); |
ewoud | 7:e14e28d8cae3 | 72 | |
ewoud | 8:55ca92c0e39d | 73 | // determine if forward or backward signal is stronger and set reference |
ewoud | 7:e14e28d8cae3 | 74 | if (request_pos > request_neg){ |
ewoud | 7:e14e28d8cae3 | 75 | request = request_pos; |
ewoud | 7:e14e28d8cae3 | 76 | } |
ewoud | 7:e14e28d8cae3 | 77 | else { |
ewoud | 7:e14e28d8cae3 | 78 | request = -request_neg; |
ewoud | 7:e14e28d8cae3 | 79 | } |
ewoud | 8:55ca92c0e39d | 80 | leftController.setSetPoint(request); |
ewoud | 8:55ca92c0e39d | 81 | |
ewoud | 7:e14e28d8cae3 | 82 | // Velocity calculation |
aberk | 0:9bca35ae9c6b | 83 | leftPulses = leftQei.getPulses(); |
ewoud | 7:e14e28d8cae3 | 84 | leftVelocity = ((float)(leftPulses - leftPrevPulses))/ (30000.0* RATE); |
aberk | 0:9bca35ae9c6b | 85 | leftPrevPulses = leftPulses; |
ewoud | 8:55ca92c0e39d | 86 | |
ewoud | 7:e14e28d8cae3 | 87 | // PID control |
ewoud | 5:8ae6d935a16a | 88 | leftController.setProcessValue(leftVelocity); |
aberk | 1:ac598811dd00 | 89 | leftPwmDuty = leftController.compute(); |
ewoud | 7:e14e28d8cae3 | 90 | if (leftPwmDuty < 0){ |
ewoud | 7:e14e28d8cae3 | 91 | leftDirection = 0; |
ewoud | 7:e14e28d8cae3 | 92 | leftMotor = -leftPwmDuty; |
ewoud | 7:e14e28d8cae3 | 93 | } |
ewoud | 7:e14e28d8cae3 | 94 | else { |
ewoud | 7:e14e28d8cae3 | 95 | leftDirection = 1; |
ewoud | 7:e14e28d8cae3 | 96 | leftMotor = leftPwmDuty; |
ewoud | 7:e14e28d8cae3 | 97 | } |
ewoud | 5:8ae6d935a16a | 98 | |
ewoud | 8:55ca92c0e39d | 99 | // User feadback |
ewoud | 5:8ae6d935a16a | 100 | scope.set(0, request); |
ewoud | 5:8ae6d935a16a | 101 | scope.set(1, leftPwmDuty); |
ewoud | 5:8ae6d935a16a | 102 | scope.set(2, leftVelocity); |
ewoud | 6:f58052f57505 | 103 | scope.send(); |
ewoud | 7:e14e28d8cae3 | 104 | pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty); |
ewoud | 2:b2ccd9f044bb | 105 | |
aberk | 0:9bca35ae9c6b | 106 | wait(RATE); |
aberk | 0:9bca35ae9c6b | 107 | } |
aberk | 0:9bca35ae9c6b | 108 | |
aberk | 0:9bca35ae9c6b | 109 | //Stop motors. |
ewoud | 2:b2ccd9f044bb | 110 | leftMotor = 0; |
aberk | 0:9bca35ae9c6b | 111 | |
aberk | 0:9bca35ae9c6b | 112 | //Close results file. |
ewoud | 2:b2ccd9f044bb | 113 | //fclose(fp); |
aberk | 0:9bca35ae9c6b | 114 | |
aberk | 0:9bca35ae9c6b | 115 | } |