worldsim/src/motorcontrollers.cpp

/********************************************************************************
 *  WorldSim -- library for robot simulations                                   *
 *  Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it>                *
 *                                                                              *
 *  This program is free software; you can redistribute it and/or modify        *
 *  it under the terms of the GNU General Public License as published by        *
 *  the Free Software Foundation; either version 2 of the License, or           *
 *  (at your option) any later version.                                         *
 *                                                                              *
 *  This program is distributed in the hope that it will be useful,             *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of              *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               *
 *  GNU General Public License for more details.                                *
 *                                                                              *
 *  You should have received a copy of the GNU General Public License           *
 *  along with this program; if not, write to the Free Software                 *
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA  *
 ********************************************************************************/

#include "worldsimconfig.h"
#include "motorcontrollers.h"
#include "phyjoint.h"
#include "world.h"

#ifdef __GNUC__
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif
    #include <yarp/os/Searchable.h>
#ifdef __GNUC__
#pragma GCC diagnostic warning "-Wunused-parameter"
#endif

#ifdef FARSA_USE_YARP_AND_ICUB
using namespace yarp::dev;
using namespace yarp::os;
#endif

namespace farsa {

wPID::wPID() {
    p_gain = 0;
    i_gain = 0;
    d_gain = 0;
    acc_ff = 0;
    fri_ff = 0;
    vel_ff = 0;
    bias = 0;
    accel_r = 0;
    setpt = 0;
    min = 0;
    max = 0;
    slew = 0;
    this_target = 0;
    next_target = 0;
    integral = 0;
    last_error = 0;
    last_output = 0;
}

double wPID::pidloop( double PV ) {
    double     this_error;
    double     this_output;
    double     accel;
    double     deriv;
    double     friction;
    /* the desired PV for this iteration is the value     */
    /* calculated as next_target during the last loop     */
    this_target = next_target;
    /* test for acceleration, compute new target PV for   */
    /* the next pass through the loop                     */
    if ( accel_r > 0 && this_target != setpt) {
        if ( this_target < setpt ) {
            next_target += accel_r;
            if ( next_target > setpt ) {
                next_target = setpt;
            }
        } else { /* this_target > setpoint */
            next_target -= accel_r;
            if ( next_target < setpt ) {
                next_target = setpt;
            }
        }
    } else {
        next_target = setpt;
    }
    /* acceleration is the difference between the PV      */
    /* target on this pass and the next pass through the  */
    /* loop                                               */
    accel = next_target - this_target;
    /* the error for the current pass is the difference   */
    /* between the current target and the current PV      */
    this_error = this_target - PV;
    /* the derivative is the difference between the error */
    /* for the current pass and the previous pass         */
    deriv = this_error - last_error;
    /* a very simple determination of whether there is    */
    /* special friction to be overcome on the next pass,  */
    /* if the current PV is 0 and the target for the next */
    /* pass is not 0, stiction could be a problem         */
    friction = ( PV == 0.0 && next_target != 0.0 );
    /* the new error is added to the integral             */
    integral += this_target - PV;
    /* now that all of the variable terms have been       */
    /* computed they can be multiplied by the appropriate */
    /* coefficients and the resulting products summed     */
    this_output = p_gain * this_error
                + i_gain * integral
                + d_gain * deriv
                + acc_ff * accel
                + vel_ff * next_target
                + fri_ff * friction
                + bias;
    last_error   = this_error;
    /* check for slew rate limiting on the output change  */
    if ( 0 != slew ) {
        if ( this_output - last_output > slew ) {
            this_output = last_output + slew;
        } else if ( last_output - this_output > slew ) {
            this_output = last_output - slew;
        }
    }
    /* now check the output value for absolute limits     */
    if ( this_output < min ) {
        this_output = min;
    } else if ( this_output > max ) {
        this_output = max;
    }
    /* store the new output value to be used as the old   */
    /* output value on the next loop pass                 */
    return last_output = this_output;
}

void wPID::reset() {
    // Resetting status variables
    this_target = 0;
    next_target = 0;
    integral = 0;
    last_error = 0;
    last_output = 0;
}

MotorController::MotorController( World* world ) {
    enabled = true;
    worldv = world;
}

MotorController::~MotorController() {
}


MultiMotorController::MultiMotorController( QVector<PhyDOF*> dofs, World* world ) :
    MotorController(world),
#ifdef FARSA_USE_YARP_AND_ICUB
    IPositionControl(),
    IVelocityControl(),
#endif
    motorsv(dofs)
{
    //--- common control variable initialization
    controlModes.fill( 1, motorsv.size() );
    refsacc.fill( 1.0, motorsv.size() );
    //--- IPositionControl variable initialization
    refs.fill( 0.0, motorsv.size() );
    refsvel.fill( 1.0, motorsv.size() );
    xfx0.fill( 0.0, motorsv.size() );
    x0.fill( 0.0, motorsv.size() );
    dx0.fill( 0.0, motorsv.size() );
    t.fill( 0.0, motorsv.size() );
    T.fill( 0.0, motorsv.size() );
    stops.fill( true, motorsv.size() );
    lastErrors.fill( 0.0, motorsv.size() );
    lastIs.fill( 0.0, motorsv.size() );
    lastTorques.fill( 0.0, motorsv.size() );
    // create PIDs with default values
    pos_pids.resize( motorsv.size() );
    for( int i=0; i<motorsv.size(); i++ ) {
        pos_pids[i] = new wPID();
        pos_pids[i]->p_gain = 0.8;
        pos_pids[i]->i_gain = 0.05;
        pos_pids[i]->d_gain = 0.0;
        pos_pids[i]->accel_r = 0;
        pos_pids[i]->min = -PI_GRECO/2.0;
        pos_pids[i]->max = +PI_GRECO/2.0;
    }
    //--- IVelocityControl variable initialization
    desiredVel.fill( 0.0, motorsv.size() );
    // create PIDs with default values
    vel_pids.resize( motorsv.size() );
    for( int i=0; i<motorsv.size(); i++ ) {
        vel_pids[i] = new wPID();
        vel_pids[i]->p_gain = 0.8;
        vel_pids[i]->i_gain = 0.1;
        vel_pids[i]->d_gain = 0.0;
        //vel_pids[i]->accel_r = 3;
        //vel_pids[i]->acc_ff = 0.3;
        vel_pids[i]->min = -toRad( 120 ); // rads / second
        vel_pids[i]->max = +toRad( 120 ); // rads / second
    }
    //--- IControlLimits variable initialization
    loLimits.resize( motorsv.size() );
    hiLimits.resize( motorsv.size() );
    for( int i=0; i<motorsv.size(); i++ ) {
        real fmin, fmax;
        motorsv[i]->limits( fmin, fmax );
        loLimits[i] = fmin;
        hiLimits[i] = fmax;
    }
}

MultiMotorController::~MultiMotorController() {
    for( int i=0; i<motorsv.size(); i++ ) {
        delete (vel_pids[i]);
        delete (pos_pids[i]);
    }
}

void MultiMotorController::update() {
    real accum, /*wishangle,*/ wishvel;
    for( int i=0; i<motorsv.size(); i++ ) {
        switch( controlModes[i] ) {
        case 1: /* Position Controller */
            if ( ! stops[i] ) {
                real tFracT = t[i] / T[i];
                real tFracT3= tFracT * tFracT * tFracT;
                real tFracT4= tFracT * tFracT3;
                real tFracT5= tFracT * tFracT4;

                //--- code from p5f code from iCub/src/controller4Dc/Code/trajectory.c
                //float xfx0 = _xf[jj] - _x0[jj];  --- already computed when positionMove has been called
                //float dx0  = _dx0[jj];           --- computed when positionMove has been called
                accum = dx0[i] * tFracT;
                accum = accum + tFracT3 * (10.0*xfx0[i] - 6.0*dx0[i]);
                accum = accum - tFracT4 * (15.0*xfx0[i] - 8.0*dx0[i]);
                accum = accum + tFracT5 * (6.0*xfx0[i] - 3.0*dx0[i]);
                //--- in p5f code are missing x0 because it's added inside step_trajectory
                accum += x0[i];
            } else {
                accum = refs[i];
            }
            //--- accum is the angle at which the joint must be at current time
            pos_pids[i]->setpt = accum;
//          wishangle = pos_pids[i]->pidloop( fromRawPosition(i) );
            motorsv[i]->setDesiredPosition( toRawPosition(accum, i) /*wishangle*/ );
            //--- advance t
            t[i] += world()->timeStep();
            //--- stop if necessary
            stops[i] = ( t[i] / T[i] > 1.0 );
        break;
        case 2: /* Velocity Controller */
            //--- the velocity at which the joint must be at current time
            vel_pids[i]->setpt = desiredVel[i];
            wishvel = vel_pids[i]->pidloop( motorsv[i]->velocity() );
            motorsv[i]->setDesiredVelocity( wishvel );
//          motorsv[i]->setDesiredVelocity(desiredVel[i]); // This disables the PID
            break;
        default:
            break;
        }
    }
    return;
}

#ifdef FARSA_USE_YARP_AND_ICUB

bool MultiMotorController::open( yarp::os::Searchable & ) {
    return true;
}

bool MultiMotorController::close( ) {
    return true;
}

bool MultiMotorController::configure( yarp::os::Searchable & ) {
    return true;
}

#endif

bool MultiMotorController::getAxes( int *ax ) {
    *ax = motorsv.size();
    return true;
}

bool MultiMotorController::setPositionMode() {
    //controlMode = 1;
    return true;
}

bool MultiMotorController::positionMove(int j, double ref) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[j]->translate() ) {
        refs[j] = ref;
    } else {
        refs[j] = toRad( ref );
    }
    if ( motorsv[j]->isLimited() ) {
        //--- check the limits
        //--- stay too near to the limits may create oscillation of the joint
        //--- about 0.5% of the range of the joint
        real offset = (fabs(hiLimits[j])-fabs(loLimits[j]))*0.005;
        refs[j] = ramp( loLimits[j]+offset, hiLimits[j]-offset, refs[j] );
    }
    //--- calculate xfx0, dx0, T
    x0[j] = fromRawPosition(j);
    xfx0[j] = refs[j] - x0[j];
    dx0[j] = motorsv[j]->velocity();
    T[j] = fabs( xfx0[j] ) / refsvel[j];
    //--- reset t to zero
    t[j] = 0.0;
    // if T[j] is less that 10 ms, then it simply stay there
    stops[j] = ( T[j] < 0.010 );
    controlModes[j] = 1;
    return true;
}

