Numero Uno / Mbed 2 deprecated PID_VelocityExample

motor controller with P velocity control

Dependencies:   HIDScope PID QEI mbed EMG

Fork of PID_VelocityExample by Aaron Berk

main.cpp@16:e9945e3b4712, 2015-10-07 (annotated)

Committer:
ewoud
Date:
Wed Oct 07 11:45:27 2015 +0000
Revision:
16:e9945e3b4712
Parent:
14:102a2b4f5c86
Child:
17:034b50f49f46
different timings for calculation and PID control; motor work with very tiny pot meter movements

Who changed what in which revision?

UserRevisionLine numberNew 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 7:e14e28d8cae3 32
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 14:102a2b4f5c86 92 leftDeltaAngle=toLeftAngle-leftAngle;
ewoud 14:102a2b4f5c86 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 16:e9945e3b4712 99 pc.printf("leftrequest: %f, rightrequest %f, leftAngle: %f, rightAngle: %f \n\r",leftRequest,rightRequest,leftAngle,rightAngle);
ewoud 16:e9945e3b4712 100 calcFlag=false;
ewoud 16:e9945e3b4712 101 }
ewoud 16:e9945e3b4712 102 else if (systemOn == true && goFlag == true){
ewoud 14:102a2b4f5c86 103 // set the PID controller to go with that speed.
ewoud 14:102a2b4f5c86 104 leftController.setSetPoint(leftRequest);
ewoud 14:102a2b4f5c86 105 rightController.setSetPoint(rightRequest);
ewoud 14:102a2b4f5c86 106
ewoud 16:e9945e3b4712 107 //pc.printf("leftAngle: %f, rightAngle: %f, leftDeltaAngle: %f, rightDeltaAngle: %f \n\r", leftAngle, rightAngle, leftDeltaAngle, rightDeltaAngle);
ewoud 9:07189a75e979 108
ewoud 12:d7bb475bb82d 109 // *******************
ewoud 9:07189a75e979 110 // Velocity calculation
ewoud 12:d7bb475bb82d 111 // left
ewoud 9:07189a75e979 112 leftPulses = leftQei.getPulses();
ewoud 9:07189a75e979 113 leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE;
ewoud 9:07189a75e979 114 leftVelocity = leftVelocity/30000; // scale to 0 - 1, max velocity = 30000
ewoud 9:07189a75e979 115 leftPrevPulses = leftPulses;
ewoud 12:d7bb475bb82d 116
ewoud 12:d7bb475bb82d 117 // right
ewoud 12:d7bb475bb82d 118 rightPulses = rightQei.getPulses();
ewoud 12:d7bb475bb82d 119 rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE;
ewoud 12:d7bb475bb82d 120 rightVelocity = rightVelocity/30000; // scale to 0 - 1, max velocity = 30000
ewoud 12:d7bb475bb82d 121 rightPrevPulses = rightPulses;
ewoud 12:d7bb475bb82d 122
ewoud 12:d7bb475bb82d 123 // ***********
ewoud 9:07189a75e979 124 // PID control
ewoud 12:d7bb475bb82d 125 // left
ewoud 9:07189a75e979 126 leftController.setProcessValue(leftVelocity);
ewoud 9:07189a75e979 127 leftPwmDuty = leftController.compute();
ewoud 9:07189a75e979 128 if (leftPwmDuty < 0){
ewoud 9:07189a75e979 129 leftDirection = 0;
ewoud 9:07189a75e979 130 leftMotor = -leftPwmDuty;
ewoud 9:07189a75e979 131 }
ewoud 9:07189a75e979 132 else {
ewoud 9:07189a75e979 133 leftDirection = 1;
ewoud 9:07189a75e979 134 leftMotor = leftPwmDuty;
ewoud 9:07189a75e979 135 }
ewoud 9:07189a75e979 136
ewoud 12:d7bb475bb82d 137 // right
ewoud 12:d7bb475bb82d 138 rightController.setProcessValue(rightVelocity);
ewoud 12:d7bb475bb82d 139 rightPwmDuty = rightController.compute();
ewoud 12:d7bb475bb82d 140 if (rightPwmDuty < 0){
ewoud 14:102a2b4f5c86 141 rightDirection = 1;
ewoud 12:d7bb475bb82d 142 rightMotor = -rightPwmDuty;
ewoud 12:d7bb475bb82d 143 }
ewoud 12:d7bb475bb82d 144 else {
ewoud 14:102a2b4f5c86 145 rightDirection = 0;
ewoud 12:d7bb475bb82d 146 rightMotor = rightPwmDuty;
ewoud 12:d7bb475bb82d 147 }
ewoud 9:07189a75e979 148
ewoud 12:d7bb475bb82d 149 // User feedback
ewoud 14:102a2b4f5c86 150 scope.set(0, rightRequest);
ewoud 12:d7bb475bb82d 151 scope.set(1, rightPwmDuty);
ewoud 12:d7bb475bb82d 152 scope.set(2, rightVelocity);
ewoud 14:102a2b4f5c86 153 scope.set(3, leftAngle);
ewoud 14:102a2b4f5c86 154 scope.set(4, rightAngle);
ewoud 9:07189a75e979 155 scope.send();
ewoud 14:102a2b4f5c86 156 //pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
ewoud 8:55ca92c0e39d 157
ewoud 9:07189a75e979 158 goFlag=false;
ewoud 7:e14e28d8cae3 159 }
aberk 0:9bca35ae9c6b 160 }
aberk 0:9bca35ae9c6b 161
aberk 0:9bca35ae9c6b 162 //Stop motors.
ewoud 2:b2ccd9f044bb 163 leftMotor = 0;
ewoud 12:d7bb475bb82d 164 rightMotor = 0;
aberk 0:9bca35ae9c6b 165
aberk 0:9bca35ae9c6b 166 }