motor controller with P velocity control
Dependencies: HIDScope PID QEI mbed EMG
Fork of PID_VelocityExample by
main.cpp@9:07189a75e979, 2015-10-06 (annotated)
- Committer:
- ewoud
- Date:
- Tue Oct 06 10:17:13 2015 +0000
- Revision:
- 9:07189a75e979
- Parent:
- 8:55ca92c0e39d
- Child:
- 12:d7bb475bb82d
added second motor and deadzone support
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 | 9:07189a75e979 | 35 | |
ewoud | 9:07189a75e979 | 36 | // Velocity calculation |
ewoud | 9:07189a75e979 | 37 | leftPulses = leftQei.getPulses(); |
ewoud | 9:07189a75e979 | 38 | leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE; |
ewoud | 9:07189a75e979 | 39 | leftVelocity = leftVelocity/30000; // scale to 0 - 1, max velocity = 30000 |
ewoud | 9:07189a75e979 | 40 | leftPrevPulses = leftPulses; |
ewoud | 9:07189a75e979 | 41 | |
ewoud | 9:07189a75e979 | 42 | // PID control |
ewoud | 9:07189a75e979 | 43 | leftController.setProcessValue(leftVelocity); |
ewoud | 9:07189a75e979 | 44 | leftPwmDuty = leftController.compute(); |
ewoud | 9:07189a75e979 | 45 | if (leftPwmDuty < 0){ |
ewoud | 9:07189a75e979 | 46 | leftDirection = 0; |
ewoud | 9:07189a75e979 | 47 | leftMotor = -leftPwmDuty; |
ewoud | 9:07189a75e979 | 48 | } |
ewoud | 9:07189a75e979 | 49 | else { |
ewoud | 9:07189a75e979 | 50 | leftDirection = 1; |
ewoud | 9:07189a75e979 | 51 | leftMotor = leftPwmDuty; |
ewoud | 9:07189a75e979 | 52 | } |
ewoud | 9:07189a75e979 | 53 | |
ewoud | 9:07189a75e979 | 54 | // testing the right motor |
ewoud | 9:07189a75e979 | 55 | rightMotor = leftMotor; |
ewoud | 9:07189a75e979 | 56 | rightDirection=leftDirection; |
ewoud | 9:07189a75e979 | 57 | |
ewoud | 9:07189a75e979 | 58 | // User feadback |
ewoud | 9:07189a75e979 | 59 | scope.set(0, request); |
ewoud | 9:07189a75e979 | 60 | scope.set(1, leftPwmDuty); |
ewoud | 9:07189a75e979 | 61 | scope.set(2, leftVelocity); |
ewoud | 9:07189a75e979 | 62 | scope.send(); |
ewoud | 9:07189a75e979 | 63 | pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty); |
ewoud | 8:55ca92c0e39d | 64 | |
ewoud | 9:07189a75e979 | 65 | goFlag=false; |
ewoud | 7:e14e28d8cae3 | 66 | } |
aberk | 0:9bca35ae9c6b | 67 | } |
aberk | 0:9bca35ae9c6b | 68 | |
aberk | 0:9bca35ae9c6b | 69 | //Stop motors. |
ewoud | 2:b2ccd9f044bb | 70 | leftMotor = 0; |
aberk | 0:9bca35ae9c6b | 71 | |
aberk | 0:9bca35ae9c6b | 72 | //Close results file. |
ewoud | 2:b2ccd9f044bb | 73 | //fclose(fp); |
aberk | 0:9bca35ae9c6b | 74 | |
aberk | 0:9bca35ae9c6b | 75 | } |