motor controller with P velocity control
Dependencies: HIDScope PID QEI mbed EMG
Fork of PID_VelocityExample by
main.cpp@12:d7bb475bb82d, 2015-10-06 (annotated)
- Committer:
- ewoud
- Date:
- Tue Oct 06 10:51:02 2015 +0000
- Revision:
- 12:d7bb475bb82d
- Parent:
- 9:07189a75e979
- Child:
- 13:40141b362092
full second motor PID control and fixed motors going into overdrive on startup
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 | #include "PID.h" |
aberk | 0:9bca35ae9c6b | 4 | #include "QEI.h" |
ewoud | 9:07189a75e979 | 5 | #include "HIDScope.h" |
ewoud | 9:07189a75e979 | 6 | #include "inits.h" // all globals, pin and variable initialization |
aberk | 0:9bca35ae9c6b | 7 | |
ewoud | 9:07189a75e979 | 8 | void setGoFlag(){ |
ewoud | 9:07189a75e979 | 9 | goFlag=true; |
ewoud | 9:07189a75e979 | 10 | } |
aberk | 0:9bca35ae9c6b | 11 | |
ewoud | 7:e14e28d8cae3 | 12 | int main() { |
ewoud | 7:e14e28d8cae3 | 13 | |
ewoud | 9:07189a75e979 | 14 | // initialize (defined in inits.h) |
ewoud | 9:07189a75e979 | 15 | initMotors(); |
ewoud | 9:07189a75e979 | 16 | initPIDs(); |
ewoud | 9:07189a75e979 | 17 | |
ewoud | 9:07189a75e979 | 18 | motorControlTicker.attach(&setGoFlag, RATE); |
ewoud | 9:07189a75e979 | 19 | |
ewoud | 8:55ca92c0e39d | 20 | endTimer.start(); //Run for 100 seconds. |
ewoud | 4:be465e9a12cb | 21 | while (endTimer.read() < 100){ |
ewoud | 9:07189a75e979 | 22 | if (goFlag==true){ |
ewoud | 9:07189a75e979 | 23 | // get 'emg' signal |
ewoud | 9:07189a75e979 | 24 | request_pos = pot1.read(); |
ewoud | 9:07189a75e979 | 25 | request_neg = pot2.read(); |
ewoud | 9:07189a75e979 | 26 | |
ewoud | 9:07189a75e979 | 27 | // determine if forward or backward signal is stronger and set reference |
ewoud | 9:07189a75e979 | 28 | if (request_pos > request_neg){ |
ewoud | 9:07189a75e979 | 29 | request = request_pos; |
ewoud | 9:07189a75e979 | 30 | } |
ewoud | 9:07189a75e979 | 31 | else { |
ewoud | 9:07189a75e979 | 32 | request = -request_neg; |
ewoud | 9:07189a75e979 | 33 | } |
ewoud | 9:07189a75e979 | 34 | leftController.setSetPoint(request); |
ewoud | 12:d7bb475bb82d | 35 | rightController.setSetPoint(request); |
ewoud | 9:07189a75e979 | 36 | |
ewoud | 12:d7bb475bb82d | 37 | // ******************* |
ewoud | 9:07189a75e979 | 38 | // Velocity calculation |
ewoud | 12:d7bb475bb82d | 39 | // left |
ewoud | 9:07189a75e979 | 40 | leftPulses = leftQei.getPulses(); |
ewoud | 9:07189a75e979 | 41 | leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE; |
ewoud | 9:07189a75e979 | 42 | leftVelocity = leftVelocity/30000; // scale to 0 - 1, max velocity = 30000 |
ewoud | 9:07189a75e979 | 43 | leftPrevPulses = leftPulses; |
ewoud | 12:d7bb475bb82d | 44 | |
ewoud | 12:d7bb475bb82d | 45 | // right |
ewoud | 12:d7bb475bb82d | 46 | rightPulses = rightQei.getPulses(); |
ewoud | 12:d7bb475bb82d | 47 | rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE; |
ewoud | 12:d7bb475bb82d | 48 | rightVelocity = rightVelocity/30000; // scale to 0 - 1, max velocity = 30000 |
ewoud | 12:d7bb475bb82d | 49 | rightPrevPulses = rightPulses; |
ewoud | 12:d7bb475bb82d | 50 | |
ewoud | 12:d7bb475bb82d | 51 | // *********** |
ewoud | 9:07189a75e979 | 52 | // PID control |
ewoud | 12:d7bb475bb82d | 53 | // left |
ewoud | 9:07189a75e979 | 54 | leftController.setProcessValue(leftVelocity); |
ewoud | 9:07189a75e979 | 55 | leftPwmDuty = leftController.compute(); |
ewoud | 9:07189a75e979 | 56 | if (leftPwmDuty < 0){ |
ewoud | 9:07189a75e979 | 57 | leftDirection = 0; |
ewoud | 9:07189a75e979 | 58 | leftMotor = -leftPwmDuty; |
ewoud | 9:07189a75e979 | 59 | } |
ewoud | 9:07189a75e979 | 60 | else { |
ewoud | 9:07189a75e979 | 61 | leftDirection = 1; |
ewoud | 9:07189a75e979 | 62 | leftMotor = leftPwmDuty; |
ewoud | 9:07189a75e979 | 63 | } |
ewoud | 9:07189a75e979 | 64 | |
ewoud | 12:d7bb475bb82d | 65 | // right |
ewoud | 12:d7bb475bb82d | 66 | rightController.setProcessValue(rightVelocity); |
ewoud | 12:d7bb475bb82d | 67 | rightPwmDuty = rightController.compute(); |
ewoud | 12:d7bb475bb82d | 68 | if (rightPwmDuty < 0){ |
ewoud | 12:d7bb475bb82d | 69 | rightDirection = 0; |
ewoud | 12:d7bb475bb82d | 70 | rightMotor = -rightPwmDuty; |
ewoud | 12:d7bb475bb82d | 71 | } |
ewoud | 12:d7bb475bb82d | 72 | else { |
ewoud | 12:d7bb475bb82d | 73 | rightDirection = 1; |
ewoud | 12:d7bb475bb82d | 74 | rightMotor = rightPwmDuty; |
ewoud | 12:d7bb475bb82d | 75 | } |
ewoud | 9:07189a75e979 | 76 | |
ewoud | 12:d7bb475bb82d | 77 | // User feedback |
ewoud | 9:07189a75e979 | 78 | scope.set(0, request); |
ewoud | 12:d7bb475bb82d | 79 | scope.set(1, rightPwmDuty); |
ewoud | 12:d7bb475bb82d | 80 | scope.set(2, rightVelocity); |
ewoud | 9:07189a75e979 | 81 | scope.send(); |
ewoud | 9:07189a75e979 | 82 | pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty); |
ewoud | 8:55ca92c0e39d | 83 | |
ewoud | 9:07189a75e979 | 84 | goFlag=false; |
ewoud | 7:e14e28d8cae3 | 85 | } |
aberk | 0:9bca35ae9c6b | 86 | } |
aberk | 0:9bca35ae9c6b | 87 | |
aberk | 0:9bca35ae9c6b | 88 | //Stop motors. |
ewoud | 2:b2ccd9f044bb | 89 | leftMotor = 0; |
ewoud | 12:d7bb475bb82d | 90 | rightMotor = 0; |
aberk | 0:9bca35ae9c6b | 91 | |
aberk | 0:9bca35ae9c6b | 92 | } |