motors.cpp

00001 /**********************************************************
00002  * Motors.cpp
00003  *
00004  * This is a motor control library for the UK1122 L298N based motor controller.
00005  * Includes PID controller to control motors speeds
00006  *
00007  *        Author: Crispian Poon
00008  *         Email: pooncg@gmail.com
00009  *       Purpose: Eurobot 2012 - ICRS Imperial College London
00010  *          Date: 4th April 2012
00011  *       Version: v0.12
00012  **********************************************************/
00013 
00014 #include "mbed.h"
00015 #include "motors.h"
00016 #include "QEI.h"    //quadrature encoder library
00017 #include "globals.h"
00018 #include "TSH.h"
00019 
00020 //*************************************************************************************
00021 //  Constructor
00022 //*************************************************************************************
00023 Motors::Motors():
00024         Encoder1 (p30, p29, NC, 1856 ,QEI::X4_ENCODING),    //connects to motor1 quadracture encoders
00025         Encoder2 (p27, p28, NC, 1856 ,QEI::X4_ENCODING),    //connects to motor2 quadracture encoders
00026         Motor1A(p17), Motor1B(p18), Motor2A(p19), Motor2B(p13), //connects to direction pins
00027         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!!
00028         PIDControllerMotor1(2.25, 0.3, 0.00001, 0.010), PIDControllerMotor2(2.25, 0.3, 0.00001, 0.010) {
00029          //PIDControllerMotor1(3.5, 0.5, 0.0, 0.010), PIDControllerMotor2(3.5, 0.5, 0.0, 0.010){
00030 
00031 //Initialise PID controllers
00032     PIDControllerMotor1.setMode(MANUAL_MODE);
00033     PIDControllerMotor2.setMode(MANUAL_MODE);
00034     PIDControllerMotor1.setBias(0);
00035     PIDControllerMotor2.setBias(0);
00036     PIDControllerMotor1.setOutputLimits(-127, 127);
00037     PIDControllerMotor2.setOutputLimits(-127, 127);
00038     PIDControllerMotor1.setInputLimits(-102, 102);
00039     PIDControllerMotor2.setInputLimits(-102, 102);
00040     _lastEncoder1 = 0;
00041     _lastEncoder2 = 0;
00042     
00043     //speed regulator task using PID. Run every 10ms.
00044     _ticker.attach(this, &Motors::speedRegulatorTask, 0.01);
00045 
00046 };
00047 
00048 
00049 //*************************************************************************************
00050 //  Public functions
00051 //*************************************************************************************
00052 
00053 //*********************************************
00054 //
00055 //  @Description    speed regulator task using PID. Run every 10ms.
00056 //
00057 //*********************************************
00058 void Motors::speedRegulatorTask() {
00059 
00060     if (_enableSpeed == 1) {
00061 
00062     int latestMotor1Speed = 0;
00063     int latestMotor2Speed = 0;
00064     int computedSpeed1 = 0;
00065     int computedSpeed2 = 0;
00066 
00067     //acceleration control
00068     if (accelerationRegister == 1) {
00069         if (_accelerationSpeed1 != 0) {
00070 
00071             if (abs(_motorSpeed1) < abs(_accelerationSpeed1)) {
00072                 _motorSpeed1 += getSignOfInt(_accelerationSpeed1);
00073             } else if (abs(_motorSpeed1) > abs(_accelerationSpeed1)) {
00074                 _motorSpeed1 = _accelerationSpeed1;
00075             }
00076         }
00077         if (_accelerationSpeed2 != 0) {
00078             if (abs(_motorSpeed2) < abs(_accelerationSpeed2)) {
00079                 _motorSpeed2 += getSignOfInt(_accelerationSpeed2);
00080             } else if (abs(_motorSpeed2) > abs(_accelerationSpeed2)) {
00081                 _motorSpeed2 = _accelerationSpeed2;
00082             }
00083         }
00084     }
00085 
00086 
00087     //MOTOR 1 PID
00088     latestMotor1Speed = getEncoder1() - _lastEncoder1;  //motor1 encoder change
00089     //PID setpoints for 50ms interval.
00090     if (_motorSpeed1 == 0) {
00091         PIDControllerMotor1.setSetPoint(0);
00092     } else {
00093         PIDControllerMotor1.setSetPoint((int)(102*((float)_motorSpeed1/127)));
00094     }
00095     //Process value
00096     PIDControllerMotor1.setProcessValue(latestMotor1Speed);
00097     //PID Compute
00098     computedSpeed1 = (int)PIDControllerMotor1.compute();
00099 
00100 
00101 
00102     //MOTOR 2 PID
00103     latestMotor2Speed = getEncoder2() - _lastEncoder2; //motor2 encoder change
00104     //PID setpoints for 50ms interval.
00105     if (_motorSpeed2 == 0) {
00106         PIDControllerMotor2.setSetPoint(0);
00107     } else {
00108         PIDControllerMotor2.setSetPoint((int)(102*((float)_motorSpeed2/127)));
00109     }
00110     //Process value
00111     PIDControllerMotor2.setProcessValue(latestMotor2Speed);
00112     //PID Compute
00113     computedSpeed2 = (int)PIDControllerMotor2.compute();
00114 
00115 
00116 
00117     //debug variables
00118     _debug1 = latestMotor1Speed;
00119     _debug2 = computedSpeed1;
00120 
00121 
00122 
00123     //Set motors speed
00124     _setSpeed(computedSpeed1, computedSpeed2);
00125 
00126 }
00127 }
00128 
00129 //*********************************************
00130 //
00131 //  @Description    External set speed function for both motors
00132 //  @Parameter  [speed]   ranges from -127 (revse motor) to 127 (forward motor)
00133 //
00134 //*********************************************
00135 void Motors::setSpeed(int speed) {
00136     setSpeed(speed, speed);
00137 }
00138 
00139 //*********************************************
00140 //
00141 //  @Description    External set speed function. Relies on the speedRegulatorTask to change speed.
00142 //  @Parameters [speed1]  min -127 (reverse motor), max 127 (forward motor)
00143 //  @Parameters [speed2]  min -127 (reverse motor), max 127 (forward motor)
00144 //
00145 //*********************************************
00146 void Motors::setSpeed(int speed1, int speed2) {
00147     //set global motor values
00148     _motorSpeed1 = speed1;
00149     _motorSpeed2 = speed2;
00150     _lastEncoder1 = getEncoder1();
00151     _lastEncoder2 = getEncoder2();
00152     _enableSpeed = 1; 
00153     //acceleration control
00154     if (accelerationRegister == 1) {
00155         //target accelerated speed
00156         _accelerationSpeed1 = speed1;
00157         _accelerationSpeed2 = speed2;
00158 
00159         //current speed
00160         _motorSpeed1 = 0;
00161         _motorSpeed2 = 0;
00162     }
00163 }
00164 
00165 //*********************************************
00166 //
00167 //  @Description    stops motors
00168 //
00169 //*********************************************
00170 void  Motors::stop()
00171 {
00172     setSpeed(0);
00173 }
00174 
00175 //*********************************************
00176 //
00177 //  @Description    stops motors
00178 //
00179 //*********************************************
00180 void  Motors::coastStop()
00181 {
00182     setSpeed(0);
00183     _enableSpeed = 0;
00184     
00185 }
00186 
00187 
00188 //*********************************************
00189 //
00190 //  @Description    resets motor1 and motor encoders
00191 //
00192 //*********************************************
00193 void Motors::resetEncoders() {
00194     Encoder1.reset();
00195     Encoder2.reset();
00196 }
00197 
00198 //*********************************************
00199 //
00200 //  @Description    gets motor1 encoder
00201 //  @returns motor1 encoder counts
00202 //
00203 //*********************************************
00204 int Motors::getEncoder1() {
00205     return  Encoder1.getPulses();
00206 }
00207 
00208 //*********************************************
00209 //
00210 //  @Description    gets motor2 encoder
00211 //  @returns motor2 encoder counts
00212 //
00213 //*********************************************
00214 int Motors::getEncoder2() {
00215     return  Encoder2.getPulses();
00216 }
00217 
00218 //*********************************************
00219 //
00220 //  @Description   converts encoder counts to distance in mm
00221 //  @Parameters [encoder]   (int) encoder counts
00222 //  @returns distance in mm
00223 //
00224 //*********************************************
00225 int Motors::encoderToDistance(int encoder) {
00226     return int((float(encoder) / float(encoderRevCount)) * wheelmm);
00227 }
00228 
00229 //*********************************************
00230 //
00231 //  @Description   converts distance in mm to encoder counts
00232 //  @Parameters [distance]   (int) distance in mm
00233 //  @returns encoder counts
00234 //
00235 //*********************************************
00236 int Motors::distanceToEncoder(int distance) {
00237     return int((float(distance) / float(wheelmm)) * encoderRevCount);
00238 }
00239 
00240 //*********************************************
00241 //
00242 //  @Description   number sign indicator. determines if number is positive or negative.
00243 //  @Parameters [direction]   (int) a number
00244 //  @returns -1 if negative, 1 if positive
00245 //
00246 //*********************************************
00247 int Motors::getSignOfInt(int direction) {
00248 
00249     if (direction > 0) {
00250         return 1;
00251     } else if (direction < 0) {
00252         return -1;
00253     }
00254     return -1;
00255 }
00256 
00257 //*********************************************
00258 //Start of quarantined functions
00259 
00260 void Motors::move(int distance, int speed) {
00261     //resetEncoders(); TODO use kalman as feedback instead!
00262 
00263     int tempEndEncoder = 0;
00264     int startEncoderCount = 0;
00265 
00266     tempEndEncoder = distanceToEncoder(abs(distance));
00267     startEncoderCount = getEncoder1();
00268 
00269     setSpeed(getSignOfInt(distance) * speed);
00270 
00271     while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
00272         setSpeed(getSignOfInt(distance) * speed);
00273     }
00274 
00275     //resetEncoders();
00276     setSpeed(0);
00277 }
00278 
00279 void Motors::turn(int angle, int speed) {
00280     //resetEncoders(); TODO use kalman as feedback instead!
00281     int tempDistance = int((float(angle) / 360) * float(robotCircumference));
00282     int tempEndEncoder = 0;
00283     int startEncoderCount = 0;
00284 
00285     tempEndEncoder = distanceToEncoder(abs(tempDistance));
00286     startEncoderCount = getEncoder1();
00287     setSpeed(getSignOfInt(tempDistance) * speed, -getSignOfInt(tempDistance) * speed);
00288 
00289     while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
00290         setSpeed(getSignOfInt(tempDistance) * speed,-getSignOfInt(tempDistance) * speed);
00291 
00292     }
00293 
00294     //resetEncoders();
00295     setSpeed(0);
00296 }
00297 
00298 //Start of quarantined functions
00299 //*********************************************
00300 
00301 
00302 //*************************************************************************************
00303 //  Private functions
00304 //*************************************************************************************
00305 
00306 //*********************************************
00307 //
00308 //  @Description    internal set speed function
00309 //  @Parameters speed1  min -127, max 127
00310 //  @Parameters speed2  min -127, max 127
00311 //
00312 //*********************************************
00313 void Motors::_setSpeed(int speed1, int speed2) {
00314 
00315 //set global encoder values
00316     _lastEncoder1 = getEncoder1();
00317     _lastEncoder2 = getEncoder2();
00318 
00319 //Speed ranges from -127 to 127
00320     if (speed1 > 0) {
00321         //Motor1 forwards
00322         Motor1Enable = (float)speed1/127;
00323         Motor1A = 1;
00324         Motor1B = 0;
00325         //pwm the h bridge driver range 0 to 1 type float.
00326 
00327     } else if (speed1 <= 0) {
00328         //Motor1 backwards
00329         Motor1Enable = (float)abs(speed1)/127;
00330         Motor1A = 0;
00331         Motor1B = 1;
00332 
00333     } 
00334     /*
00335     else if (speed1 ==0) {
00336         _stop(1,0);
00337     }
00338     */
00339     if (speed2 > 0) {
00340         //Motor2 forwards
00341         Motor2Enable = (float)speed2/127;
00342 
00343         Motor2A = 1;
00344         Motor2B = 0;
00345 
00346     } else if (speed2 <= 0) {
00347         //Motor2 backwards
00348         Motor2Enable = (float)abs(speed2)/127;
00349         Motor2A = 0;
00350         Motor2B = 1;
00351     } 
00352     /*
00353     else if (speed2 == 0) {
00354         _stop(0,1);
00355     }
00356     */
00357 
00358 }
00359 
00360 
00361 //*********************************************
00362 //
00363 //  @Description    stop command for both motors
00364 //
00365 //*********************************************
00366 void Motors::_stop() {
00367     _stop(1,1);
00368 }
00369 
00370 //*********************************************
00371 //
00372 //  @Description    stop command for individual motors
00373 //  @Parameter  [motor1]  stops motor1. =1 is stop. =0 do nothing
00374 //  @Parameter  [motor2]  stops motor2. =1 is stop. =0 do nothing
00375 //
00376 //*********************************************
00377 void Motors::_stop(int motor1, int motor2) {
00378     if (motor1 == 1) {
00379         Motor1Enable = 1;
00380         Motor1A = 0;
00381         Motor1B = 0;
00382     }
00383 
00384     if (motor2 == 1) {
00385         Motor2Enable = 1;
00386         Motor2A = 0;
00387         Motor2B = 0;
00388     }
00389 
00390 }
00391 
00392 //*************************************************************************************
00393 //  Redundant functions
00394 //*************************************************************************************
00395 
00396 //Redudant
00397 void Motors::setMode(int mode) {
00398 }
00399 
00400 //Redudant
00401 void Motors::sendCommand(char command) {
00402 }
00403 
00404 //Redudant
00405 void Motors::sendCommand(char command1, char command2 ) {
00406 }