Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
motors/motors.cpp
- Committer:
- narshu
- Date:
- 2012-05-04
- Revision:
- 23:1901cb6d0d95
- Parent:
- 22:7ba09c0af0d0
File content as of revision 23:1901cb6d0d95:
/********************************************************** * Motors.cpp * * This is a motor control library for the UK1122 L298N based motor controller. * Includes PID controller to control motors speeds * * Author: Crispian Poon * Email: pooncg@gmail.com * Purpose: Eurobot 2012 - ICRS Imperial College London * Date: 4th April 2012 * Version: v0.12 **********************************************************/ #include "mbed.h" #include "motors.h" #include "QEI.h" //quadrature encoder library #include "globals.h" #include "TSH.h" //************************************************************************************* // Constructor //************************************************************************************* Motors::Motors(): Encoder1 (p30, p29, NC, 1856 ,QEI::X4_ENCODING), //connects to motor1 quadracture encoders Encoder2 (p27, p28, NC, 1856 ,QEI::X4_ENCODING), //connects to motor2 quadracture encoders Motor1A(p17), Motor1B(p18), Motor2A(p19), Motor2B(p13), //connects to direction pins Motor1Enable(p25), Motor2Enable(p26), //Connects to control board enable pins to control motors speeds. PWM pins. Remember enable must be set before the direction pins changed!! PIDControllerMotor1(2.25, 0.3, 0.00001, 0.010), PIDControllerMotor2(2.25, 0.3, 0.00001, 0.010) { //PIDControllerMotor1(3.5, 0.5, 0.0, 0.010), PIDControllerMotor2(3.5, 0.5, 0.0, 0.010){ //Initialise PID controllers PIDControllerMotor1.setMode(MANUAL_MODE); PIDControllerMotor2.setMode(MANUAL_MODE); PIDControllerMotor1.setBias(0); PIDControllerMotor2.setBias(0); PIDControllerMotor1.setOutputLimits(-127, 127); PIDControllerMotor2.setOutputLimits(-127, 127); PIDControllerMotor1.setInputLimits(-102, 102); PIDControllerMotor2.setInputLimits(-102, 102); _lastEncoder1 = 0; _lastEncoder2 = 0; //speed regulator task using PID. Run every 10ms. _ticker.attach(this, &Motors::speedRegulatorTask, 0.01); }; //************************************************************************************* // Public functions //************************************************************************************* //********************************************* // // @Description speed regulator task using PID. Run every 10ms. // //********************************************* void Motors::speedRegulatorTask() { if (_enableSpeed == 1) { int latestMotor1Speed = 0; int latestMotor2Speed = 0; int computedSpeed1 = 0; int computedSpeed2 = 0; //acceleration control if (accelerationRegister == 1) { if (_accelerationSpeed1 != 0) { if (abs(_motorSpeed1) < abs(_accelerationSpeed1)) { _motorSpeed1 += getSignOfInt(_accelerationSpeed1); } else if (abs(_motorSpeed1) > abs(_accelerationSpeed1)) { _motorSpeed1 = _accelerationSpeed1; } } if (_accelerationSpeed2 != 0) { if (abs(_motorSpeed2) < abs(_accelerationSpeed2)) { _motorSpeed2 += getSignOfInt(_accelerationSpeed2); } else if (abs(_motorSpeed2) > abs(_accelerationSpeed2)) { _motorSpeed2 = _accelerationSpeed2; } } } //MOTOR 1 PID latestMotor1Speed = getEncoder1() - _lastEncoder1; //motor1 encoder change //PID setpoints for 50ms interval. if (_motorSpeed1 == 0) { PIDControllerMotor1.setSetPoint(0); } else { PIDControllerMotor1.setSetPoint((int)(102*((float)_motorSpeed1/127))); } //Process value PIDControllerMotor1.setProcessValue(latestMotor1Speed); //PID Compute computedSpeed1 = (int)PIDControllerMotor1.compute(); //MOTOR 2 PID latestMotor2Speed = getEncoder2() - _lastEncoder2; //motor2 encoder change //PID setpoints for 50ms interval. if (_motorSpeed2 == 0) { PIDControllerMotor2.setSetPoint(0); } else { PIDControllerMotor2.setSetPoint((int)(102*((float)_motorSpeed2/127))); } //Process value PIDControllerMotor2.setProcessValue(latestMotor2Speed); //PID Compute computedSpeed2 = (int)PIDControllerMotor2.compute(); //debug variables _debug1 = latestMotor1Speed; _debug2 = computedSpeed1; //Set motors speed _setSpeed(computedSpeed1, computedSpeed2); } } //********************************************* // // @Description External set speed function for both motors // @Parameter [speed] ranges from -127 (revse motor) to 127 (forward motor) // //********************************************* void Motors::setSpeed(int speed) { setSpeed(speed, speed); } //********************************************* // // @Description External set speed function. Relies on the speedRegulatorTask to change speed. // @Parameters [speed1] min -127 (reverse motor), max 127 (forward motor) // @Parameters [speed2] min -127 (reverse motor), max 127 (forward motor) // //********************************************* void Motors::setSpeed(int speed1, int speed2) { //set global motor values _motorSpeed1 = speed1; _motorSpeed2 = speed2; _lastEncoder1 = getEncoder1(); _lastEncoder2 = getEncoder2(); _enableSpeed = 1; //acceleration control if (accelerationRegister == 1) { //target accelerated speed _accelerationSpeed1 = speed1; _accelerationSpeed2 = speed2; //current speed _motorSpeed1 = 0; _motorSpeed2 = 0; } } //********************************************* // // @Description stops motors // //********************************************* void Motors::stop() { setSpeed(0); } //********************************************* // // @Description stops motors // //********************************************* void Motors::coastStop() { setSpeed(0); _enableSpeed = 0; } //********************************************* // // @Description resets motor1 and motor encoders // //********************************************* void Motors::resetEncoders() { Encoder1.reset(); Encoder2.reset(); } //********************************************* // // @Description gets motor1 encoder // @returns motor1 encoder counts // //********************************************* int Motors::getEncoder1() { return Encoder1.getPulses(); } //********************************************* // // @Description gets motor2 encoder // @returns motor2 encoder counts // //********************************************* int Motors::getEncoder2() { return Encoder2.getPulses(); } //********************************************* // // @Description converts encoder counts to distance in mm // @Parameters [encoder] (int) encoder counts // @returns distance in mm // //********************************************* int Motors::encoderToDistance(int encoder) { return int((float(encoder) / float(encoderRevCount)) * wheelmm); } //********************************************* // // @Description converts distance in mm to encoder counts // @Parameters [distance] (int) distance in mm // @returns encoder counts // //********************************************* int Motors::distanceToEncoder(int distance) { return int((float(distance) / float(wheelmm)) * encoderRevCount); } //********************************************* // // @Description number sign indicator. determines if number is positive or negative. // @Parameters [direction] (int) a number // @returns -1 if negative, 1 if positive // //********************************************* int Motors::getSignOfInt(int direction) { if (direction > 0) { return 1; } else if (direction < 0) { return -1; } return -1; } //********************************************* //Start of quarantined functions void Motors::move(int distance, int speed) { //resetEncoders(); TODO use kalman as feedback instead! int tempEndEncoder = 0; int startEncoderCount = 0; tempEndEncoder = distanceToEncoder(abs(distance)); startEncoderCount = getEncoder1(); setSpeed(getSignOfInt(distance) * speed); while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) { setSpeed(getSignOfInt(distance) * speed); } //resetEncoders(); setSpeed(0); } void Motors::turn(int angle, int speed) { //resetEncoders(); TODO use kalman as feedback instead! int tempDistance = int((float(angle) / 360) * float(robotCircumference)); int tempEndEncoder = 0; int startEncoderCount = 0; tempEndEncoder = distanceToEncoder(abs(tempDistance)); startEncoderCount = getEncoder1(); setSpeed(getSignOfInt(tempDistance) * speed, -getSignOfInt(tempDistance) * speed); while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) { setSpeed(getSignOfInt(tempDistance) * speed,-getSignOfInt(tempDistance) * speed); } //resetEncoders(); setSpeed(0); } //Start of quarantined functions //********************************************* //************************************************************************************* // Private functions //************************************************************************************* //********************************************* // // @Description internal set speed function // @Parameters speed1 min -127, max 127 // @Parameters speed2 min -127, max 127 // //********************************************* void Motors::_setSpeed(int speed1, int speed2) { //set global encoder values _lastEncoder1 = getEncoder1(); _lastEncoder2 = getEncoder2(); //Speed ranges from -127 to 127 if (speed1 > 0) { //Motor1 forwards Motor1Enable = (float)speed1/127; Motor1A = 1; Motor1B = 0; //pwm the h bridge driver range 0 to 1 type float. } else if (speed1 <= 0) { //Motor1 backwards Motor1Enable = (float)abs(speed1)/127; Motor1A = 0; Motor1B = 1; } /* else if (speed1 ==0) { _stop(1,0); } */ if (speed2 > 0) { //Motor2 forwards Motor2Enable = (float)speed2/127; Motor2A = 1; Motor2B = 0; } else if (speed2 <= 0) { //Motor2 backwards Motor2Enable = (float)abs(speed2)/127; Motor2A = 0; Motor2B = 1; } /* else if (speed2 == 0) { _stop(0,1); } */ } //********************************************* // // @Description stop command for both motors // //********************************************* void Motors::_stop() { _stop(1,1); } //********************************************* // // @Description stop command for individual motors // @Parameter [motor1] stops motor1. =1 is stop. =0 do nothing // @Parameter [motor2] stops motor2. =1 is stop. =0 do nothing // //********************************************* void Motors::_stop(int motor1, int motor2) { if (motor1 == 1) { Motor1Enable = 1; Motor1A = 0; Motor1B = 0; } if (motor2 == 1) { Motor2Enable = 1; Motor2A = 0; Motor2B = 0; } } //************************************************************************************* // Redundant functions //************************************************************************************* //Redudant void Motors::setMode(int mode) { } //Redudant void Motors::sendCommand(char command) { } //Redudant void Motors::sendCommand(char command1, char command2 ) { }