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@14:102a2b4f5c86, 2015-10-06 (annotated)

Committer:
ewoud
Date:
Tue Oct 06 14:17:11 2015 +0000
Revision:
14:102a2b4f5c86
Parent:
13:40141b362092
Child:
16:e9945e3b4712
integrating xy to angle calculations, not working yet

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 14:102a2b4f5c86 11 if (goFlag==true && systemOn==true){
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 }
aberk 0:9bca35ae9c6b 17
ewoud 13:40141b362092 18 void systemStart(){
ewoud 13:40141b362092 19 systemOn=true;
ewoud 13:40141b362092 20 }
ewoud 13:40141b362092 21 void systemStop(){
ewoud 13:40141b362092 22 systemOn=false;
ewoud 13:40141b362092 23 leftMotor=0;
ewoud 13:40141b362092 24 rightMotor=0;
ewoud 13:40141b362092 25 }
ewoud 7:e14e28d8cae3 26 int main() {
ewoud 7:e14e28d8cae3 27
ewoud 9:07189a75e979 28 // initialize (defined in inits.h)
ewoud 9:07189a75e979 29 initMotors();
ewoud 9:07189a75e979 30 initPIDs();
ewoud 9:07189a75e979 31
ewoud 9:07189a75e979 32 motorControlTicker.attach(&setGoFlag, RATE);
ewoud 9:07189a75e979 33
ewoud 13:40141b362092 34 startButton.rise(&systemStart);
ewoud 13:40141b362092 35 stopButton.rise(&systemStop);
ewoud 13:40141b362092 36
ewoud 8:55ca92c0e39d 37 endTimer.start(); //Run for 100 seconds.
ewoud 4:be465e9a12cb 38 while (endTimer.read() < 100){
ewoud 13:40141b362092 39 if (goFlag==true && systemOn==true){
ewoud 9:07189a75e979 40 // get 'emg' signal
ewoud 9:07189a75e979 41 request_pos = pot1.read();
ewoud 9:07189a75e979 42 request_neg = pot2.read();
ewoud 9:07189a75e979 43
ewoud 14:102a2b4f5c86 44 // determine if forward or backward signal is stronger
ewoud 9:07189a75e979 45 if (request_pos > request_neg){
ewoud 9:07189a75e979 46 request = request_pos;
ewoud 9:07189a75e979 47 }
ewoud 9:07189a75e979 48 else {
ewoud 9:07189a75e979 49 request = -request_neg;
ewoud 9:07189a75e979 50 }
ewoud 14:102a2b4f5c86 51
ewoud 14:102a2b4f5c86 52 // calculate required rotational velocity from the requested horizontal velocity
ewoud 14:102a2b4f5c86 53 // first get the current position from the motor encoders
ewoud 14:102a2b4f5c86 54 leftAngle=(leftQei.getPulses()/round)*360;
ewoud 14:102a2b4f5c86 55 rightAngle=(rightQei.getPulses()/round)*360;
ewoud 14:102a2b4f5c86 56 if (leftAngle < -90 or leftAngle > 90 or rightAngle < -90 or rightAngle > 90 ){
ewoud 14:102a2b4f5c86 57 pc.printf("out of bounds \n\r");
ewoud 14:102a2b4f5c86 58 leftRequest=0;
ewoud 14:102a2b4f5c86 59 rightRequest=0;
ewoud 14:102a2b4f5c86 60 if (leftAngle < -90){leftRequest=0.1;}
ewoud 14:102a2b4f5c86 61 if (leftAngle > 90){leftRequest=-0.1;}
ewoud 14:102a2b4f5c86 62 if (rightAngle < -90){rightRequest=0.1;}
ewoud 14:102a2b4f5c86 63 if (rightAngle > 90){rightRequest=-0.1;}
ewoud 14:102a2b4f5c86 64 }
ewoud 14:102a2b4f5c86 65 else {
ewoud 14:102a2b4f5c86 66 currentX = (tan(rightAngle*M_PI/180)*l)/(tan(leftAngle*M_PI/180)+tan(rightAngle*M_PI/180));
ewoud 14:102a2b4f5c86 67 currentY = tan(leftAngle*M_PI/180)*currentX;
ewoud 14:102a2b4f5c86 68
ewoud 14:102a2b4f5c86 69 if (sqrt(pow(currentX,2)+pow(currentY,2)) > armlength){ // too far from left arm
ewoud 14:102a2b4f5c86 70 //return 0;
ewoud 14:102a2b4f5c86 71 }
ewoud 14:102a2b4f5c86 72 if (sqrt(pow(l-currentX,2)+pow(currentY,2)) > armlength){ // too far from right arm
ewoud 14:102a2b4f5c86 73 //return 0;
ewoud 14:102a2b4f5c86 74 }
ewoud 14:102a2b4f5c86 75 // calculate the position to go to according the the current position + the distance that should be covered in this timestep
ewoud 14:102a2b4f5c86 76 toX=currentX+request*RATE*10; // should be request*RATE
ewoud 14:102a2b4f5c86 77 //toX=5;
ewoud 14:102a2b4f5c86 78 //toY=pCurrent.posY+0*RATE;
ewoud 14:102a2b4f5c86 79 toY=currentY;
ewoud 14:102a2b4f5c86 80
ewoud 14:102a2b4f5c86 81 toLeftAngle = atan(toY/toX)*180/M_PI;
ewoud 14:102a2b4f5c86 82 toRightAngle = atan(toY/(l-toX))*180/M_PI;
ewoud 14:102a2b4f5c86 83
ewoud 14:102a2b4f5c86 84 // calculate how much the angles should change in this timestep
ewoud 14:102a2b4f5c86 85 leftDeltaAngle=toLeftAngle-leftAngle;
ewoud 14:102a2b4f5c86 86 rightDeltaAngle=toRightAngle-rightAngle;
ewoud 14:102a2b4f5c86 87
ewoud 14:102a2b4f5c86 88 // calculate the neccesairy velocities to make these angles happen.
ewoud 14:102a2b4f5c86 89 leftRequest=(leftDeltaAngle/RATE);
ewoud 14:102a2b4f5c86 90 rightRequest=(rightDeltaAngle/RATE);
ewoud 14:102a2b4f5c86 91 }
ewoud 14:102a2b4f5c86 92
ewoud 14:102a2b4f5c86 93 // set the PID controller to go with that speed.
ewoud 14:102a2b4f5c86 94 leftController.setSetPoint(leftRequest);
ewoud 14:102a2b4f5c86 95 rightController.setSetPoint(rightRequest);
ewoud 14:102a2b4f5c86 96
ewoud 14:102a2b4f5c86 97 pc.printf("leftAngle: %f, rightAngle: %f, leftDeltaAngle: %f, rightDeltaAngle: %f \n\r", leftAngle, rightAngle, leftDeltaAngle, rightDeltaAngle);
ewoud 9:07189a75e979 98
ewoud 12:d7bb475bb82d 99 // *******************
ewoud 9:07189a75e979 100 // Velocity calculation
ewoud 12:d7bb475bb82d 101 // left
ewoud 9:07189a75e979 102 leftPulses = leftQei.getPulses();
ewoud 9:07189a75e979 103 leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE;
ewoud 9:07189a75e979 104 leftVelocity = leftVelocity/30000; // scale to 0 - 1, max velocity = 30000
ewoud 9:07189a75e979 105 leftPrevPulses = leftPulses;
ewoud 12:d7bb475bb82d 106
ewoud 12:d7bb475bb82d 107 // right
ewoud 12:d7bb475bb82d 108 rightPulses = rightQei.getPulses();
ewoud 12:d7bb475bb82d 109 rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE;
ewoud 12:d7bb475bb82d 110 rightVelocity = rightVelocity/30000; // scale to 0 - 1, max velocity = 30000
ewoud 12:d7bb475bb82d 111 rightPrevPulses = rightPulses;
ewoud 12:d7bb475bb82d 112
ewoud 12:d7bb475bb82d 113 // ***********
ewoud 9:07189a75e979 114 // PID control
ewoud 12:d7bb475bb82d 115 // left
ewoud 9:07189a75e979 116 leftController.setProcessValue(leftVelocity);
ewoud 9:07189a75e979 117 leftPwmDuty = leftController.compute();
ewoud 9:07189a75e979 118 if (leftPwmDuty < 0){
ewoud 9:07189a75e979 119 leftDirection = 0;
ewoud 9:07189a75e979 120 leftMotor = -leftPwmDuty;
ewoud 9:07189a75e979 121 }
ewoud 9:07189a75e979 122 else {
ewoud 9:07189a75e979 123 leftDirection = 1;
ewoud 9:07189a75e979 124 leftMotor = leftPwmDuty;
ewoud 9:07189a75e979 125 }
ewoud 9:07189a75e979 126
ewoud 12:d7bb475bb82d 127 // right
ewoud 12:d7bb475bb82d 128 rightController.setProcessValue(rightVelocity);
ewoud 12:d7bb475bb82d 129 rightPwmDuty = rightController.compute();
ewoud 12:d7bb475bb82d 130 if (rightPwmDuty < 0){
ewoud 14:102a2b4f5c86 131 rightDirection = 1;
ewoud 12:d7bb475bb82d 132 rightMotor = -rightPwmDuty;
ewoud 12:d7bb475bb82d 133 }
ewoud 12:d7bb475bb82d 134 else {
ewoud 14:102a2b4f5c86 135 rightDirection = 0;
ewoud 12:d7bb475bb82d 136 rightMotor = rightPwmDuty;
ewoud 12:d7bb475bb82d 137 }
ewoud 9:07189a75e979 138
ewoud 12:d7bb475bb82d 139 // User feedback
ewoud 14:102a2b4f5c86 140 scope.set(0, rightRequest);
ewoud 12:d7bb475bb82d 141 scope.set(1, rightPwmDuty);
ewoud 12:d7bb475bb82d 142 scope.set(2, rightVelocity);
ewoud 14:102a2b4f5c86 143 scope.set(3, leftAngle);
ewoud 14:102a2b4f5c86 144 scope.set(4, rightAngle);
ewoud 9:07189a75e979 145 scope.send();
ewoud 14:102a2b4f5c86 146 //pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
ewoud 8:55ca92c0e39d 147
ewoud 9:07189a75e979 148 goFlag=false;
ewoud 7:e14e28d8cae3 149 }
aberk 0:9bca35ae9c6b 150 }
aberk 0:9bca35ae9c6b 151
aberk 0:9bca35ae9c6b 152 //Stop motors.
ewoud 2:b2ccd9f044bb 153 leftMotor = 0;
ewoud 12:d7bb475bb82d 154 rightMotor = 0;
aberk 0:9bca35ae9c6b 155
aberk 0:9bca35ae9c6b 156 }