motor controller with P velocity control
Dependencies: HIDScope PID QEI mbed EMG
Fork of PID_VelocityExample by
main.cpp
- Committer:
- ewoud
- Date:
- 2015-10-06
- Revision:
- 9:07189a75e979
- Parent:
- 8:55ca92c0e39d
- Child:
- 12:d7bb475bb82d
File content as of revision 9:07189a75e979:
//****************************************************************************/ // Includes #include "PID.h" #include "QEI.h" #include "HIDScope.h" #include "inits.h" // all globals, pin and variable initialization void setGoFlag(){ goFlag=true; } int main() { // initialize (defined in inits.h) initMotors(); initPIDs(); motorControlTicker.attach(&setGoFlag, RATE); endTimer.start(); //Run for 100 seconds. while (endTimer.read() < 100){ if (goFlag==true){ // get 'emg' signal request_pos = pot1.read(); request_neg = pot2.read(); // determine if forward or backward signal is stronger and set reference if (request_pos > request_neg){ request = request_pos; } else { request = -request_neg; } leftController.setSetPoint(request); // Velocity calculation leftPulses = leftQei.getPulses(); leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE; leftVelocity = leftVelocity/30000; // scale to 0 - 1, max velocity = 30000 leftPrevPulses = leftPulses; // PID control leftController.setProcessValue(leftVelocity); leftPwmDuty = leftController.compute(); if (leftPwmDuty < 0){ leftDirection = 0; leftMotor = -leftPwmDuty; } else { leftDirection = 1; leftMotor = leftPwmDuty; } // testing the right motor rightMotor = leftMotor; rightDirection=leftDirection; // User feadback scope.set(0, request); scope.set(1, leftPwmDuty); scope.set(2, leftVelocity); scope.send(); pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty); goFlag=false; } } //Stop motors. leftMotor = 0; //Close results file. //fclose(fp); }