An advanced robot that uses PID control on the motor speed, and an IMU for making accurate turns.
Dependencies: mbed Motor ITG3200 QEI ADXL345 IMUfilter PID
main.cpp
- Committer:
- aberk
- Date:
- 2010-09-10
- Revision:
- 0:7440a03255a7
File content as of revision 0:7440a03255a7:
/** * Includes. */ #include "Motor.h" #include "QEI.h" #include "PID.h" #include "IMU.h" /** * Defines. */ #define BUFFER_SIZE 1024 //Used for data logging arrays #define RATE 0.01 //PID loop timing //PID tuning constants. #define L_KC 0.4312 //Forward left motor Kc #define L_TI 0.1 //Forward left motor Ti #define L_TD 0.0 //Forward left motor Td #define R_KC 0.4620 //Forward right motor Kc #define R_TI 0.1 //Forward right motor Ti #define R_TD 0.0 //Forward right motor Td //PID input/output limits. #define L_INPUT_MIN 0 //Forward left motor minimum input #define L_INPUT_MAX 3000 //Forward left motor maximum input #define L_OUTPUT_MIN 0.0 //Forward left motor minimum output #define L_OUTPUT_MAX 0.9 //Forward left motor maximum output #define R_INPUT_MIN 0 //Forward right motor input minimum #define R_INPUT_MAX 3200 //Forward right motor input maximum #define R_OUTPUT_MIN 0.0 //Forward right motor output minimum #define R_OUTPUT_MAX 0.9 //Forward right motor output maximum //Physical dimensions. #define PULSES_PER_REV 624 #define WHEEL_DIAMETER 58.928 //mm #define ROTATION_DISTANCE 220.0 //mm #define REVS_PER_ROTATION (ROTATION_DISTANCE / WHEEL_DIAMETER) #define PULSES_PER_ROTATION (REVS_PER_ROTATION * PULSES_PER_REV) #define PULSES_PER_MM (PULSES_PER_REV / WHEEL_DIAMETER) #define DISTANCE_PER_PULSE (WHEEL_DIAMETER / PULSES_PER_REV) #define ENCODING 2 //Use X2 encoding #define WHEEL_DISTANCE (ROTATION_DISTANCE / DISTANCE_PER_PULSE) /** * Function Prototypes */ void initializeMotors(void); void initializePid(void); //cmd = "move", "turn" //direction = "forward", "backward", "left", "right" //length = distance in metres or angle in degrees void processCommand(char* cmd, char* direction, float length); /** * Globals */ //Debug. Serial pc(USBTX, USBRX); //Motor control. 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 PID leftPid(L_KC, L_TI, L_TD, RATE); //Kc, Ti, Td PID rightPid(R_KC, R_TI, R_TD, RATE); //Kc, Ti, Td //IMU and peripherals run at a different rate to the main PID loop. IMU imu(IMU_RATE, GYRO_MEAS_ERROR, ACCELEROMETER_RATE, GYROSCOPE_RATE); //Filesystem. LocalFileSystem local("local"); //Left motor working variables. int leftPulses = 0; int leftPrevPulses = 0; float leftVelocity = 0.0; float leftVelocityBuffer[BUFFER_SIZE]; //Right motor working variables. int rightPulses = 0; int rightPrevPulses = 0; float rightVelocity = 0.0; float rightVelocityBuffer[BUFFER_SIZE]; //General working variables. float heading = 0.0; float prevHeading = 0.0; float degreesTurned = 0.0; float positionSetPoint = 0.0; float headingSetPoint = 0.0; //Store the initial and end heading during a straight line section //in order to be able to correct turns. float startHeading = 0.0; float endHeading = 0.0; //Command Parser. char cmd0[16]; //{"move", "turn"} char cmd1[16]; //{{"forward", "backward"}, {"left", "right"}} float cmd2 = 0; //{distance in METRES, angle in DEGREES} void initializeMotors(void) { //Set motor PWM periods to 20KHz. leftMotor.period(0.00005); leftMotor.speed(0.0); rightMotor.period(0.00005); rightMotor.speed(0.0); } void initializePid(void) { //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(L_INPUT_MIN, L_INPUT_MAX); leftPid.setOutputLimits(L_OUTPUT_MIN, L_OUTPUT_MAX); leftPid.setMode(AUTO_MODE); rightPid.setInputLimits(R_INPUT_MIN, R_INPUT_MAX); rightPid.setOutputLimits(R_OUTPUT_MIN, R_OUTPUT_MAX); rightPid.setMode(AUTO_MODE); } void processCommand(char* cmd, char* direction, float length) { //move command. if (strcmp(cmd, "move") == 0) { int i = 0; //Data log array index. //The PID controller works in the % (unsigned) domain, so we'll //need to multiply the output by -1 if our motors need //to go "backwards". int leftDirection = 1; int rightDirection = 1; //Convert from metres into millimetres. length *= 1000; //Work out how many pulses are required to go that many millimetres. length *= PULSES_PER_MM; //Make sure we scale the number of pulses according to our encoding method. length /= ENCODING; positionSetPoint = length; if (strcmp(direction, "forward") == 0) { //Leave left and rightDirection as +ve. } else if (strcmp(cmd1, "backward") == 0) { //Change left and rightDirection to -ve. leftDirection = -1; rightDirection = -1; } startHeading = imu.getYaw(); //With left set point == right set point, the rover veers off to the //right - by slowing the right motor down slightly it goes relatively //straight. leftPid.setSetPoint(1000); rightPid.setSetPoint(975); //Keep going until we've moved the correct distance defined by the //position set point. Take the absolute value of the pulses as we might //be moving backwards. while ((abs(leftPulses) < positionSetPoint) && (abs(rightPulses) < positionSetPoint)) { //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) / RATE; 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() * leftDirection); rightPulses = rightQei.getPulses(); rightVelocity = (rightPulses - rightPrevPulses) / RATE; rightPrevPulses = rightPulses; rightPid.setProcessValue(fabs(rightVelocity)); rightMotor.speed(rightPid.compute() * rightDirection); i++; wait(RATE); } leftMotor.brake(); rightMotor.brake(); endHeading = imu.getYaw(); } //turn command. else if (strcmp(cmd0, "turn") == 0) { //The PID controller works in the % (unsigned) domain, so we'll //need to multiply the output by -1 for the motor which needs //to go "backwards". int leftDirection = 1; int rightDirection = 1; headingSetPoint = length + (endHeading - startHeading); //In case the rover tries to [pointlessly] turn >360 degrees. if (headingSetPoint > 359.8) { headingSetPoint -= 359.8; } //Set up the right previous heading for the initial degreesTurned //calculation. prevHeading = fabs(imu.getYaw()); if (strcmp(cmd1, "left") == 0) { //When turning left, the left motor needs to go backwards //while the right motor goes forwards. leftDirection = -1; } else if (strcmp(cmd1, "right") == 0) { //When turning right, the right motor needs to go backwards //while the left motor goes forwards. rightDirection = -1; } //Turn slowly to ensure we don't overshoot the angle by missing //the relatively slow readings [in comparison to the PID loop speed] //from the IMU. leftPid.setSetPoint(500); rightPid.setSetPoint(500); //Keep turning until we've moved through the correct angle as defined //by the heading set point. while (degreesTurned < headingSetPoint) { //Get the new heading and subtract the previous heading to //determine how many more degrees we've moved through. //As we're only interested in the relative angle (as opposed to //absolute) we'll take the absolute value of the heading to avoid //any nastiness when trying to calculate angles as they jump from //179.9 to -179.9 degrees. heading = fabs(imu.getYaw()); degreesTurned += fabs(heading - prevHeading); prevHeading = heading; //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) / RATE; 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() * leftDirection); rightPulses = rightQei.getPulses(); rightVelocity = (rightPulses - rightPrevPulses) / RATE; rightPrevPulses = rightPulses; rightPid.setProcessValue(fabs(rightVelocity)); rightMotor.speed(rightPid.compute() * rightDirection); wait(RATE); } leftMotor.brake(); rightMotor.brake(); } //Reset working variables. leftQei.reset(); rightQei.reset(); leftPulses = 0; leftPrevPulses = 0; leftVelocity = 0.0; leftMotor.speed(0.0); rightPulses = 0; rightPrevPulses = 0; rightVelocity = 0.0; rightMotor.speed(0.0); leftPid.setSetPoint(0.0); leftPid.setProcessValue(0.0); rightPid.setSetPoint(0.0); rightPid.setProcessValue(0.0); positionSetPoint = 0.0; headingSetPoint = 0.0; heading = 0.0; prevHeading = 0.0; degreesTurned = 0.0; } int main() { pc.printf("Starting mbed rover test...\n"); initializeMotors(); initializePid(); //Some delay before we start moving. wait(5); //Open the command script. FILE* commands = fopen("/local/commands.txt", "r"); //Check if we were successful in opening the command script. if (commands == NULL) { pc.printf("Could not open command file...\n"); exit(EXIT_FAILURE); } else { pc.printf("Succesfully opened command file!\n"); } //While there's another line to read from the command script. while (fscanf(commands, "%s%s%f", cmd0, cmd1, &cmd2) >= 0) { pc.printf("%s %s %f\n", cmd0, cmd1, cmd2); processCommand(cmd0, cmd1, cmd2); wait(1); } fclose(commands); }