Shuto Naruse
Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Diff: motors/motors.cpp
- Revision:
- 10:294b9adbc9d3
- Parent:
- 2:cffa347bb943
- Child:
- 22:7ba09c0af0d0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/motors/motors.cpp Sat Apr 28 18:10:55 2012 +0000
@@ -0,0 +1,384 @@
+/**********************************************************
+ * 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