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