motor controller with P velocity control
Dependencies: HIDScope PID QEI mbed EMG
Fork of PID_VelocityExample by
Diff: main.cpp
- Revision:
- 7:e14e28d8cae3
- Parent:
- 6:f58052f57505
- Child:
- 8:55ca92c0e39d
--- a/main.cpp Fri Sep 25 13:41:59 2015 +0000 +++ b/main.cpp Mon Oct 05 13:28:12 2015 +0000 @@ -3,14 +3,14 @@ //****************************************************************************/ #include "PID.h" #include "QEI.h" - #include "HIDScope.h" // Require the HIDScope library +#include "HIDScope.h" // Require the HIDScope library //****************************************************************************/ // Defines //****************************************************************************/ -#define RATE 0.01 -#define Kc 0.2 -#define Ti 0.0 +#define RATE 0.01 +#define Kc 0.35 +#define Ti 0.15 #define Td 0.0 //****************************************************************************/ @@ -29,11 +29,8 @@ QEI leftQei(D12, D13, NC, 624); PID leftController(Kc, Ti, Td, RATE); AnalogIn pot1(A0); -//------- -// Files -//------- -//LocalFileSystem local("local"); -//FILE* fp; +AnalogIn pot2(A1); + //-------- // Timers //-------- @@ -48,40 +45,24 @@ volatile float leftPwmDutyChange; volatile float leftVelocity = 0.0; //Velocity to reach. -int goal = 3000; float request; +float request_pos; +float request_neg; -//****************************************************************************/ -// Prototypes -//****************************************************************************/ -//Set motors to go "forward", brake off, not moving. -void initializeMotors(void); -//Set up PID controllers with appropriate limits and biases. -void initializePidControllers(void); -void initializeMotors(void){ - +int main() { + //Initialization of motor leftMotor.period_us(50); leftMotor = 1.0; leftBrake = 0.0; leftDirection = 1; - -} - -void initializePidControllers(void){ - - leftController.setInputLimits(0.0, 30000.0); - leftController.setOutputLimits(0.0, 1.0); + + //Initialization of PID controller + leftController.setInputLimits(-1.0, 1.0); + leftController.setOutputLimits(-1.0, 1.0); leftController.setBias(0.0); leftController.setMode(AUTO_MODE); -} - -int main() { - //scopeTimer.attach_us(&scope, &HIDScope::send, 1e4); - //Initialization. - initializeMotors(); - initializePidControllers(); //Open results file. //fp = fopen("/local/pidtest.csv", "w"); @@ -94,23 +75,39 @@ //Run for 3 seconds. while (endTimer.read() < 100){ - request = pot1.read()*30000; + request_pos = pot1.read(); + request_neg = pot2.read(); + + if (request_pos > request_neg){ + request = request_pos; + } + else { + request = -request_neg; + } + // Velocity calculation leftController.setSetPoint(request); leftPulses = leftQei.getPulses(); - leftVelocity = (leftPulses - leftPrevPulses) / RATE; + leftVelocity = ((float)(leftPulses - leftPrevPulses))/ (30000.0* RATE); leftPrevPulses = leftPulses; - + // PID control leftController.setProcessValue(leftVelocity); leftPwmDuty = leftController.compute(); - leftMotor = leftPwmDuty; + if (leftPwmDuty < 0){ + leftDirection = 0; + leftMotor = -leftPwmDuty; + } + else { + leftDirection = 1; + leftMotor = leftPwmDuty; + } scope.set(0, request); scope.set(1, leftPwmDuty); scope.set(2, leftVelocity); scope.send(); - pc.printf("request: %f, lefVelocity: %f, output: %f \n\r",request,leftVelocity,leftPwmDuty); + pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty); //fprintf(fp, "%f,%f\n", leftVelocity, goal); wait(RATE);