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