motor controller with P velocity control
Dependencies: HIDScope PID QEI mbed EMG
Fork of PID_VelocityExample by
Diff: main.cpp
- Revision:
- 18:4ee32b922251
- Parent:
- 17:034b50f49f46
- Child:
- 19:3ca10fe26131
diff -r 034b50f49f46 -r 4ee32b922251 main.cpp --- a/main.cpp Wed Oct 07 12:19:30 2015 +0000 +++ b/main.cpp Wed Oct 07 13:19:25 2015 +0000 @@ -4,7 +4,6 @@ #include "QEI.h" #include "HIDScope.h" #include <math.h> -//#include "Point.cpp" #include "inits.h" // all globals, pin and variable initialization void setGoFlag(){ @@ -53,10 +52,10 @@ else { request = -request_neg; } - + // calculate required rotational velocity from the requested horizontal velocity // first get the current position from the motor encoders - // make them start at 45 degrees + // make them start at 45 degree. leftAngle=(leftQei.getPulses()/round)*360+45; rightAngle=(rightQei.getPulses()/round)*360+45; @@ -81,9 +80,7 @@ //} // calculate the position to go to according the the current position + the distance that should be covered in this timestep toX=currentX+request*calcRATE; // should be request*RATE - //toX=5; - //toY=pCurrent.posY+0*RATE; - toY=currentY; + toY=currentY+0*calcRATE; toLeftAngle = atan(toY/toX)*180/M_PI; toRightAngle = atan(toY/(l-toX))*180/M_PI; @@ -97,10 +94,9 @@ rightRequest=(rightDeltaAngle/calcRATE); } //pc.printf("leftrequest: %f, rightrequest %f, leftAngle: %f, rightAngle: %f \n\r",leftRequest,rightRequest,leftAngle,rightAngle); - didCalc=true; calcFlag=false; } - else if (systemOn == true && goFlag == true && didCalc==false){ + else if (systemOn == true && goFlag == true){ // set the PID controller to go with that speed. leftController.setSetPoint(leftRequest); @@ -154,9 +150,9 @@ } // User feedback - scope.set(0, rightRequest); + scope.set(0, rightRequest*10); scope.set(1, rightPwmDuty); - scope.set(2, rightVelocity); + scope.set(2, rightVelocity*10); scope.set(3, leftAngle); scope.set(4, rightAngle); scope.send(); @@ -164,13 +160,6 @@ goFlag=false; } - - if (systemOn == true && goFlag == true && didCalc == true){ - //leftPrevPulses = leftQei.getPulses(); - //rightPrevPulses = rightQei.getPulses(); - didCalc=false; - //goFlag=false; - } } //Stop motors.