motor controller with P velocity control
Dependencies: HIDScope PID QEI mbed EMG
Fork of PID_VelocityExample by
Diff: main.cpp
- Revision:
- 16:e9945e3b4712
- Parent:
- 14:102a2b4f5c86
- Child:
- 17:034b50f49f46
diff -r 102a2b4f5c86 -r e9945e3b4712 main.cpp --- a/main.cpp Tue Oct 06 14:17:11 2015 +0000 +++ b/main.cpp Wed Oct 07 11:45:27 2015 +0000 @@ -8,13 +8,18 @@ #include "inits.h" // all globals, pin and variable initialization void setGoFlag(){ - if (goFlag==true && systemOn==true){ + if (goFlag==true && systemOn==true && calcFlag==false){ 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 setCalcFlag(){ + if (calcFlag==true && systemOn==true){ + pc.printf("ERROR: INCORRECT TIMINGS, look at the setCalcFlag \n\r"); + } + calcFlag=true; +} void systemStart(){ systemOn=true; } @@ -29,14 +34,14 @@ initMotors(); initPIDs(); + speedcalcTicker.attach(&setCalcFlag, calcRATE); 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){ + while (true){ + if (systemOn==true && calcFlag==true){ // get 'emg' signal request_pos = pot1.read(); request_neg = pot2.read(); @@ -51,29 +56,31 @@ // 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 ){ + // make them start at 45 degrees + leftAngle=(leftQei.getPulses()/round)*360+45; + rightAngle=(rightQei.getPulses()/round)*360+45; + + if (leftAngle < 0 or leftAngle > 90 or rightAngle < 0 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;} + //if (leftAngle < -45){leftRequest=0.1;} + //if (leftAngle > 45){leftRequest=-0.1;} + //if (rightAngle < -45){rightRequest=0.1;} + //if (rightAngle > 45){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 + //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 + //} + //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=currentX+request*calcRATE; // should be request*RATE //toX=5; //toY=pCurrent.posY+0*RATE; toY=currentY; @@ -86,15 +93,18 @@ rightDeltaAngle=toRightAngle-rightAngle; // calculate the neccesairy velocities to make these angles happen. - leftRequest=(leftDeltaAngle/RATE); - rightRequest=(rightDeltaAngle/RATE); + leftRequest=(leftDeltaAngle/calcRATE); + rightRequest=(rightDeltaAngle/calcRATE); } - + pc.printf("leftrequest: %f, rightrequest %f, leftAngle: %f, rightAngle: %f \n\r",leftRequest,rightRequest,leftAngle,rightAngle); + calcFlag=false; + } + else if (systemOn == true && goFlag == true){ // 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); + //pc.printf("leftAngle: %f, rightAngle: %f, leftDeltaAngle: %f, rightDeltaAngle: %f \n\r", leftAngle, rightAngle, leftDeltaAngle, rightDeltaAngle); // ******************* // Velocity calculation