File content as of revision 26:0995f61cb7b8:

/**********************************************************
 * 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 ) {
}