/* function to handle joystick control */
void Beluga::JoyStickControl(std::vector<double> js_axes,
unsigned int js_buttons)
{
double speed = 0;
double vert = 0;
double turn = 0;
double x = -js_axes[0];
double y = js_axes[1];
double w = 0;//js_axes[2];
double z = 0;//-js_axes[3];
speed = MT_DeadBandAndScale(y, m_dSpeedDeadBand, m_dMaxSpeed);
turn = MT_DeadBandAndScale(x, m_dTurnDeadBand, m_dMaxTurn);
#ifndef _WIN32
vert = MT_DeadBandAndScale(z, m_dSpeedDeadBand, m_dMaxVertSpeed);
#else
if(js_buttons & BELUGA_UP_BUTTON)
{
vert = m_dVertSpeed;
}
if(js_buttons & BELUGA_DOWN_BUTTON)
{
vert = -m_dVertSpeed;
}
#endif
SendCommand(speed, vert, turn);
m_vdControls[BELUGA_CONTROL_FWD_SPEED] = speed;
m_vdControls[BELUGA_CONTROL_STEERING] = turn;
m_vdControls[BELUGA_CONTROL_VERT_SPEED] = vert;
}
void MT_SteeredRobot::JoyStickControl(std::vector<double> js_axes,
unsigned int js_buttons)
{
double X = js_axes[0];
double Y = js_axes[1];
double speed = MT_DeadBandAndScale(Y, m_dSpeedDeadBand, m_dMaxSpeed);
double omega = MT_DeadBandAndScale(X,
m_dTurningRateDeadBand,
m_dMaxTurningRate);
SetSpeedOmega(speed, omega);
}
