main.cpp

00001 //****************************************************************************/
00002 // Includes
00003 //****************************************************************************/
00004 #include "PID.h"
00005 #include "QEI.h"
00006 #include "mbed.h"
00007 
00008 //****************************************************************************/
00009 // Defines
00010 //****************************************************************************/
00011 #define RATE  0.1
00012 #define Kc   1.0
00013 #define Ti    0.0
00014 #define Td    0.0
00015 
00016 //****************************************************************************/
00017 // Globals
00018 //****************************************************************************/
00019 //--------
00020 // Motors
00021 //--------
00022 //Left motor.
00023 PwmOut leftMotor(PB_4);
00024 DigitalOut leftBrake(PA_15);
00025 DigitalOut leftDirection(PB_7);
00026 QEI leftQei(PC_10, PC_12, NC, 624);
00027 PID leftController(Kc, Ti, Td, RATE);
00028 
00029 //Right motor.
00030 PwmOut rightMotor(PB_10);
00031 DigitalOut rightBrake(PA_13);
00032 DigitalOut rightDirection(PA_14);
00033 QEI rightQei(PA_5, PA_6, NC, 624);
00034 PID rightController(Kc, Ti, Td, RATE);
00035 
00036 //--------
00037 // Serial
00038 //--------
00039 Serial pc(SERIAL_TX,SERIAL_RX);
00040 
00041 //--------
00042 // Timers
00043 //--------
00044 Timer endTimer;
00045 //--------------------
00046 // Working variables.
00047 //--------------------
00048 volatile int leftPulses     = 0;
00049 volatile int leftPrevPulses = 0;
00050 volatile float leftPwmDuty  = 1.0;
00051 volatile float leftVelocity = 0.0;
00052 
00053 volatile int rightPulses     = 0;
00054 volatile int rightPrevPulses = 0;
00055 volatile float rightPwmDuty  = 1.0;
00056 volatile float rightVelocity = 0.0;
00057 
00058 //Velocity to reach.
00059 int goal = 50;
00060 
00061 //****************************************************************************/
00062 // Prototypes
00063 //****************************************************************************/
00064 //Set motors to go "forward", brake off, not moving.
00065 void initializeMotors(void);
00066 //Set up PID controllers with appropriate limits and biases.
00067 void initializePidControllers(void);
00068 
00069 void initializeMotors(void)
00070 {
00071 
00072     leftMotor.period_us(50);
00073     leftMotor = 1.0;
00074     leftBrake = 0.0;
00075     leftDirection = 0;
00076 
00077     rightMotor.period_us(50);
00078     rightMotor = 1.0;
00079     rightBrake = 0.0;
00080     rightDirection = 0;
00081 
00082 }
00083 
00084 void initializePidControllers(void)
00085 {
00086 
00087     leftController.setInputLimits(0.0, 10500.0);
00088     leftController.setOutputLimits(0.0, 1.0);
00089     leftController.setBias(1.0);
00090     leftController.setMode(AUTO_MODE);
00091 
00092     rightController.setInputLimits(0.0, 10500.0);
00093     rightController.setOutputLimits(0.0, 1.0);
00094     rightController.setBias(1.0);
00095     rightController.setMode(AUTO_MODE);
00096 }
00097 
00098 int main()
00099 {
00100 
00101     int updateTime=0;
00102 
00103     pc.baud(115200);
00104  pc.printf("start\n\r");
00105     //Initialization.
00106     initializeMotors();
00107     
00108     wait(1);
00109     /*
00110     while(1){
00111         leftPulses = leftQei.getPulses();
00112         rightPulses = rightQei.getPulses();
00113         pc.printf("leftPulses = %d\n\r",leftPulses);
00114         pc.printf("rightPulses = %d\n\r",rightPulses);
00115         wait(0.5);
00116         }
00117     */
00118     initializePidControllers();
00119 
00120 
00121     endTimer.start();
00122 
00123     //Set velocity set point.
00124     leftController.setSetPoint(goal);
00125     rightController.setSetPoint(goal);
00126 
00127     //Run for 3 seconds.
00128     while (endTimer.read() < 15.0f) {
00129         
00130         leftDirection = 1;
00131         rightDirection = 1;
00132         
00133         if( (endTimer.read_ms()-updateTime) > (RATE*1000)) {
00134             leftPulses = leftQei.getPulses();
00135             leftVelocity = (leftPulses - leftPrevPulses) / RATE;
00136             leftPrevPulses = leftPulses;
00137             leftController.setProcessValue(leftVelocity);
00138             leftPwmDuty = leftController.compute();
00139             leftMotor = leftPwmDuty;
00140 
00141             rightPulses = rightQei.getPulses();
00142             rightVelocity = (rightPulses - rightPrevPulses) / RATE;
00143             rightPrevPulses = rightPulses;
00144             rightController.setProcessValue(rightVelocity);
00145             rightPwmDuty = rightController.compute();
00146             rightMotor = rightPwmDuty;
00147 
00148             updateTime = endTimer.read_ms();
00149 
00150             pc.printf("leftPulses = %d\n\r",leftPulses);
00151             pc.printf("rightPulses = %d\n\r",rightPulses);
00152             pc.printf("leftPwm = %f\n\r",leftPwmDuty);
00153             pc.printf("rightPwm = %f\n\r",rightPwmDuty);
00154         }
00155         //wait(RATE);
00156     }
00157 
00158     //Stop motors.
00159     leftMotor  = 1.0;
00160     rightMotor =1.0;
00161     
00162     rightDirection = 0;
00163     leftDirection = 0;
00164     endTimer.reset();
00165     wait(1.0f);
00166 
00167     pc.printf("stop\n]r");
00168 
00169     //Set velocity set point.
00170     leftController.setSetPoint(goal+1000);
00171     rightController.setSetPoint(goal);
00172 
00173         leftDirection = 1;
00174         rightDirection = 1;
00175 
00176     //Run for 2 seconds.
00177     while (endTimer.read() < 2.0f) {
00178 
00179         if(endTimer.read_ms()-updateTime > RATE*1000) {
00180             leftPulses = leftQei.getPulses();
00181             leftVelocity = (leftPulses - leftPrevPulses) / RATE;
00182             leftPrevPulses = leftPulses;
00183             leftController.setProcessValue(leftVelocity);
00184             leftPwmDuty = leftController.compute();
00185             leftMotor = leftPwmDuty;
00186 
00187             rightPulses = rightQei.getPulses();
00188             rightVelocity = (rightPulses - rightPrevPulses) / RATE;
00189             rightPrevPulses = rightPulses;
00190             rightController.setProcessValue(rightVelocity);
00191             rightPwmDuty = rightController.compute();
00192             rightMotor = rightPwmDuty;
00193 
00194             updateTime = endTimer.read_ms();
00195 
00196             pc.printf("leftPulses = %d\n\r",leftPulses);
00197             pc.printf("rightPulses = %d\n\r",rightPulses);
00198         }
00199         //wait(RATE);
00200     }
00201     
00202     rightDirection = 0;
00203     leftDirection = 0;
00204     
00205 }