motor controller with P velocity control
Dependencies: HIDScope PID QEI mbed EMG
Fork of PID_VelocityExample by
Diff: main.cpp
- Revision:
- 14:102a2b4f5c86
- Parent:
- 13:40141b362092
- Child:
- 16:e9945e3b4712
diff -r 40141b362092 -r 102a2b4f5c86 main.cpp --- a/main.cpp Tue Oct 06 11:30:38 2015 +0000 +++ b/main.cpp Tue Oct 06 14:17:11 2015 +0000 @@ -3,10 +3,13 @@ #include "PID.h" #include "QEI.h" #include "HIDScope.h" +#include <math.h> +//#include "Point.cpp" #include "inits.h" // all globals, pin and variable initialization void setGoFlag(){ - if (goFlag==true){ + if (goFlag==true && systemOn==true){ + pc.printf("ERROR: INCORRECT TIMINGS, look at the setGoFlag \n\r"); // flag is already set true: code too slow or frequency too high } goFlag=true; @@ -38,15 +41,60 @@ request_pos = pot1.read(); request_neg = pot2.read(); - // determine if forward or backward signal is stronger and set reference + // determine if forward or backward signal is stronger if (request_pos > request_neg){ request = request_pos; } else { request = -request_neg; } - leftController.setSetPoint(request); - rightController.setSetPoint(request); + + // calculate required rotational velocity from the requested horizontal velocity + // first get the current position from the motor encoders + leftAngle=(leftQei.getPulses()/round)*360; + rightAngle=(rightQei.getPulses()/round)*360; + if (leftAngle < -90 or leftAngle > 90 or rightAngle < -90 or rightAngle > 90 ){ + pc.printf("out of bounds \n\r"); + leftRequest=0; + rightRequest=0; + if (leftAngle < -90){leftRequest=0.1;} + if (leftAngle > 90){leftRequest=-0.1;} + if (rightAngle < -90){rightRequest=0.1;} + if (rightAngle > 90){rightRequest=-0.1;} + } + else { + currentX = (tan(rightAngle*M_PI/180)*l)/(tan(leftAngle*M_PI/180)+tan(rightAngle*M_PI/180)); + currentY = tan(leftAngle*M_PI/180)*currentX; + + if (sqrt(pow(currentX,2)+pow(currentY,2)) > armlength){ // too far from left arm + //return 0; + } + if (sqrt(pow(l-currentX,2)+pow(currentY,2)) > armlength){ // too far from right arm + //return 0; + } + // calculate the position to go to according the the current position + the distance that should be covered in this timestep + toX=currentX+request*RATE*10; // should be request*RATE + //toX=5; + //toY=pCurrent.posY+0*RATE; + toY=currentY; + + toLeftAngle = atan(toY/toX)*180/M_PI; + toRightAngle = atan(toY/(l-toX))*180/M_PI; + + // calculate how much the angles should change in this timestep + leftDeltaAngle=toLeftAngle-leftAngle; + rightDeltaAngle=toRightAngle-rightAngle; + + // calculate the neccesairy velocities to make these angles happen. + leftRequest=(leftDeltaAngle/RATE); + rightRequest=(rightDeltaAngle/RATE); + } + + // set the PID controller to go with that speed. + leftController.setSetPoint(leftRequest); + rightController.setSetPoint(rightRequest); + + pc.printf("leftAngle: %f, rightAngle: %f, leftDeltaAngle: %f, rightDeltaAngle: %f \n\r", leftAngle, rightAngle, leftDeltaAngle, rightDeltaAngle); // ******************* // Velocity calculation @@ -80,20 +128,22 @@ rightController.setProcessValue(rightVelocity); rightPwmDuty = rightController.compute(); if (rightPwmDuty < 0){ - rightDirection = 0; + rightDirection = 1; rightMotor = -rightPwmDuty; } else { - rightDirection = 1; + rightDirection = 0; rightMotor = rightPwmDuty; } // User feedback - scope.set(0, request); + scope.set(0, rightRequest); scope.set(1, rightPwmDuty); scope.set(2, rightVelocity); + scope.set(3, leftAngle); + scope.set(4, rightAngle); scope.send(); - pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty); + //pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty); goFlag=false; }