bool MultiMotorController::positionMove(const double *newrefs) {
    for( int i=0; i<refs.size(); i++ ) {
        if ( motorsv[i]->translate() ) {
            refs[i] = newrefs[i];
        } else {
            refs[i] = toRad( newrefs[i] );
        }
        if ( motorsv[i]->isLimited() ) {
            //--- check the limits
            //--- stay too near to the limits may create oscillation of the joint
            //--- about 0.5% of the range of the joint
            real offset = (fabs(hiLimits[i])-fabs(loLimits[i]))*0.005;
            refs[i] = ramp( loLimits[i]+offset, hiLimits[i]-offset, refs[i] );
        }
        //--- calculate xfx0, dx0, T
        x0[i] = fromRawPosition( i );
        xfx0[i] = refs[i] - x0[i];
        dx0[i] = motorsv[i]->velocity();
        T[i] = fabs( xfx0[i] ) / refsvel[i];
        //--- reset t to zero
        t[i] = 0.0;
        // if T[j] is less that 10 ms, then it simply stay there
        stops[i] = ( T[i] < 0.010 );
        controlModes[i] = 1;
    }
    return true;
}

bool MultiMotorController::relativeMove(int j, double delta) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[j]->translate() ) {
        refs[j] += delta;
    } else {
        refs[j] += toRad( delta );
    }
    if ( motorsv[j]->isLimited() ) {
        //--- check the limits
        //--- stay too near to the limits may create oscillation of the joint
        //--- about 0.5% of the range of the joint
        real offset = (fabs(hiLimits[j])-fabs(loLimits[j]))*0.005;
        refs[j] = ramp( loLimits[j]+offset, hiLimits[j]-offset, refs[j] );
    }
    //--- calculate xfx0, dx0, T
    x0[j] = fromRawPosition(j);
    xfx0[j] = refs[j] - x0[j];
    dx0[j] = motorsv[j]->velocity();
    T[j] = fabs( xfx0[j] ) / refsvel[j];
    //--- reset t to zero
    t[j] = 0.0;
    stops[j] = false;
    controlModes[j] = 1;
    return true;
}

