/*
* Sets speed variables for automatic adjustement
* DEPRECATED: Now it only calls setSpeedBase
*/
void setSpeed(float oV, float oW, float iV, float iW)
{ /*
AcquireMutex(access_speed);
objV = oV;
objW = oW;
incV = iV;
incW = iW;
ReleaseMutex(access_speed);
wait1Msec(1); // To force timeslice to end and give other threads time
*/
setSpeedBase(oV, oW);
}
/**
* Set odometer to a specified state
* @param _bState State to set odometer values
* @return true if command was sended successfully, else return false
*/
bool PlayerHandler::setOdometer(RoboCompDifferentialRobot::TBaseState _bState){
if(setSpeedBase(0.f,0.f))
{
mutex->lock();
// posIface->data->pose.pos.x=_bState.x;
// posIface->data->pose.pos.y=_bState.z;
// posIface->data->pose.yaw=_bState.alpha;
// posIface->data->velocity.pos.x=0.;
// posIface->data->velocity.yaw=0.;
pos->SetOdometry(_bState.x,_bState.z,_bState.alpha);
mutex->unlock();
return true;
}
else
return false;
}
/**
* Reset odometer
* @return true if command was sended successfully, else return false
*/
bool PlayerHandler::resetOdometer(){
if(setSpeedBase(0.f,0.f))
{
mutex->lock();
client->Read();
// alfa = posIface->data->pose.yaw;
alfa = pos->GetYaw();
r00 = sin(alfa);
r01 = -cos(alfa);
r10 = cos(alfa);
r11 = sin(alfa);
// t0 = posIface->data->pose.pos.y*1000.;
// t1 = posIface->data->pose.pos.x*1000.;
t0 = pos->GetYPos()*1000.;
t1 = pos->GetXPos()*1000.;
mutex->unlock();
return true;
}
else
return false;
}
/**
* Stops base
* @return true if sucess
*/
bool PlayerHandler::stopBase()
{
return setSpeedBase(0.f,0.f);
}
