// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
bool ChIrrGuiDriverSTR::OnEvent(const SEvent& event) {
// Only interpret keyboard inputs.
if (event.EventType != EET_KEY_INPUT_EVENT)
return false;
if (event.KeyInput.PressedDown) {
switch (event.KeyInput.Key) {
case KEY_KEY_T: // left post up
SetDisplacementLeft(m_displLeft + m_displDelta);
return true;
case KEY_KEY_G: // left post down
SetDisplacementLeft(m_displLeft - m_displDelta);
return true;
case KEY_KEY_Y: // right post up
SetDisplacementRight(m_displRight + m_displDelta);
return true;
case KEY_KEY_H: // right post down
SetDisplacementRight(m_displRight - m_displDelta);
return true;
case KEY_KEY_A:
SetSteering(m_steering - m_steeringDelta);
return true;
case KEY_KEY_D:
SetSteering(m_steering + m_steeringDelta);
return true;
default:
break;
}
}
return false;
}
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void CFourWheelVehiclePhysics::SteeringTurn( float carSpeed, const vehicleparams_t &vehicleData, bool bTurnLeft )
{
float flSteeringRate = STEERING_BASE_RATE;
if ( bTurnLeft )
{
// TODO: change the log function to an approx.
m_nTurnLeftCount = clamp( m_nTurnLeftCount, 2, 30 );
flSteeringRate *= log( (float) m_nTurnLeftCount );
flSteeringRate *= gpGlobals->frametime;
SetSteering( -1, flSteeringRate );
m_nTurnLeftCount++;
m_nTurnRightCount = 2;
}
else
{
// TODO: change the log function to an approx.
m_nTurnRightCount = clamp( m_nTurnRightCount, 2, 30 );
flSteeringRate *= log( (float) m_nTurnRightCount );
flSteeringRate *= gpGlobals->frametime;
SetSteering( 1, flSteeringRate );
m_nTurnLeftCount = 2;
m_nTurnRightCount++;
}
}
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void CFourWheelVehiclePhysics::SteeringTurnAnalog( float carSpeed, const vehicleparams_t &vehicleData, float sidemove )
{
// OLD Code
#if 0
float flSteeringRate = STEERING_BASE_RATE;
float factor = clamp( fabs( sidemove ) / STICK_EXTENTS, 0.0f, 1.0f );
factor *= 30;
flSteeringRate *= log( factor );
flSteeringRate *= gpGlobals->frametime;
SetSteering( sidemove < 0.0f ? -1 : 1, flSteeringRate );
#else
// This is tested with gamepads with analog sticks. It gives full analog control
// allowing the player to hold shallow turns.
float steering = sidemove / STICK_EXTENTS;
steering = clamp( steering, -1.0f, 1.0f );
SetSteering( steering, 0 );
#endif
// Neutralize
m_nTurnLeftCount = 2;
m_nTurnRightCount = 2;
}
void CustomVehicleController::Cleanup()
{
SetBrakes(NULL);
SetEngine(NULL);
SetSteering(NULL);
SetHandBrakes(NULL);
NewtonDestroyCollision(m_tireCastShape);
}
void DriveTrain::Steer(float radian, float speed, float a) {
A=a;
thetaRC = pi - radian; //convert steering angle to rear center wheel angle
if(thetaRC != pi / 2) //If we are not driving straight forward...
{
if(thetaRC < pi / 2) //Right Turn
{
RightTurn4Wheels();
}
else if(thetaRC > pi / 2) //Left Turn
{
LeftTurn4Wheels();
}
}
else //thetaRC = pi / 2
{
thetaFL = pi / 2;
thetaFR = pi / 2;
thetaRL = pi / 2;
thetaRR = pi / 2;
FLRatio = 1;
FRRatio = 1;
RLRatio = 1;
RRRatio = 1;
}
//Solve for fastest wheel speed
double speedarray[] = {fabs(FL), fabs(FR), fabs(RL), fabs(RR)};
int length = 4;
double maxspeed = speedarray[0];
for(int i = 1; i < length; i++)
{
if(speedarray[i] > maxspeed)
maxspeed = speedarray[i];
}
//Set ratios based on maximum wheel speed
FLRatio = FL/maxspeed;
FRRatio = FR/maxspeed;
RLRatio = RL/maxspeed;
RRRatio = RR/maxspeed;
//Set drive speeds
SetDriveSpeed(-FLRatio*speed, FRRatio*speed, -RLRatio*speed, RRRatio*speed);
//Set Steering PID Setpoints
float FLSetPoint = (1.25 + 2.5/pi*thetaFL);
float FRSetPoint = (1.25 + 2.5/pi*thetaFR);
float RLSetPoint = (1.25 + 2.5/pi*thetaRL);
float RRSetPoint = (1.25 + 2.5/pi*thetaRR);
SetSteering(FLSetPoint, FRSetPoint, RLSetPoint, RRSetPoint, true);
}
void CarSuspension::Init(const CarSuspensionInfo & info)
{
this->info = info;
btQuaternion toe(Direction::up, info.toe * M_PI / 180.0);
btQuaternion cam(Direction::forward, -info.camber * M_PI / 180.0);
orientation_ext = toe * cam;
steering_axis = Direction::up * cos(-info.caster * M_PI / 180.0) +
Direction::right * sin(-info.caster * M_PI / 180.0);
position = info.position;
SetSteering(0.0);
}
void DriveTrain::Crab(float twist, float y, float x, bool UseGyro) {
robotangle = (gyro->GetAngle())*pi/180;
float FWD = y;
float STR = x;
if(UseGyro)
{
FWD = y*cos(robotangle) + x*sin(robotangle);
STR = -y*sin(robotangle) + x*cos(robotangle);
}
radius = sqrt(pow(2*Y,2)+pow(X,2));
AP = STR - twist*X/radius;
BP = STR + twist*X/radius;
CP = FWD - twist*2*Y/radius;
DP = FWD + twist*2*Y/radius;
float FLSetPoint = 2.5;
float FRSetPoint = 2.5;
float RLSetPoint = 2.5;
float RRSetPoint = 2.5;
if(DP != 0 || BP != 0)
FLSetPoint = (2.5 + 2.5/pi*atan2(BP,DP));
if(BP != 0 || CP != 0)
FRSetPoint = (2.5 + 2.5/pi*atan2(BP,CP));
if(AP != 0 || DP != 0)
RLSetPoint = (2.5 + 2.5/pi*atan2(AP,DP));
if(AP != 0 || CP != 0)
RRSetPoint = (2.5 + 2.5/pi*atan2(AP,CP));
//SetSteerSetpoint(FLSetPoint, FRSetPoint, RLSetPoint, RRSetPoint, true);
SetSteering(FLSetPoint, FRSetPoint, RLSetPoint, RRSetPoint, true);
FL = sqrt(pow(BP,2)+pow(DP,2));
FR = sqrt(pow(BP,2)+pow(CP,2));
RL = sqrt(pow(AP,2)+pow(DP,2));
RR = sqrt(pow(AP,2)+pow(CP,2));
//Solve for fastest wheel speed
double speedarray[] = {fabs(FL), fabs(FR), fabs(RL), fabs(RR)};
int length = 4;
double maxspeed = speedarray[0];
for(int i = 1; i < length; i++)
{
if(speedarray[i] > maxspeed)
maxspeed = speedarray[i];
}
//Set ratios based on maximum wheel speed
if(maxspeed > 1 || maxspeed < -1)
{
FLRatio = FL/maxspeed;
FRRatio = FR/maxspeed;
RLRatio = RL/maxspeed;
RRRatio = RR/maxspeed;
}
else
{
FLRatio = FL;
FRRatio = FR;
RLRatio = RL;
RRRatio = RR;
}
//Set drive speeds
SetDriveSpeed(FLRatio, -FRRatio, RLRatio, -RRRatio);
}
void DriveTrain::Lock() //locks wheels to prevent robot movement
{
SetSteering(2.0, 0.75, 3.25, 4.5, true);
SetDriveSpeed(0,0,0,0);
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
bool ChIrrGuiDriver::OnEvent(const SEvent& event) {
if (m_mode == JOYSTICK) {
/// Only handles joystick events
if (event.EventType != EET_JOYSTICK_INPUT_EVENT)
return false;
/// Driver only handles input every 16 ticks (~60 Hz)
if (m_dT < 16) {
m_dT++;
return true;
}
m_dT = 0;
double th = event.JoystickEvent.Axis[SEvent::SJoystickEvent::AXIS_Z] + SHRT_MAX;
double br = event.JoystickEvent.Axis[SEvent::SJoystickEvent::AXIS_R] + SHRT_MAX;
double new_steering = (double)event.JoystickEvent.Axis[SEvent::SJoystickEvent::AXIS_X] / SHRT_MAX;
double new_throttle = (2 * SHRT_MAX - th) / (2 * SHRT_MAX);
double new_braking = (2 * SHRT_MAX - br) / (2 * SHRT_MAX);
if (m_steering != new_steering)
SetSteering(new_steering);
if (m_throttle != new_throttle)
SetThrottle(new_throttle);
if (m_braking != new_braking)
SetBraking(new_braking);
if (event.JoystickEvent.Axis[SEvent::SJoystickEvent::AXIS_Y] != SHRT_MAX) {
SetThrottle(0);
/// Gear is set to reverse
if (event.JoystickEvent.IsButtonPressed(22)) {
m_app.m_powertrain->SetDriveMode(ChPowertrain::REVERSE);
} else if (event.JoystickEvent.IsButtonPressed(12) || event.JoystickEvent.IsButtonPressed(13) ||
event.JoystickEvent.IsButtonPressed(14) || event.JoystickEvent.IsButtonPressed(15) ||
event.JoystickEvent.IsButtonPressed(16) || event.JoystickEvent.IsButtonPressed(17)) {
// All 'forward' gears set drive mode to forward, regardless of gear
m_app.m_powertrain->SetDriveMode(ChPowertrain::FORWARD);
} else {
m_app.m_powertrain->SetDriveMode(ChPowertrain::NEUTRAL);
}
}
return true;
}
/// Only interpret keyboard inputs.
if (event.EventType != EET_KEY_INPUT_EVENT)
return false;
if (event.KeyInput.PressedDown) {
switch (event.KeyInput.Key) {
case KEY_KEY_A:
if (m_mode == KEYBOARD)
SetSteering(m_steering - m_steeringDelta);
return true;
case KEY_KEY_D:
if (m_mode == KEYBOARD)
SetSteering(m_steering + m_steeringDelta);
return true;
case KEY_KEY_W:
if (m_mode == KEYBOARD) {
SetThrottle(m_throttle + m_throttleDelta);
if (m_throttle > 0)
SetBraking(m_braking - m_brakingDelta * 3.0);
}
return true;
case KEY_KEY_S:
if (m_mode == KEYBOARD) {
SetThrottle(m_throttle - m_throttleDelta * 3.0);
if (m_throttle <= 0)
SetBraking(m_braking + m_brakingDelta);
}
return true;
default:
break;
}
} else {
switch (event.KeyInput.Key) {
case KEY_KEY_L:
m_mode = LOCK;
return true;
case KEY_KEY_K:
m_throttle = 0;
m_steering = 0;
m_braking = 0;
m_mode = KEYBOARD;
return true;
case KEY_KEY_J:
if (m_data_driver) {
m_mode = DATAFILE;
m_time_shift = m_app.m_vehicle->GetSystem()->GetChTime();
}
return true;
case KEY_KEY_Z:
if (m_mode == KEYBOARD)
m_app.m_powertrain->SetDriveMode(ChPowertrain::FORWARD);
return true;
case KEY_KEY_X:
if (m_mode == KEYBOARD)
m_app.m_powertrain->SetDriveMode(ChPowertrain::NEUTRAL);
return true;
case KEY_KEY_C:
if (m_mode == KEYBOARD)
m_app.m_powertrain->SetDriveMode(ChPowertrain::REVERSE);
return true;
default:
break;
}
}
return false;
}
