marxbotmotors.cpp

/********************************************************************************
 *  FARSA Experiments Library                                                   *
 *  Copyright (C) 2007-2012                                                     *
 *  Gianluca Massera <emmegian@yahoo.it>                                        *
 *  Stefano Nolfi <stefano.nolfi@istc.cnr.it>                                   *
 *  Tomassino Ferrauto <tomassino.ferrauto@istc.cnr.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 "marxbotmotors.h"
#include "configurationhelper.h"
#include "randomgenerator.h"
#include "logger.h"

namespace farsa {

MarXbotMotor::MarXbotMotor(ConfigurationParameters& params, QString prefix) :
    Motor(params, prefix),
    m_marxbotResource("robot"),
    m_neuronsIteratorResource("neuronsIterator")
{
    // Reading parameters
    m_marxbotResource = ConfigurationHelper::getString(params, prefix + "marxbot", m_marxbotResource);
    m_neuronsIteratorResource = ConfigurationHelper::getString(params, prefix + "neuronsIterator", m_neuronsIteratorResource);

    // Declaring the resources that are needed here
    usableResources(QStringList() << m_marxbotResource << m_neuronsIteratorResource);
}

MarXbotMotor::~MarXbotMotor()
{
    // Nothing to do here
}

void MarXbotMotor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    Motor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "MarXbotMotor", this);
    params.createParameter(prefix, "marxbot", m_marxbotResource);
    params.createParameter(prefix, "neuronsIterator", m_neuronsIteratorResource);
}

void MarXbotMotor::describe(QString type)
{
    // Calling parent function
    Motor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The base class for MarXbot motors");
    d.describeString("marxbot").def("robot").help("The name of the resource associated with the MarXbot robot to use (default is \"robot\")");
    d.describeString("neuronsIterator").def("neuronsIterator").help("The name of the resource associated with the neural network iterator (default is \"neuronsIterator\")");
}

void MarXbotMotor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    Motor::resourceChanged(resourceName, changeType);

    // Here we only check whether the resource has been deleted and reset the check flag, the
    // actual work is done in subclasses
    if (changeType == Deleted) {
        resetNeededResourcesCheck();
        return;
    }
}

MarXbotWheelVelocityMotor::MarXbotWheelVelocityMotor(ConfigurationParameters& params, QString prefix) :
    MarXbotMotor(params, prefix),
    m_robot(NULL),
    m_neuronsIterator(NULL)
{
}

MarXbotWheelVelocityMotor::~MarXbotWheelVelocityMotor()
{
    // Nothing to do here
}

void MarXbotWheelVelocityMotor::save(ConfigurationParameters& params, QString prefix)
{
    // Calling parent function
    MarXbotMotor::save(params, prefix);

    // Saving parameters
    params.startObjectParameters(prefix, "MarXbotWheelVelocityMotor", this);
}

void MarXbotWheelVelocityMotor::describe(QString type)
{
    // Calling parent function
    MarXbotMotor::describe(type);

    // Describing our parameters
    Descriptor d = addTypeDescription(type, "The motor controlling the velocity of the wheels of the MarXbot robot");
}

void MarXbotWheelVelocityMotor::update()
{
    // Checking all resources we need exist
    checkAllNeededResourcesExist();

    // Acquiring the lock to get resources
    ResourcesLocker locker( this );

    // Getting limit velocities for wheels
    double minSpeed1;
    double minSpeed2;
    double maxSpeed1;
    double maxSpeed2;
    m_robot->wheelsController()->getSpeedLimits(minSpeed1, minSpeed2, maxSpeed1, maxSpeed2);

    // Computing desired wheel velocities
    m_neuronsIterator->setCurrentBlock(name());
    const double v1 = (maxSpeed1 - minSpeed1) * m_neuronsIterator->getOutput() + minSpeed1;
    m_neuronsIterator->nextNeuron();
    const double v2 = (maxSpeed2 - minSpeed2) * m_neuronsIterator->getOutput() + minSpeed2;

    m_robot->wheelsController()->setSpeeds(v1, v2);
}

int MarXbotWheelVelocityMotor::size()
{
    return 2;
}

void MarXbotWheelVelocityMotor::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    // Calling parent function
    MarXbotMotor::resourceChanged(resourceName, changeType);

    if (changeType == Deleted) {
        return;
    }

    if (resourceName == m_marxbotResource) {
        m_robot = getResource<PhyMarXbot>();
    } else if (resourceName == m_neuronsIteratorResource) {
        m_neuronsIterator = getResource<NeuronsIterator>();
        m_neuronsIterator->setCurrentBlock(name());
        for (int i = 0; i < size(); i++, m_neuronsIterator->nextNeuron()) {
            m_neuronsIterator->setGraphicProperties("W" + QString::number(i), 0.0, 1.0, Qt::red);
        }
    } else {
        Logger::info("Unknown resource " + resourceName + " for " + name());
    }
}

}