Aaron Berk
Proportional, integral, derivative velocity control of a motor.
Revision 0:9bca35ae9c6b, committed 2010-08-03
- Comitter:
- aberk
- Date:
- Tue Aug 03 09:24:06 2010 +0000
- Child:
- 1:ac598811dd00
- Commit message:
- Version 1.0
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID_lib.lib Tue Aug 03 09:24:06 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/programs/PID_lib/latest \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI_lib.lib Tue Aug 03 09:24:06 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/programs/QEI_lib/latest \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Aug 03 09:24:06 2010 +0000
@@ -0,0 +1,104 @@
+//****************************************************************************/
+// Includes
+//****************************************************************************/
+#include "PID.h"
+#include "QEI.h"
+
+//****************************************************************************/
+// Defines
+//****************************************************************************/
+#define RATE 0.01
+#define Kc -2.6
+#define Ti 0.0
+#define Td 0.0
+
+//****************************************************************************/
+// Globals
+//****************************************************************************/
+//--------
+// Motors
+//--------
+//Left motor.
+PwmOut leftMotor(p23);
+DigitalOut leftBrake(p19);
+DigitalOut leftDirection(p28);
+QEI leftQei(p29, p30, NC, 624);
+PID leftController(Kc, Ti, Td, RATE);
+//-------
+// 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;
+
+//****************************************************************************/
+// 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 = 0;
+
+}
+
+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() {
+
+ //Initialization.
+ initializeMotors();
+ initializePidControllers();
+
+ //Open results file.
+ fp = fopen("/local/pidtest.csv", "w");
+
+ endTimer.start();
+
+ //Set velocity set point.
+ leftController.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.getRealOutput();
+ leftMotor = leftPwmDuty;
+ fprintf(fp, "%f,%f\n", leftVelocity, goal);
+ wait(RATE);
+ }
+
+ //Stop motors.
+ leftMotor = 1.0;
+
+ //Close results file.
+ fclose(fp);
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Aug 03 09:24:06 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/9114680c05da