Shuto Naruse
Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Diff: motors/motors.cpp
- Revision:
- 9:377560539b74
- Parent:
- 8:ffc7d8af2d5a
- Child:
- 10:294b9adbc9d3
--- a/motors/motors.cpp Fri Apr 27 18:36:54 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,384 +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():
- 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;
-
- //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() {
-
- 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