motor controller with P velocity control
Dependencies: HIDScope PID QEI mbed EMG
Fork of PID_VelocityExample by
main.cpp
- Committer:
- ewoud
- Date:
- 2015-09-24
- Revision:
- 4:be465e9a12cb
- Parent:
- 3:4c93be3a9010
- Child:
- 5:8ae6d935a16a
File content as of revision 4:be465e9a12cb:
//****************************************************************************/ // Includes //****************************************************************************/ #include "PID.h" #include "QEI.h" #include "HIDScope.h" // Require the HIDScope library //****************************************************************************/ // Defines //****************************************************************************/ #define RATE 0.01 #define Kc -2.6 #define Ti 0.0 #define Td 0.0 //****************************************************************************/ // Globals //****************************************************************************/ Serial pc(USBTX, USBRX); HIDScope scope(2); // Instantize a 2-channel HIDScope object Ticker scopeTimer; // Instantize the timer for sending data to the PC //-------- // Motors //-------- //Left motor. PwmOut leftMotor(D5); DigitalOut leftBrake(D3); DigitalOut leftDirection(D4); QEI leftQei(D12, D13, NC, 624); PID leftController(Kc, Ti, Td, RATE); AnalogIn pot1(A0); //------- // Files //------- //LocalFileSystem local("local"); //FILE* fp; //-------- // Timers //-------- Timer endTimer; //-------------------- // Working variables. //-------------------- volatile int leftPulses = 0; volatile int leftPrevPulses = 0; volatile float leftPwmDuty = 1.0; volatile float leftVelocity = 0.0; //Velocity to reach. int goal = 3000; float measure; //****************************************************************************/ // 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){ leftMotor.period_us(50); leftMotor = 1.0; leftBrake = 0.0; leftDirection = 1; } void initializePidControllers(void){ leftController.setInputLimits(0.0, 10500.0); leftController.setOutputLimits(0.0, 1.0); leftController.setBias(1.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"); endTimer.start(); //Set velocity set point. //Run for 3 seconds. while (endTimer.read() < 100){ measure = pot1.read()*10500; leftController.setSetPoint(measure); leftPulses = leftQei.getPulses(); leftVelocity = (leftPulses - leftPrevPulses) / RATE; leftPrevPulses = leftPulses; scope.set(0, measure); scope.set(1, leftVelocity); leftController.setProcessValue(-leftVelocity); leftPwmDuty = leftController.compute(); leftMotor = leftPwmDuty; pc.printf("leftpusles: %d, lefVelocity: %f, leftPwmDuty: %f \n\r",leftPulses,leftVelocity,leftPwmDuty); //fprintf(fp, "%f,%f\n", leftVelocity, goal); wait(RATE); } //Stop motors. leftMotor = 0; //Close results file. //fclose(fp); }