motor controller with P velocity control
Dependencies: HIDScope PID QEI mbed EMG
Fork of PID_VelocityExample by
Diff: main.cpp
- Revision:
- 19:3ca10fe26131
- Parent:
- 18:4ee32b922251
diff -r 4ee32b922251 -r 3ca10fe26131 main.cpp --- a/main.cpp Wed Oct 07 13:19:25 2015 +0000 +++ b/main.cpp Mon Oct 12 08:13:29 2015 +0000 @@ -52,7 +52,9 @@ else { request = -request_neg; } - + + request=request*maxspeed; // turn [-1, 1] into [-max cm/s, max cm/s] + // calculate required rotational velocity from the requested horizontal velocity // first get the current position from the motor encoders // make them start at 45 degree. @@ -79,21 +81,22 @@ //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*calcRATE; // should be request*RATE + toX=currentX+request/200*calcRATE; // should be request*RATE, 200 is a magical number to make request work in cm/s + //toY=currentY+0*calcRATE; toY=currentY+0*calcRATE; - + 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)/10; - rightDeltaAngle=(toRightAngle-rightAngle)/10; + leftDeltaAngle=(toLeftAngle-leftAngle); + rightDeltaAngle=(toRightAngle-rightAngle); // calculate the neccesairy velocities to make these angles happen. leftRequest=(leftDeltaAngle/calcRATE); rightRequest=(rightDeltaAngle/calcRATE); } - //pc.printf("leftrequest: %f, rightrequest %f, leftAngle: %f, rightAngle: %f \n\r",leftRequest,rightRequest,leftAngle,rightAngle); + //pc.printf("leftRequest: %f, rightRequest %f, curX: %f, curY: %f \n\r",leftRequest,rightRequest,currentX,currentY); calcFlag=false; } else if (systemOn == true && goFlag == true){