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