motor controller with P velocity control
Dependencies: HIDScope PID QEI mbed EMG
Fork of PID_VelocityExample by
main.cpp@17:034b50f49f46, 2015-10-07 (annotated)
- Committer:
- ewoud
- Date:
- Wed Oct 07 12:19:30 2015 +0000
- Revision:
- 17:034b50f49f46
- Parent:
- 16:e9945e3b4712
- Child:
- 18:4ee32b922251
fixed timing issue by introducing a clock to get the loop time
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 | 14:102a2b4f5c86 | 6 | #include <math.h> |
ewoud | 14:102a2b4f5c86 | 7 | //#include "Point.cpp" |
ewoud | 9:07189a75e979 | 8 | #include "inits.h" // all globals, pin and variable initialization |
aberk | 0:9bca35ae9c6b | 9 | |
ewoud | 9:07189a75e979 | 10 | void setGoFlag(){ |
ewoud | 16:e9945e3b4712 | 11 | if (goFlag==true && systemOn==true && calcFlag==false){ |
ewoud | 14:102a2b4f5c86 | 12 | pc.printf("ERROR: INCORRECT TIMINGS, look at the 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 | } |
ewoud | 16:e9945e3b4712 | 17 | void setCalcFlag(){ |
ewoud | 16:e9945e3b4712 | 18 | if (calcFlag==true && systemOn==true){ |
ewoud | 16:e9945e3b4712 | 19 | pc.printf("ERROR: INCORRECT TIMINGS, look at the setCalcFlag \n\r"); |
ewoud | 16:e9945e3b4712 | 20 | } |
ewoud | 16:e9945e3b4712 | 21 | calcFlag=true; |
ewoud | 16:e9945e3b4712 | 22 | } |
ewoud | 13:40141b362092 | 23 | void systemStart(){ |
ewoud | 13:40141b362092 | 24 | systemOn=true; |
ewoud | 13:40141b362092 | 25 | } |
ewoud | 13:40141b362092 | 26 | void systemStop(){ |
ewoud | 13:40141b362092 | 27 | systemOn=false; |
ewoud | 13:40141b362092 | 28 | leftMotor=0; |
ewoud | 13:40141b362092 | 29 | rightMotor=0; |
ewoud | 13:40141b362092 | 30 | } |
ewoud | 7:e14e28d8cae3 | 31 | int main() { |
ewoud | 17:034b50f49f46 | 32 | velocityTimer.start(); |
ewoud | 9:07189a75e979 | 33 | // initialize (defined in inits.h) |
ewoud | 9:07189a75e979 | 34 | initMotors(); |
ewoud | 9:07189a75e979 | 35 | initPIDs(); |
ewoud | 9:07189a75e979 | 36 | |
ewoud | 16:e9945e3b4712 | 37 | speedcalcTicker.attach(&setCalcFlag, calcRATE); |
ewoud | 9:07189a75e979 | 38 | motorControlTicker.attach(&setGoFlag, RATE); |
ewoud | 9:07189a75e979 | 39 | |
ewoud | 13:40141b362092 | 40 | startButton.rise(&systemStart); |
ewoud | 13:40141b362092 | 41 | stopButton.rise(&systemStop); |
ewoud | 13:40141b362092 | 42 | |
ewoud | 16:e9945e3b4712 | 43 | while (true){ |
ewoud | 16:e9945e3b4712 | 44 | if (systemOn==true && calcFlag==true){ |
ewoud | 9:07189a75e979 | 45 | // get 'emg' signal |
ewoud | 9:07189a75e979 | 46 | request_pos = pot1.read(); |
ewoud | 9:07189a75e979 | 47 | request_neg = pot2.read(); |
ewoud | 9:07189a75e979 | 48 | |
ewoud | 14:102a2b4f5c86 | 49 | // determine if forward or backward signal is stronger |
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 | 14:102a2b4f5c86 | 56 | |
ewoud | 14:102a2b4f5c86 | 57 | // calculate required rotational velocity from the requested horizontal velocity |
ewoud | 14:102a2b4f5c86 | 58 | // first get the current position from the motor encoders |
ewoud | 16:e9945e3b4712 | 59 | // make them start at 45 degrees |
ewoud | 16:e9945e3b4712 | 60 | leftAngle=(leftQei.getPulses()/round)*360+45; |
ewoud | 16:e9945e3b4712 | 61 | rightAngle=(rightQei.getPulses()/round)*360+45; |
ewoud | 16:e9945e3b4712 | 62 | |
ewoud | 16:e9945e3b4712 | 63 | if (leftAngle < 0 or leftAngle > 90 or rightAngle < 0 or rightAngle > 90 ){ |
ewoud | 14:102a2b4f5c86 | 64 | pc.printf("out of bounds \n\r"); |
ewoud | 14:102a2b4f5c86 | 65 | leftRequest=0; |
ewoud | 14:102a2b4f5c86 | 66 | rightRequest=0; |
ewoud | 16:e9945e3b4712 | 67 | //if (leftAngle < -45){leftRequest=0.1;} |
ewoud | 16:e9945e3b4712 | 68 | //if (leftAngle > 45){leftRequest=-0.1;} |
ewoud | 16:e9945e3b4712 | 69 | //if (rightAngle < -45){rightRequest=0.1;} |
ewoud | 16:e9945e3b4712 | 70 | //if (rightAngle > 45){rightRequest=-0.1;} |
ewoud | 14:102a2b4f5c86 | 71 | } |
ewoud | 14:102a2b4f5c86 | 72 | else { |
ewoud | 14:102a2b4f5c86 | 73 | currentX = (tan(rightAngle*M_PI/180)*l)/(tan(leftAngle*M_PI/180)+tan(rightAngle*M_PI/180)); |
ewoud | 14:102a2b4f5c86 | 74 | currentY = tan(leftAngle*M_PI/180)*currentX; |
ewoud | 14:102a2b4f5c86 | 75 | |
ewoud | 16:e9945e3b4712 | 76 | //if (sqrt(pow(currentX,2)+pow(currentY,2)) > armlength){ // too far from left arm |
ewoud | 14:102a2b4f5c86 | 77 | //return 0; |
ewoud | 16:e9945e3b4712 | 78 | //} |
ewoud | 16:e9945e3b4712 | 79 | //if (sqrt(pow(l-currentX,2)+pow(currentY,2)) > armlength){ // too far from right arm |
ewoud | 14:102a2b4f5c86 | 80 | //return 0; |
ewoud | 16:e9945e3b4712 | 81 | //} |
ewoud | 14:102a2b4f5c86 | 82 | // calculate the position to go to according the the current position + the distance that should be covered in this timestep |
ewoud | 16:e9945e3b4712 | 83 | toX=currentX+request*calcRATE; // should be request*RATE |
ewoud | 14:102a2b4f5c86 | 84 | //toX=5; |
ewoud | 14:102a2b4f5c86 | 85 | //toY=pCurrent.posY+0*RATE; |
ewoud | 14:102a2b4f5c86 | 86 | toY=currentY; |
ewoud | 14:102a2b4f5c86 | 87 | |
ewoud | 14:102a2b4f5c86 | 88 | toLeftAngle = atan(toY/toX)*180/M_PI; |
ewoud | 14:102a2b4f5c86 | 89 | toRightAngle = atan(toY/(l-toX))*180/M_PI; |
ewoud | 14:102a2b4f5c86 | 90 | |
ewoud | 14:102a2b4f5c86 | 91 | // calculate how much the angles should change in this timestep |
ewoud | 17:034b50f49f46 | 92 | leftDeltaAngle=(toLeftAngle-leftAngle)/10; |
ewoud | 17:034b50f49f46 | 93 | rightDeltaAngle=(toRightAngle-rightAngle)/10; |
ewoud | 14:102a2b4f5c86 | 94 | |
ewoud | 14:102a2b4f5c86 | 95 | // calculate the neccesairy velocities to make these angles happen. |
ewoud | 16:e9945e3b4712 | 96 | leftRequest=(leftDeltaAngle/calcRATE); |
ewoud | 16:e9945e3b4712 | 97 | rightRequest=(rightDeltaAngle/calcRATE); |
ewoud | 14:102a2b4f5c86 | 98 | } |
ewoud | 17:034b50f49f46 | 99 | //pc.printf("leftrequest: %f, rightrequest %f, leftAngle: %f, rightAngle: %f \n\r",leftRequest,rightRequest,leftAngle,rightAngle); |
ewoud | 17:034b50f49f46 | 100 | didCalc=true; |
ewoud | 16:e9945e3b4712 | 101 | calcFlag=false; |
ewoud | 16:e9945e3b4712 | 102 | } |
ewoud | 17:034b50f49f46 | 103 | else if (systemOn == true && goFlag == true && didCalc==false){ |
ewoud | 17:034b50f49f46 | 104 | |
ewoud | 14:102a2b4f5c86 | 105 | // set the PID controller to go with that speed. |
ewoud | 14:102a2b4f5c86 | 106 | leftController.setSetPoint(leftRequest); |
ewoud | 14:102a2b4f5c86 | 107 | rightController.setSetPoint(rightRequest); |
ewoud | 14:102a2b4f5c86 | 108 | |
ewoud | 16:e9945e3b4712 | 109 | //pc.printf("leftAngle: %f, rightAngle: %f, leftDeltaAngle: %f, rightDeltaAngle: %f \n\r", leftAngle, rightAngle, leftDeltaAngle, rightDeltaAngle); |
ewoud | 9:07189a75e979 | 110 | |
ewoud | 12:d7bb475bb82d | 111 | // ******************* |
ewoud | 9:07189a75e979 | 112 | // Velocity calculation |
ewoud | 12:d7bb475bb82d | 113 | // left |
ewoud | 17:034b50f49f46 | 114 | thistime=velocityTimer.read(); |
ewoud | 17:034b50f49f46 | 115 | looptime=thistime-lasttime; |
ewoud | 17:034b50f49f46 | 116 | lasttime=thistime; |
ewoud | 17:034b50f49f46 | 117 | //pc.printf("looptime: %f",looptime); |
ewoud | 17:034b50f49f46 | 118 | |
ewoud | 9:07189a75e979 | 119 | leftPulses = leftQei.getPulses(); |
ewoud | 17:034b50f49f46 | 120 | leftVelocity = ((float)(leftPulses - leftPrevPulses))/ looptime; |
ewoud | 9:07189a75e979 | 121 | leftVelocity = leftVelocity/30000; // scale to 0 - 1, max velocity = 30000 |
ewoud | 9:07189a75e979 | 122 | leftPrevPulses = leftPulses; |
ewoud | 12:d7bb475bb82d | 123 | |
ewoud | 12:d7bb475bb82d | 124 | // right |
ewoud | 12:d7bb475bb82d | 125 | rightPulses = rightQei.getPulses(); |
ewoud | 17:034b50f49f46 | 126 | rightVelocity = ((float)(rightPulses - rightPrevPulses))/ looptime; |
ewoud | 12:d7bb475bb82d | 127 | rightVelocity = rightVelocity/30000; // scale to 0 - 1, max velocity = 30000 |
ewoud | 12:d7bb475bb82d | 128 | rightPrevPulses = rightPulses; |
ewoud | 12:d7bb475bb82d | 129 | |
ewoud | 12:d7bb475bb82d | 130 | // *********** |
ewoud | 9:07189a75e979 | 131 | // PID control |
ewoud | 12:d7bb475bb82d | 132 | // left |
ewoud | 9:07189a75e979 | 133 | leftController.setProcessValue(leftVelocity); |
ewoud | 9:07189a75e979 | 134 | leftPwmDuty = leftController.compute(); |
ewoud | 9:07189a75e979 | 135 | if (leftPwmDuty < 0){ |
ewoud | 9:07189a75e979 | 136 | leftDirection = 0; |
ewoud | 9:07189a75e979 | 137 | leftMotor = -leftPwmDuty; |
ewoud | 9:07189a75e979 | 138 | } |
ewoud | 9:07189a75e979 | 139 | else { |
ewoud | 9:07189a75e979 | 140 | leftDirection = 1; |
ewoud | 9:07189a75e979 | 141 | leftMotor = leftPwmDuty; |
ewoud | 9:07189a75e979 | 142 | } |
ewoud | 9:07189a75e979 | 143 | |
ewoud | 12:d7bb475bb82d | 144 | // right |
ewoud | 12:d7bb475bb82d | 145 | rightController.setProcessValue(rightVelocity); |
ewoud | 12:d7bb475bb82d | 146 | rightPwmDuty = rightController.compute(); |
ewoud | 12:d7bb475bb82d | 147 | if (rightPwmDuty < 0){ |
ewoud | 14:102a2b4f5c86 | 148 | rightDirection = 1; |
ewoud | 12:d7bb475bb82d | 149 | rightMotor = -rightPwmDuty; |
ewoud | 12:d7bb475bb82d | 150 | } |
ewoud | 12:d7bb475bb82d | 151 | else { |
ewoud | 14:102a2b4f5c86 | 152 | rightDirection = 0; |
ewoud | 12:d7bb475bb82d | 153 | rightMotor = rightPwmDuty; |
ewoud | 12:d7bb475bb82d | 154 | } |
ewoud | 9:07189a75e979 | 155 | |
ewoud | 12:d7bb475bb82d | 156 | // User feedback |
ewoud | 14:102a2b4f5c86 | 157 | scope.set(0, rightRequest); |
ewoud | 12:d7bb475bb82d | 158 | scope.set(1, rightPwmDuty); |
ewoud | 12:d7bb475bb82d | 159 | scope.set(2, rightVelocity); |
ewoud | 14:102a2b4f5c86 | 160 | scope.set(3, leftAngle); |
ewoud | 14:102a2b4f5c86 | 161 | scope.set(4, rightAngle); |
ewoud | 9:07189a75e979 | 162 | scope.send(); |
ewoud | 14:102a2b4f5c86 | 163 | //pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty); |
ewoud | 8:55ca92c0e39d | 164 | |
ewoud | 9:07189a75e979 | 165 | goFlag=false; |
ewoud | 7:e14e28d8cae3 | 166 | } |
ewoud | 17:034b50f49f46 | 167 | |
ewoud | 17:034b50f49f46 | 168 | if (systemOn == true && goFlag == true && didCalc == true){ |
ewoud | 17:034b50f49f46 | 169 | //leftPrevPulses = leftQei.getPulses(); |
ewoud | 17:034b50f49f46 | 170 | //rightPrevPulses = rightQei.getPulses(); |
ewoud | 17:034b50f49f46 | 171 | didCalc=false; |
ewoud | 17:034b50f49f46 | 172 | //goFlag=false; |
ewoud | 17:034b50f49f46 | 173 | } |
aberk | 0:9bca35ae9c6b | 174 | } |
aberk | 0:9bca35ae9c6b | 175 | |
aberk | 0:9bca35ae9c6b | 176 | //Stop motors. |
ewoud | 2:b2ccd9f044bb | 177 | leftMotor = 0; |
ewoud | 12:d7bb475bb82d | 178 | rightMotor = 0; |
aberk | 0:9bca35ae9c6b | 179 | |
aberk | 0:9bca35ae9c6b | 180 | } |