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