bool MultiMotorController::relativeMove(const double *deltas) {
    for( int i=0; i<refs.size(); i++ ) {
        if ( motorsv[i]->translate() ) {
            refs[i] += deltas[i];
        } else {
            refs[i] += toRad( deltas[i] );
        }
        if ( motorsv[i]->isLimited() ) {
            //--- check the limits
            //--- stay too near to the limits may create oscillation of the joint
            //--- about 0.5% of the range of the joint
            real offset = (fabs(hiLimits[i])-fabs(loLimits[i]))*0.005;
            refs[i] = ramp( loLimits[i]+offset, hiLimits[i]-offset, refs[i] );
        }
        //--- calculate xfx0, dx0, T
        x0[i] = fromRawPosition( i );
        xfx0[i] = refs[i] - x0[i];
        dx0[i] = motorsv[i]->velocity();
        T[i] = fabs( xfx0[i] ) / refsvel[i];
        //--- reset t to zero
        t[i] = 0.0;
        stops[i] = false;
        controlModes[i] = 1;
    }
    return true;
}

bool MultiMotorController::checkMotionDone(int j, bool *flag) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    //--- I should check the actual position or just if stops[j] is true ??
    *flag = ( fabs(fromRawPosition(j) - refs[j]) < 0.01745329 );
    //*flag = stops[j];
    return true;
}

