motor controller with P velocity control
Dependencies: HIDScope PID QEI mbed EMG
Fork of PID_VelocityExample by
main.cpp
- Committer:
- ewoud
- Date:
- 2015-10-06
- Revision:
- 14:102a2b4f5c86
- Parent:
- 13:40141b362092
- Child:
- 16:e9945e3b4712
File content as of revision 14:102a2b4f5c86:
//****************************************************************************/ // Includes #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 && 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; } void systemStart(){ systemOn=true; } void systemStop(){ systemOn=false; leftMotor=0; rightMotor=0; } int main() { // initialize (defined in inits.h) initMotors(); initPIDs(); motorControlTicker.attach(&setGoFlag, RATE); startButton.rise(&systemStart); stopButton.rise(&systemStop); endTimer.start(); //Run for 100 seconds. while (endTimer.read() < 100){ if (goFlag==true && systemOn==true){ // get 'emg' signal request_pos = pot1.read(); request_neg = pot2.read(); // determine if forward or backward signal is stronger if (request_pos > request_neg){ request = request_pos; } else { request = -request_neg; } // 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 // left leftPulses = leftQei.getPulses(); leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE; leftVelocity = leftVelocity/30000; // scale to 0 - 1, max velocity = 30000 leftPrevPulses = leftPulses; // right rightPulses = rightQei.getPulses(); rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE; rightVelocity = rightVelocity/30000; // scale to 0 - 1, max velocity = 30000 rightPrevPulses = rightPulses; // *********** // PID control // left leftController.setProcessValue(leftVelocity); leftPwmDuty = leftController.compute(); if (leftPwmDuty < 0){ leftDirection = 0; leftMotor = -leftPwmDuty; } else { leftDirection = 1; leftMotor = leftPwmDuty; } // right rightController.setProcessValue(rightVelocity); rightPwmDuty = rightController.compute(); if (rightPwmDuty < 0){ rightDirection = 1; rightMotor = -rightPwmDuty; } else { rightDirection = 0; rightMotor = rightPwmDuty; } // User feedback 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); goFlag=false; } } //Stop motors. leftMotor = 0; rightMotor = 0; }