motor controller with P velocity control
Dependencies: HIDScope PID QEI mbed EMG
Fork of PID_VelocityExample by
Diff: main.cpp
- Revision:
- 12:d7bb475bb82d
- Parent:
- 9:07189a75e979
- Child:
- 13:40141b362092
diff -r b351f99f0aa0 -r d7bb475bb82d main.cpp --- a/main.cpp Tue Oct 06 10:20:50 2015 +0000 +++ b/main.cpp Tue Oct 06 10:51:02 2015 +0000 @@ -32,14 +32,25 @@ request = -request_neg; } leftController.setSetPoint(request); + rightController.setSetPoint(request); + // ******************* // 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){ @@ -51,14 +62,22 @@ leftMotor = leftPwmDuty; } - // testing the right motor - rightMotor = leftMotor; - rightDirection=leftDirection; + // right + rightController.setProcessValue(rightVelocity); + rightPwmDuty = rightController.compute(); + if (rightPwmDuty < 0){ + rightDirection = 0; + rightMotor = -rightPwmDuty; + } + else { + rightDirection = 1; + rightMotor = rightPwmDuty; + } - // User feadback + // User feedback scope.set(0, request); - scope.set(1, leftPwmDuty); - scope.set(2, leftVelocity); + scope.set(1, rightPwmDuty); + scope.set(2, rightVelocity); scope.send(); pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty); @@ -68,8 +87,6 @@ //Stop motors. leftMotor = 0; + rightMotor = 0; - //Close results file. - //fclose(fp); - }