source: proiecte/SIMEO/Simeo/src/SimeoEngine/ActuatorProvider.cpp @ 167

#include <stdio.h>
#include "ActuatorProvider.h"


ActuatorProvider::ActuatorProvider(Channel *chan, btHingeConstraint *joint, int orient)
: XProvider(chan)
{
        m_joint = joint;
        m_orient = orient;
}

ActuatorProvider::~ActuatorProvider()
{
}

btHingeConstraint *ActuatorProvider::getConstraint()
{
        return m_joint;
}

int ActuatorProvider::getOrient()
{
        return m_orient;
}

void ActuatorProvider::updatePost()
{
        float angle;
        float speed = 200.0;
        float force = 1000.0;
        m_chan->read(&angle, MPI_FLOAT, 1);
        m_joint->enableAngularMotor(true, computeVelocity(m_joint->getHingeAngle(), btScalar(angle)) * speed, btScalar(force));
}

void ActuatorProvider::packData(channelData *data, int id)
{
        data->angle = m_joint->getHingeAngle();
        data->maxAngle = m_joint->getUpperLimit();
        data->minAngle = m_joint->getLowerLimit();
        data->orient = m_orient;
        data->type = ACTUATOR;
}

btScalar ActuatorProvider::computeVelocity(btScalar currentAngle, btScalar targetAngle) {

        btScalar deltaAngle = targetAngle - currentAngle;

        if(deltaAngle < 0)
                deltaAngle += 2 * M_PI;

        if(deltaAngle > M_PI)
                deltaAngle -= 2 * M_PI;

        return deltaAngle;
}