Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of PID_VelocityExample by
Revision 2:646e3bca12a8, committed 2016-06-27
- Comitter:
- soulx
- Date:
- Mon Jun 27 23:12:28 2016 +0000
- Parent:
- 1:ac598811dd00
- Commit message:
- edit 2 motor
Changed in this revision
--- a/QEI.lib Sat Nov 27 11:37:20 2010 +0000 +++ b/QEI.lib Mon Jun 27 23:12:28 2016 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa +https://developer.mbed.org/teams/Betago/code/QEI/#0da4d510e7b8
--- a/main.cpp Sat Nov 27 11:37:20 2010 +0000 +++ b/main.cpp Mon Jun 27 23:12:28 2016 +0000 @@ -3,12 +3,13 @@ //****************************************************************************/ #include "PID.h" #include "QEI.h" +#include "mbed.h" //****************************************************************************/ // Defines //****************************************************************************/ -#define RATE 0.01 -#define Kc -2.6 +#define RATE 0.1 +#define Kc 1.0 #define Ti 0.0 #define Td 0.0 @@ -16,19 +17,27 @@ // Globals //****************************************************************************/ //-------- -// Motors +// Motors //-------- //Left motor. -PwmOut leftMotor(p23); -DigitalOut leftBrake(p19); -DigitalOut leftDirection(p28); -QEI leftQei(p29, p30, NC, 624); +PwmOut leftMotor(PB_4); +DigitalOut leftBrake(PA_15); +DigitalOut leftDirection(PB_7); +QEI leftQei(PC_10, PC_12, NC, 624); PID leftController(Kc, Ti, Td, RATE); -//------- -// Files -//------- -LocalFileSystem local("local"); -FILE* fp; + +//Right motor. +PwmOut rightMotor(PB_10); +DigitalOut rightBrake(PA_13); +DigitalOut rightDirection(PA_14); +QEI rightQei(PA_5, PA_6, NC, 624); +PID rightController(Kc, Ti, Td, RATE); + +//-------- +// Serial +//-------- +Serial pc(SERIAL_TX,SERIAL_RX); + //-------- // Timers //-------- @@ -40,8 +49,14 @@ volatile int leftPrevPulses = 0; volatile float leftPwmDuty = 1.0; volatile float leftVelocity = 0.0; + +volatile int rightPulses = 0; +volatile int rightPrevPulses = 0; +volatile float rightPwmDuty = 1.0; +volatile float rightVelocity = 0.0; + //Velocity to reach. -int goal = 3000; +int goal = 50; //****************************************************************************/ // Prototypes @@ -51,54 +66,140 @@ //Set up PID controllers with appropriate limits and biases. void initializePidControllers(void); -void initializeMotors(void){ +void initializeMotors(void) +{ leftMotor.period_us(50); leftMotor = 1.0; leftBrake = 0.0; leftDirection = 0; + rightMotor.period_us(50); + rightMotor = 1.0; + rightBrake = 0.0; + rightDirection = 0; + } -void initializePidControllers(void){ +void initializePidControllers(void) +{ leftController.setInputLimits(0.0, 10500.0); leftController.setOutputLimits(0.0, 1.0); leftController.setBias(1.0); leftController.setMode(AUTO_MODE); + rightController.setInputLimits(0.0, 10500.0); + rightController.setOutputLimits(0.0, 1.0); + rightController.setBias(1.0); + rightController.setMode(AUTO_MODE); } -int main() { +int main() +{ + int updateTime=0; + + pc.baud(115200); + pc.printf("start\n\r"); //Initialization. initializeMotors(); + + wait(1); + /* + while(1){ + leftPulses = leftQei.getPulses(); + rightPulses = rightQei.getPulses(); + pc.printf("leftPulses = %d\n\r",leftPulses); + pc.printf("rightPulses = %d\n\r",rightPulses); + wait(0.5); + } + */ initializePidControllers(); - //Open results file. - fp = fopen("/local/pidtest.csv", "w"); - + endTimer.start(); //Set velocity set point. leftController.setSetPoint(goal); + rightController.setSetPoint(goal); //Run for 3 seconds. - while (endTimer.read() < 3.0){ - leftPulses = leftQei.getPulses(); - leftVelocity = (leftPulses - leftPrevPulses) / RATE; - leftPrevPulses = leftPulses; - leftController.setProcessValue(leftVelocity); - leftPwmDuty = leftController.compute(); - leftMotor = leftPwmDuty; - fprintf(fp, "%f,%f\n", leftVelocity, goal); - wait(RATE); + while (endTimer.read() < 15.0f) { + + leftDirection = 1; + rightDirection = 1; + + if( (endTimer.read_ms()-updateTime) > (RATE*1000)) { + leftPulses = leftQei.getPulses(); + leftVelocity = (leftPulses - leftPrevPulses) / RATE; + leftPrevPulses = leftPulses; + leftController.setProcessValue(leftVelocity); + leftPwmDuty = leftController.compute(); + leftMotor = leftPwmDuty; + + rightPulses = rightQei.getPulses(); + rightVelocity = (rightPulses - rightPrevPulses) / RATE; + rightPrevPulses = rightPulses; + rightController.setProcessValue(rightVelocity); + rightPwmDuty = rightController.compute(); + rightMotor = rightPwmDuty; + + updateTime = endTimer.read_ms(); + + pc.printf("leftPulses = %d\n\r",leftPulses); + pc.printf("rightPulses = %d\n\r",rightPulses); + pc.printf("leftPwm = %f\n\r",leftPwmDuty); + pc.printf("rightPwm = %f\n\r",rightPwmDuty); + } + //wait(RATE); } //Stop motors. leftMotor = 1.0; + rightMotor =1.0; - //Close results file. - fclose(fp); + rightDirection = 0; + leftDirection = 0; + endTimer.reset(); + wait(1.0f); + + pc.printf("stop\n]r"); + + //Set velocity set point. + leftController.setSetPoint(goal+1000); + rightController.setSetPoint(goal); + + leftDirection = 1; + rightDirection = 1; + + //Run for 2 seconds. + while (endTimer.read() < 2.0f) { + if(endTimer.read_ms()-updateTime > RATE*1000) { + leftPulses = leftQei.getPulses(); + leftVelocity = (leftPulses - leftPrevPulses) / RATE; + leftPrevPulses = leftPulses; + leftController.setProcessValue(leftVelocity); + leftPwmDuty = leftController.compute(); + leftMotor = leftPwmDuty; + + rightPulses = rightQei.getPulses(); + rightVelocity = (rightPulses - rightPrevPulses) / RATE; + rightPrevPulses = rightPulses; + rightController.setProcessValue(rightVelocity); + rightPwmDuty = rightController.compute(); + rightMotor = rightPwmDuty; + + updateTime = endTimer.read_ms(); + + pc.printf("leftPulses = %d\n\r",leftPulses); + pc.printf("rightPulses = %d\n\r",rightPulses); + } + //wait(RATE); + } + + rightDirection = 0; + leftDirection = 0; + }
--- a/mbed.bld Sat Nov 27 11:37:20 2010 +0000 +++ b/mbed.bld Mon Jun 27 23:12:28 2016 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/9114680c05da +http://mbed.org/users/mbed_official/code/mbed/builds/6c34061e7c34 \ No newline at end of file