You are viewing an older revision! See the latest version
mbed Rover
The mbed Rover is the result of combining the Stinger Robot Kit, the QEI, PID, and IMU libraries along with a ITG3200 gyroscope, ADXL345 accelerometer, and a pair of Polulu MD10B breakout boards driven by the Motor library.
Simple Rover¶
The simplest implementation involves driving the motors forward a set number of pulses as measured by the quadrature encoders. This involves no speed feedback from the motors and causes the rover to drift in different directions in a non-deterministic manner. If there was a bias to the direction of drift it would also be difficult to correct it with simply changing the speed of one wheel as we would most likely want to dynamically change the speed of the wheel throughout any movement (e.g. increase PWM signal at the beginning to overcome static friction, then decrease to a lower level).
Import program
00001 /** 00002 * Drive a robot forwards or backwards by supplying a PWM signal to 00003 * H-bridges connected to the motors. 00004 */ 00005 00006 #include "Motor.h" 00007 #include "QEI.h" 00008 00009 #define PULSES_PER_REVOLUTION 624 00010 00011 Motor leftMotor(p21, p20, p19); //pwm, inB, inA 00012 Motor rightMotor(p22, p17, p18); //pwm, inA, inB 00013 QEI leftQei(p29, p30, NC, 624); //chanA, chanB, index, ppr 00014 QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr 00015 00016 int main() { 00017 00018 leftMotor.period(0.00005); //Set motor PWM periods to 20KHz. 00019 rightMotor.period(0.00005); 00020 00021 int leftPulses = 0; //How far the left wheel has travelled. 00022 int rightPulses = 0; //How far the right wheel has travelled. 00023 int distance = 6000; //Number of pulses to travel. 00024 00025 wait(5); //Wait a few seconds before we start moving. 00026 00027 leftMotor.speed(0.4); 00028 rightMotor.speed(0.4); 00029 00030 while((leftPulses < distance) || (rightPulses < distance)){ 00031 00032 leftPulses = leftQei.getPulses(); 00033 rightPulses = rightQei.getPulses(); 00034 00035 } 00036 00037 leftMotor.brake(); 00038 rightMotor.brake(); 00039 00040 }
PID Rover¶
A more advanced implementation uses a PID controller to try and keep the speeds of the two motors the same. The PID tuning constants for the specific motors were calculated using the method outlined in the PID cookbook page. As this method dynamically controls the speed of the motors, we can change the speed set point of one motor if we find a consistent bias in any drift along a straight line section.
Import program
00001 /** 00002 * Drive a robot forwards or backwards by using a PID controller to vary 00003 * the PWM signal to H-bridges connected to the motors to attempt to maintain 00004 * a constant velocity. 00005 */ 00006 00007 #include "Motor.h" 00008 #include "QEI.h" 00009 #include "PID.h" 00010 00011 Motor leftMotor(p21, p20, p19); //pwm, inB, inA 00012 Motor rightMotor(p22, p17, p18); //pwm, inA, inB 00013 QEI leftQei(p29, p30, NC, 624); //chanA, chanB, index, ppr 00014 QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr 00015 //Tuning parameters calculated from step tests; 00016 //see http://mbed.org/cookbook/PID for examples. 00017 PID leftPid(0.4312, 0.1, 0.0, 0.01); //Kc, Ti, Td 00018 PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td 00019 00020 int main() { 00021 00022 leftMotor.period(0.00005); //Set motor PWM periods to 20KHz. 00023 rightMotor.period(0.00005); 00024 00025 //Input and output limits have been determined 00026 //empirically with the specific motors being used. 00027 //Change appropriately for different motors. 00028 //Input units: counts per second. 00029 //Output units: PwmOut duty cycle as %. 00030 //Default limits are for moving forward. 00031 leftPid.setInputLimits(0, 3000); 00032 leftPid.setOutputLimits(0.0, 0.9); 00033 leftPid.setMode(AUTO_MODE); 00034 rightPid.setInputLimits(0, 3200); 00035 rightPid.setOutputLimits(0.0, 0.9); 00036 rightPid.setMode(AUTO_MODE); 00037 00038 00039 int leftPulses = 0; //How far the left wheel has travelled. 00040 int leftPrevPulses = 0; //The previous reading of how far the left wheel 00041 //has travelled. 00042 float leftVelocity = 0.0; //The velocity of the left wheel in pulses per 00043 //second. 00044 int rightPulses = 0; //How far the right wheel has travelled. 00045 int rightPrevPulses = 0; //The previous reading of how far the right wheel 00046 //has travelled. 00047 float rightVelocity = 0.0; //The velocity of the right wheel in pulses per 00048 //second. 00049 int distance = 6000; //Number of pulses to travel. 00050 00051 wait(5); //Wait a few seconds before we start moving. 00052 00053 //Velocity to mantain in pulses per second. 00054 leftPid.setSetPoint(1000); 00055 rightPid.setSetPoint(975); 00056 00057 while ((leftPulses < distance) || (rightPulses < distance)) { 00058 00059 //Get the current pulse count and subtract the previous one to 00060 //calculate the current velocity in counts per second. 00061 leftPulses = leftQei.getPulses(); 00062 leftVelocity = (leftPulses - leftPrevPulses) / 0.01; 00063 leftPrevPulses = leftPulses; 00064 //Use the absolute value of velocity as the PID controller works 00065 //in the % (unsigned) domain and will get confused with -ve values. 00066 leftPid.setProcessValue(fabs(leftVelocity)); 00067 leftMotor.speed(leftPid.compute()); 00068 00069 rightPulses = rightQei.getPulses(); 00070 rightVelocity = (rightPulses - rightPrevPulses) / 0.01; 00071 rightPrevPulses = rightPulses; 00072 rightPid.setProcessValue(fabs(rightVelocity)); 00073 rightMotor.speed(rightPid.compute()); 00074 00075 wait(0.01); 00076 00077 } 00078 00079 leftMotor.brake(); 00080 rightMotor.brake(); 00081 00082 }