bool MultiMotorController::checkMotionDone(bool *flag) {
    bool ends = true;
    for( int i=0; i<refs.size(); i++ ) {
        //--- I should check the actual position or just if stops[j] is true ??
        ends &= ( fabs(fromRawPosition(i) - refs[i]) < 0.01745329 );
        //ends &= stops[i];
        if ( !ends ) break;
    }
    *flag = ends;
    return true;
}

bool MultiMotorController::setRefSpeed(int j, double sp) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[j]->translate() ) {
        refsvel[j] = sp;
    } else {
        refsvel[j] = toRad(sp);
    }
    motorsv[j]->setMaxVelocity( refsvel[j] );
    return true;
}

bool MultiMotorController::setRefSpeeds(const double *spds) {
    for( int i=0; i<refs.size(); i++ ) {
        if ( motorsv[i]->translate() ) {
            refsvel[i] = spds[i];
        } else {
            refsvel[i] = toRad(spds[i]);
        }
        motorsv[i]->setMaxVelocity( refsvel[i] );
    }
    return true;
}

bool MultiMotorController::setRefAcceleration(int j, double acc) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[j]->translate() ) {
        refsacc[j] = acc;
    } else {
        refsacc[j] = toRad(acc);
    }
    return true;
}

bool MultiMotorController::setRefAccelerations(const double *accs) {
    for( int i=0; i<refs.size(); i++ ) {
        if ( motorsv[i]->translate() ) {
            refsacc[i] = accs[i];
        } else {
            refsacc[i] = toRad(accs[i]);
        }
    }
    return true;
}

bool MultiMotorController::getRefSpeed(int j, double *ref) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[j]->translate() ) {
        *ref = refsvel[j];
    } else {
        *ref = toDegree(refsvel[j]);
    }
    return true;
}

bool MultiMotorController::getRefSpeeds(double *spds) {
    for( int i=0; i<refsvel.size(); i++ ) {
        if ( motorsv[i]->translate() ) {
            spds[i] = refsvel[i];
        } else {
            spds[i] = toDegree(refsvel[i]);
        }
    }
    return true;
}

bool MultiMotorController::getRefAcceleration(int j, double *acc) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[j]->translate() ) {
        *acc = refsacc[j];
    } else {
        *acc = toDegree(refsacc[j]);
    }
    return true;
}

bool MultiMotorController::getRefAccelerations(double *accs) {
    for( int i=0; i<refsvel.size(); i++ ) {
        if ( motorsv[i]->translate() ) {
            accs[i] = refsacc[i];
        } else {
            accs[i] = toDegree(refsacc[i]);
        }
    }
    return true;
}

bool MultiMotorController::stop(int j) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    refs[j] = fromRawPosition(j);
    stops[j] = true;
    desiredVel[j] = 0;
    motorsv[j]->setVelocity(0.0);
    controlModes[j] = 1;
    vel_pids[j]->reset();
    return true;
}

bool MultiMotorController::stop() {
    for( int i=0; i<motorsv.size(); i++ ) {
        refs[i] = fromRawPosition(i);
        stops[i] = true;
        desiredVel[i] = 0;
        motorsv[i]->setVelocity(0.0);
        controlModes[i] = 1;
        vel_pids[i]->reset();
    }
    return true;
}

