motor controller with P velocity control

Dependencies:   HIDScope PID QEI mbed EMG

Fork of PID_VelocityExample by Aaron Berk

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?

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