main.cpp

00001 /**
00002  PID Controller for maintaining constant velocity for robots using incremental encoder
00003  */
00004 
00005 #include "Motor.h"
00006 #include "PID.h"
00007 #include "IncrementalEncoder.h"
00008 
00009 IncrementalEncoder leftQei(p29);
00010 IncrementalEncoder rightQei(p30);
00011 Motor leftMotor(p23, p6, p5); // pwm, fwd, rev
00012 Motor rightMotor(p22, p8, p7); // pwm, fwd, rev
00013 
00014 PID leftPid(0.4312, 0.1, 0.0, 0.01);  //Kc, Ti, Td
00015 PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
00016 
00017 int main() {
00018 
00019     leftMotor.period(0.00005);  //Set motor PWM periods to 20KHz.
00020     rightMotor.period(0.00005);
00021 
00022     //Input and output limits have been determined
00023     //empirically with the specific motors being used.
00024     //Change appropriately for different motors.
00025     //Input  units: counts per second.
00026     //Output units: PwmOut duty cycle as %.
00027     //Default limits are for moving forward.
00028     leftPid.setInputLimits(0, 3000);
00029     leftPid.setOutputLimits(0.0, 0.9);
00030     leftPid.setMode(AUTO_MODE);
00031     rightPid.setInputLimits(0, 3200);
00032     rightPid.setOutputLimits(0.0, 0.9);
00033     rightPid.setMode(AUTO_MODE);
00034 
00035 
00036     int leftPulses      = 0; //How far the left wheel has travelled.
00037     int leftPrevPulses  = 0; //The previous reading of how far the left wheel
00038     //has travelled.
00039     float leftVelocity  = 0.0; //The velocity of the left wheel in pulses per
00040     //second.
00041     int rightPulses     = 0; //How far the right wheel has travelled.
00042     int rightPrevPulses = 0; //The previous reading of how far the right wheel
00043     //has travelled.
00044     float rightVelocity = 0.0; //The velocity of the right wheel in pulses per
00045     //second.
00046     int distance     = 600; //Number of pulses to travel.
00047 
00048     wait(5); //Wait a few seconds before we start moving.
00049 
00050     //Velocity to mantain in pulses per second.
00051     leftPid.setSetPoint(20);
00052     rightPid.setSetPoint(20);
00053 
00054     while ((leftPulses < distance) || (rightPulses < distance)) {
00055 
00056         //Get the current pulse count and subtract the previous one to
00057         //calculate the current velocity in counts per second.
00058         leftPulses = leftQei.readTotal();
00059         leftVelocity = (leftPulses - leftPrevPulses) / 0.01;
00060         leftPrevPulses = leftPulses;
00061         //Use the absolute value of velocity as the PID controller works
00062         //in the % (unsigned) domain and will get confused with -ve values.
00063         leftPid.setProcessValue(fabs(leftVelocity));
00064         leftMotor.speed(leftPid.compute());
00065 
00066         rightPulses = rightQei.readTotal();
00067         rightVelocity = (rightPulses - rightPrevPulses) / 0.01;
00068         rightPrevPulses = rightPulses;
00069         rightPid.setProcessValue(fabs(rightVelocity));
00070         rightMotor.speed(rightPid.compute());
00071 
00072         wait(0.01);
00073 
00074     }
00075 
00076     leftMotor.brake();
00077     rightMotor.brake();
00078 
00079 }