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@18:4ee32b922251, 2015-10-07 (annotated)

Committer:
ewoud
Date:
Wed Oct 07 13:19:25 2015 +0000
Revision:
18:4ee32b922251
Parent:
17:034b50f49f46
Child:
19:3ca10fe26131
working xy control

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 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 18:4ee32b922251 55
ewoud 14:102a2b4f5c86 56 // calculate required rotational velocity from the requested horizontal velocity
ewoud 14:102a2b4f5c86 57 // first get the current position from the motor encoders
ewoud 18:4ee32b922251 58 // make them start at 45 degree.
ewoud 16:e9945e3b4712 59 leftAngle=(leftQei.getPulses()/round)*360+45;
ewoud 16:e9945e3b4712 60 rightAngle=(rightQei.getPulses()/round)*360+45;
ewoud 16:e9945e3b4712 61
ewoud 16:e9945e3b4712 62 if (leftAngle < 0 or leftAngle > 90 or rightAngle < 0 or rightAngle > 90 ){
ewoud 14:102a2b4f5c86 63 pc.printf("out of bounds \n\r");
ewoud 14:102a2b4f5c86 64 leftRequest=0;
ewoud 14:102a2b4f5c86 65 rightRequest=0;
ewoud 16:e9945e3b4712 66 //if (leftAngle < -45){leftRequest=0.1;}
ewoud 16:e9945e3b4712 67 //if (leftAngle > 45){leftRequest=-0.1;}
ewoud 16:e9945e3b4712 68 //if (rightAngle < -45){rightRequest=0.1;}
ewoud 16:e9945e3b4712 69 //if (rightAngle > 45){rightRequest=-0.1;}
ewoud 14:102a2b4f5c86 70 }
ewoud 14:102a2b4f5c86 71 else {
ewoud 14:102a2b4f5c86 72 currentX = (tan(rightAngle*M_PI/180)*l)/(tan(leftAngle*M_PI/180)+tan(rightAngle*M_PI/180));
ewoud 14:102a2b4f5c86 73 currentY = tan(leftAngle*M_PI/180)*currentX;
ewoud 14:102a2b4f5c86 74
ewoud 16:e9945e3b4712 75 //if (sqrt(pow(currentX,2)+pow(currentY,2)) > armlength){ // too far from left arm
ewoud 14:102a2b4f5c86 76 //return 0;
ewoud 16:e9945e3b4712 77 //}
ewoud 16:e9945e3b4712 78 //if (sqrt(pow(l-currentX,2)+pow(currentY,2)) > armlength){ // too far from right arm
ewoud 14:102a2b4f5c86 79 //return 0;
ewoud 16:e9945e3b4712 80 //}
ewoud 14:102a2b4f5c86 81 // calculate the position to go to according the the current position + the distance that should be covered in this timestep
ewoud 16:e9945e3b4712 82 toX=currentX+request*calcRATE; // should be request*RATE
ewoud 18:4ee32b922251 83 toY=currentY+0*calcRATE;
ewoud 14:102a2b4f5c86 84
ewoud 14:102a2b4f5c86 85 toLeftAngle = atan(toY/toX)*180/M_PI;
ewoud 14:102a2b4f5c86 86 toRightAngle = atan(toY/(l-toX))*180/M_PI;
ewoud 14:102a2b4f5c86 87
ewoud 14:102a2b4f5c86 88 // calculate how much the angles should change in this timestep
ewoud 17:034b50f49f46 89 leftDeltaAngle=(toLeftAngle-leftAngle)/10;
ewoud 17:034b50f49f46 90 rightDeltaAngle=(toRightAngle-rightAngle)/10;
ewoud 14:102a2b4f5c86 91
ewoud 14:102a2b4f5c86 92 // calculate the neccesairy velocities to make these angles happen.
ewoud 16:e9945e3b4712 93 leftRequest=(leftDeltaAngle/calcRATE);
ewoud 16:e9945e3b4712 94 rightRequest=(rightDeltaAngle/calcRATE);
ewoud 14:102a2b4f5c86 95 }
ewoud 17:034b50f49f46 96 //pc.printf("leftrequest: %f, rightrequest %f, leftAngle: %f, rightAngle: %f \n\r",leftRequest,rightRequest,leftAngle,rightAngle);
ewoud 16:e9945e3b4712 97 calcFlag=false;
ewoud 16:e9945e3b4712 98 }
ewoud 18:4ee32b922251 99 else if (systemOn == true && goFlag == true){
ewoud 17:034b50f49f46 100
ewoud 14:102a2b4f5c86 101 // set the PID controller to go with that speed.
ewoud 14:102a2b4f5c86 102 leftController.setSetPoint(leftRequest);
ewoud 14:102a2b4f5c86 103 rightController.setSetPoint(rightRequest);
ewoud 14:102a2b4f5c86 104
ewoud 16:e9945e3b4712 105 //pc.printf("leftAngle: %f, rightAngle: %f, leftDeltaAngle: %f, rightDeltaAngle: %f \n\r", leftAngle, rightAngle, leftDeltaAngle, rightDeltaAngle);
ewoud 9:07189a75e979 106
ewoud 12:d7bb475bb82d 107 // *******************
ewoud 9:07189a75e979 108 // Velocity calculation
ewoud 12:d7bb475bb82d 109 // left
ewoud 17:034b50f49f46 110 thistime=velocityTimer.read();
ewoud 17:034b50f49f46 111 looptime=thistime-lasttime;
ewoud 17:034b50f49f46 112 lasttime=thistime;
ewoud 17:034b50f49f46 113 //pc.printf("looptime: %f",looptime);
ewoud 17:034b50f49f46 114
ewoud 9:07189a75e979 115 leftPulses = leftQei.getPulses();
ewoud 17:034b50f49f46 116 leftVelocity = ((float)(leftPulses - leftPrevPulses))/ looptime;
ewoud 9:07189a75e979 117 leftVelocity = leftVelocity/30000; // scale to 0 - 1, max velocity = 30000
ewoud 9:07189a75e979 118 leftPrevPulses = leftPulses;
ewoud 12:d7bb475bb82d 119
ewoud 12:d7bb475bb82d 120 // right
ewoud 12:d7bb475bb82d 121 rightPulses = rightQei.getPulses();
ewoud 17:034b50f49f46 122 rightVelocity = ((float)(rightPulses - rightPrevPulses))/ looptime;
ewoud 12:d7bb475bb82d 123 rightVelocity = rightVelocity/30000; // scale to 0 - 1, max velocity = 30000
ewoud 12:d7bb475bb82d 124 rightPrevPulses = rightPulses;
ewoud 12:d7bb475bb82d 125
ewoud 12:d7bb475bb82d 126 // ***********
ewoud 9:07189a75e979 127 // PID control
ewoud 12:d7bb475bb82d 128 // left
ewoud 9:07189a75e979 129 leftController.setProcessValue(leftVelocity);
ewoud 9:07189a75e979 130 leftPwmDuty = leftController.compute();
ewoud 9:07189a75e979 131 if (leftPwmDuty < 0){
ewoud 9:07189a75e979 132 leftDirection = 0;
ewoud 9:07189a75e979 133 leftMotor = -leftPwmDuty;
ewoud 9:07189a75e979 134 }
ewoud 9:07189a75e979 135 else {
ewoud 9:07189a75e979 136 leftDirection = 1;
ewoud 9:07189a75e979 137 leftMotor = leftPwmDuty;
ewoud 9:07189a75e979 138 }
ewoud 9:07189a75e979 139
ewoud 12:d7bb475bb82d 140 // right
ewoud 12:d7bb475bb82d 141 rightController.setProcessValue(rightVelocity);
ewoud 12:d7bb475bb82d 142 rightPwmDuty = rightController.compute();
ewoud 12:d7bb475bb82d 143 if (rightPwmDuty < 0){
ewoud 14:102a2b4f5c86 144 rightDirection = 1;
ewoud 12:d7bb475bb82d 145 rightMotor = -rightPwmDuty;
ewoud 12:d7bb475bb82d 146 }
ewoud 12:d7bb475bb82d 147 else {
ewoud 14:102a2b4f5c86 148 rightDirection = 0;
ewoud 12:d7bb475bb82d 149 rightMotor = rightPwmDuty;
ewoud 12:d7bb475bb82d 150 }
ewoud 9:07189a75e979 151
ewoud 12:d7bb475bb82d 152 // User feedback
ewoud 18:4ee32b922251 153 scope.set(0, rightRequest*10);
ewoud 12:d7bb475bb82d 154 scope.set(1, rightPwmDuty);
ewoud 18:4ee32b922251 155 scope.set(2, rightVelocity*10);
ewoud 14:102a2b4f5c86 156 scope.set(3, leftAngle);
ewoud 14:102a2b4f5c86 157 scope.set(4, rightAngle);
ewoud 9:07189a75e979 158 scope.send();
ewoud 14:102a2b4f5c86 159 //pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
ewoud 8:55ca92c0e39d 160
ewoud 9:07189a75e979 161 goFlag=false;
ewoud 7:e14e28d8cae3 162 }
aberk 0:9bca35ae9c6b 163 }
aberk 0:9bca35ae9c6b 164
aberk 0:9bca35ae9c6b 165 //Stop motors.
ewoud 2:b2ccd9f044bb 166 leftMotor = 0;
ewoud 12:d7bb475bb82d 167 rightMotor = 0;
aberk 0:9bca35ae9c6b 168
aberk 0:9bca35ae9c6b 169 }