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