A robot rover with distance sensing and audiovisual effects
Dependencies: 4DGL-uLCD-SE Motordriver PID mbed
Fork of PIDRover by
main.cpp
- Committer:
- aberk
- Date:
- 2010-09-10
- Revision:
- 0:be99ed42340d
- Child:
- 1:c70bc01ebfdd
File content as of revision 0:be99ed42340d:
/** * Drive a robot forwards or backwards by using a PID controller to vary * the PWM signal to H-bridges connected to the motors to attempt to maintain * a constant velocity. */ #include "Motor.h" #include "QEI.h" #include "PID.h" Motor leftMotor(p21, p20, p19); //pwm, inB, inA Motor rightMotor(p22, p17, p18); //pwm, inA, inB QEI leftQei(p29, p30, NC, 624); //chanA, chanB, index, ppr QEI rightQei(p27, p28, NC, 624); //chanB, chanA, index, ppr //Tuning parameters calculated from step tests; //see http://mbed.org/cookbook/PID for examples. PID leftPid(0.4312, 0.1, 0.0, 0.01); //Kc, Ti, Td PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td int main() { leftMotor.period(0.00005); //Set motor PWM periods to 20KHz. rightMotor.period(0.00005); //Input and output limits have been determined //empirically with the specific motors being used. //Change appropriately for different motors. //Input units: counts per second. //Output units: PwmOut duty cycle as %. //Default limits are for moving forward. leftPid.setInputLimits(0, 3000); leftPid.setOutputLimits(0.0, 0.9); leftPid.setMode(AUTO_MODE); rightPid.setInputLimits(0, 3200); rightPid.setOutputLimits(0.0, 0.9); rightPid.setMode(AUTO_MODE); int leftPulses = 0; //How far the left wheel has travelled. int leftPrevPulses = 0; //The previous reading of how far the left wheel //has travelled. float leftVelocity = 0.0; //The velocity of the left wheel in pulses per //second. int rightPulses = 0; //How far the right wheel has travelled. int rightPrevPulses = 0; //The previous reading of how far the right wheel //has travelled. float rightVelocity = 0.0; //The velocity of the right wheel in pulses per //second. int distance = 6000; //Number of pulses to travel. wait(5); //Wait a few seconds before we start moving. //Velocity to mantain in pulses per second. leftPid.setSetPoint(1000); rightPid.setSetPoint(975); while ((leftPulses < distance) || (rightPulses < distance)) { //Get the current pulse count and subtract the previous one to //calculate the current velocity in counts per second. leftPulses = leftQei.getPulses(); leftVelocity = (leftPulses - leftPrevPulses) / 0.01; leftPrevPulses = leftPulses; //Use the absolute value of velocity as the PID controller works //in the % (unsigned) domain and will get confused with -ve values. leftPid.setProcessValue(fabs(leftVelocity)); leftMotor.speed(leftPid.compute()); rightPulses = rightQei.getPulses(); rightVelocity = (rightPulses - rightPrevPulses) / 0.01; rightPrevPulses = rightPulses; rightPid.setProcessValue(fabs(rightVelocity)); rightMotor.speed(rightPid.compute()); wait(0.01); } leftMotor.brake(); rightMotor.brake(); }