bool MultiMotorController::setVelocityMode() {
    //controlMode = 2;
    return true;
}

bool MultiMotorController::velocityMove(int j, double sp) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    controlModes[j] = 2;
    desiredVel[j] = toRad(sp);
    return true;
}

bool MultiMotorController::velocityMove(const double *sp) {
    for( int i=0; i<desiredVel.size(); i++ ) {
        desiredVel[i] = toRad(sp[i]);
        controlModes[i] = 2;
    }
    return true;
}

bool MultiMotorController::setPositionMode( int j ) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    //--- set the position to actual position and stop the joint
    refs[j] = fromRawPosition(j);
    stops[j] = true;
    controlModes[j] = 1;
    return true;
}

bool MultiMotorController::setVelocityMode(int j ) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    //--- set the desired velocity to zero and stop the joint
    desiredVel[j] = 0;
    stops[j] = true;
    controlModes[j] = 2;
    return true;
}

bool MultiMotorController::setTorqueMode( int j ) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    //--- torque mode not implemented yet
    return false;
}

bool MultiMotorController::getControlMode( int j, int* mode ) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    *mode = controlModes[j];
    return true;
}

bool MultiMotorController::getControlModes( int* modes ) {
    for( int i=0; i<motorsv.size(); i++ ) {
        modes[i] = controlModes[i];
    }
    return true;
}

bool MultiMotorController::setOpenLoopMode( int ) {
    return false;
}

bool MultiMotorController::resetEncoder( int ) {
    return true;
}

bool MultiMotorController::resetEncoders() {
    return true;
}

bool MultiMotorController::setEncoder( int, double ) {
    return true;
}

bool MultiMotorController::setEncoders( const double* ) {
    return true;
}

bool MultiMotorController::getEncoder( int j, double *v ) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[j]->translate() ) {
        (*v) = fromRawPosition(j);
    } else {
        (*v) = toDegree( fromRawPosition(j) );
    }
    return true;
}

bool MultiMotorController::getEncoders( double *encs ) {
    for( int i=0; i<motorsv.size(); i++ ) {
        if ( motorsv[i]->translate() ) {
            encs[i] = fromRawPosition(i);
        } else {
            encs[i] = toDegree( fromRawPosition(i) );
        }
    }
    return true;
}

bool MultiMotorController::getEncoderSpeed( int j, double *sp ) {
    if ( j < 0 && j >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[j]->translate() ) {
        (*sp) = motorsv[j]->velocity();
    } else {
        (*sp) = toDegree(motorsv[j]->velocity());
    }
    return true;
}

bool MultiMotorController::getEncoderSpeeds( double *spds ) {
    for( int i=0; i<motorsv.size(); i++ ) {
        if ( motorsv[i]->translate() ) {
            spds[i] = motorsv[i]->velocity();
        } else {
            spds[i] = toDegree(motorsv[i]->velocity());
        }
    }
    return true;
}

bool MultiMotorController::getEncoderAcceleration( int, double * ) {
    return false;
}

bool MultiMotorController::getEncoderAccelerations( double * ) {
    return false;
}

bool MultiMotorController::setLimits( int axis, double min, double max ) {
    if ( axis < 0 && axis >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[axis]->translate() ) {
        loLimits[axis] = min;
        hiLimits[axis] = max;
    } else {
        loLimits[axis] = toRad( min );
        hiLimits[axis] = toRad( max );
    }
    return true;
}
    
bool MultiMotorController::getLimits( int axis, double *min, double *max ) {
    if ( axis < 0 && axis >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[axis]->translate() ) {
        *min = loLimits[axis];
        *max = hiLimits[axis];
    } else {
        *min = toDegree( loLimits[axis] );
        *max = toDegree( hiLimits[axis] );
    }
    return true;
}

bool MultiMotorController::setLimitsRaw( int axis, double min, double max ) {
    if ( axis < 0 && axis >= motorsv.size() ) {
        return false;
    }
    if ( motorsv[axis]->translate() ) {
        motorsv[axis]->setLimits( min, max );
    } else {
        motorsv[axis]->setLimits( toRad(min), toRad(max) );
    }
    return true;
}

