Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Diff: motors.cpp
- Revision:
- 2:cffa347bb943
- Parent:
- 1:bbabbd997d21
- Child:
- 3:429829612cf9
--- a/motors.cpp Fri Apr 20 21:56:15 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,382 +0,0 @@ -/********************************************************** - * 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(): - current1(p15), current2(p16), - 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(3.5, 0.5, 0, 0.010), PIDControllerMotor2(3.5, 0.5, 0, 0.010) { - -//Initialise PID controllers - PIDControllerMotor1.setMode(MANUAL_MODE); - PIDControllerMotor2.setMode(MANUAL_MODE); - PIDControllerMotor1.setBias(-16); - PIDControllerMotor2.setBias(-16); - PIDControllerMotor1.setOutputLimits(-127, 127); - PIDControllerMotor2.setOutputLimits(-127, 127); - PIDControllerMotor1.setInputLimits(-102, 102); - PIDControllerMotor2.setInputLimits(-102, 102); - _lastEncoder1 = 0; - _lastEncoder2 = 0; - -}; - - -//************************************************************************************* -// Public functions -//************************************************************************************* - -//********************************************* -// -// @Description speed regulator task using PID. Run every 10ms. -// -//********************************************* -void Motors::speedRegulatorTask() { - - 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(); - - //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 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 ) { -} \ No newline at end of file