motor controller with P velocity control

Dependencies:   HIDScope PID QEI mbed EMG

Fork of PID_VelocityExample by Aaron Berk

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?

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 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 }