bool MultiMotorController::getLimitsRaw( int axis, double *min, double *max ) {
    if ( axis < 0 && axis >= motorsv.size() ) {
        return false;
    }
    real fmin, fmax;
    motorsv[axis]->limits( fmin, fmax );
    if ( motorsv[axis]->translate() ) {
        *min = fmin;
        *max = fmax;
    } else {
        *min = toDegree( fmin );
        *max = toDegree( fmax );
    }
    return true;
}

bool MultiMotorController::setEnableLimitsRaw( int axis, bool enable ) {
    if ( axis < 0 && axis >= motorsv.size() ) {
        return false;
    }
    if ( enable ) {
        motorsv[axis]->enableLimits();
    } else {
        motorsv[axis]->disableLimits();
    }
    return true;
}

real MultiMotorController::fromRawPosition( int i ) {
    real rmin, rmax;
    motorsv[i]->limits( rmin, rmax );
    return linearMap( motorsv[i]->position(), rmin, rmax, loLimits[i], hiLimits[i] );
}

real MultiMotorController::toRawPosition( real norawpos, int i ) {
    real rmin, rmax;
    motorsv[i]->limits( rmin, rmax );
    return linearMap( norawpos, loLimits[i], hiLimits[i], rmin, rmax );
}

WheelMotorController::WheelMotorController( QVector<PhyDOF*> wheels, World* world ) :
    MotorController(world),
    motorsv(wheels) {
    desiredVel.fill( 0.0, motorsv.size() );
    minVel.fill( -1.0, motorsv.size() );
    maxVel.fill( 1.0, motorsv.size() );
}

WheelMotorController::~WheelMotorController() {
    /* nothing to do */
}

void WheelMotorController::update() {
    for( int i=0; i<motorsv.size(); i++ ) {
        motorsv[i]->setDesiredVelocity( desiredVel[i] );
    }
}

void WheelMotorController::setSpeeds( QVector<double> speeds ) {
    for( int i=0; i<qMin(speeds.size(), desiredVel.size()); i++ ) {
        if ( speeds[i] < minVel[i] ) {
            desiredVel[i] = minVel[i];
        } else if ( speeds[i] > maxVel[i] ) {
            desiredVel[i] = maxVel[i];
        } else {
            desiredVel[i] = speeds[i];
        }
    }
}

void WheelMotorController::setSpeeds( double sp1, double sp2 ) {
    if ( sp1 < minVel[0] ) {
        desiredVel[0] = minVel[0];
    } else if ( sp1 > maxVel[0] ) {
        desiredVel[0] = maxVel[0];
    } else {
        desiredVel[0] = sp1;
    }

    if ( sp2 < minVel[1] ) {
        desiredVel[1] = minVel[1];
    } else if ( sp2 > maxVel[0] ) {
        desiredVel[1] = maxVel[1];
    } else {
        desiredVel[1] = sp2;
    }
}

void WheelMotorController::getSpeeds( QVector<double>& speeds ) {
    speeds.clear();
    for( int i=0; i<motorsv.size(); i++ ) {
        speeds.append(motorsv[i]->velocity());
    }
}

void WheelMotorController::getSpeeds( double& sp1, double& sp2 ) {
    sp1 = motorsv[0]->velocity();
    sp2 = motorsv[1]->velocity();
}

void WheelMotorController::setSpeedLimits( QVector<double> minSpeeds, QVector<double> maxSpeeds ) {
    minVel = minSpeeds;
    maxVel = maxSpeeds;
}

void WheelMotorController::setSpeedLimits( double minSpeed1, double minSpeed2, double maxSpeed1, double maxSpeed2 ) {
    minVel[0] = minSpeed1;
    minVel[1] = minSpeed2;
    maxVel[0] = maxSpeed1;
    maxVel[1] = maxSpeed2;
}

void WheelMotorController::getSpeedLimits( QVector<double>& minSpeeds, QVector<double>& maxSpeeds ) {
    minSpeeds = minVel;
    maxSpeeds = maxVel;
}

void WheelMotorController::getSpeedLimits( double& minSpeed1, double& minSpeed2, double& maxSpeed1, double& maxSpeed2 ) {
    minSpeed1 = minVel[0];
    minSpeed2 = minVel[1];
    maxSpeed1 = maxVel[0];
    maxSpeed2 = maxVel[1];
}

} // end namespace farsa