private static float GetDeltaTriggers(GamepadModel model)
{
var lTrigger = ScalingUtils.ByteToFloat(model.LeftTrigger);
var rTrigger = ScalingUtils.ByteToFloat(model.RightTrigger);
var deltaTriggers = rTrigger - lTrigger;
return(ScalingUtils.SymmetricalConstrain(deltaTriggers, 1.0f) * 800.0f); // -800:800 range
}
private static float GetRotateInSpotDirection(GamepadModel model)
{
if (model.IsLeftTriggerButtonPressed)
{
return(-200.0f);
}
return(200.0f);
}
/// <summary>
/// Produces acc & dir output:
/// Rover moving forward-backward: dir0, acc ranging -100:100
/// Rover moving & rotating: dir ranging -1:1, acc ranging -100:100
/// Rover rotating in spot to the left: acc -200, acc ranging -100:100
/// Rover rotating in spot to the right: acc 200, acc ranging -100:100
/// </summary>
/// <param name="model"></param>
/// <returns></returns>
protected override RoverProcessorResult DoMix(GamepadModel model)
{
if (model.IsLeftTriggerButtonPressed || model.IsRightTriggerButtonPressed)
{
return(Rotate(model));
}
return(ForwardReverse(model));
}
private static RoverProcessorResult Rotate(GamepadModel model)
{
var result = new RoverProcessorResult
{
Acceleration = GetDeltaTriggers(model),
Direction = GetRotateInSpotDirection(model)
};
return(result);
}
public virtual TResult Mix(GamepadModel model)
{
var filteringResult = model;
foreach (var strategy in FilteringStrategies)
{
filteringResult = strategy.Filter(filteringResult);
}
return(DoMix(filteringResult));
}
private (short, short) ProcessMixing(GamepadModel gamepadState, short tempFwd, short tempBwd, short tempLeft, short tempRight)
{
short mLeft = _config.Centervalue, mRight = _config.Centervalue;
/* We are not holding the right button:
* back and forwards
* left trigger -> move backwards both motors
* right trigger -> move forwards both motors
* left thumbstick left -> move by arc to the left
* left thumbstich right -> move tby arc to the right
*/
if (!gamepadState.IsRightPressed)
{
if (tempLeft < _config.Deadband && tempRight < _config.Deadband)
{
mLeft = (short)(tempFwd - tempBwd + _config.Centervalue);
mRight = mLeft;
}
if (tempRight > _config.Deadband)
{
mLeft = (short)(_config.Centervalue + tempFwd - tempBwd);
mRight = (short)(_config.Centervalue + tempRight + tempFwd - tempBwd);
}
if (tempLeft <= _config.Deadband)
{
return(mLeft, mRight);
}
mRight = (short)(_config.Centervalue + tempFwd - tempBwd);
mLeft = (short)(_config.Centervalue + tempLeft + tempFwd - tempBwd);
}
/* while holding right button:
* left thumbstick left -> rotate left
* left thumbstik right -> rotate right
*/
else //rotate around point
{
if (tempRight > _config.Deadband)
{
mLeft = (short)(_config.Centervalue - tempRight);
mRight = (short)(_config.Centervalue + tempRight);
}
if (tempLeft <= _config.Deadband)
{
return(mLeft, mRight);
}
mLeft = (short)(_config.Centervalue + tempLeft);
mRight = (short)(_config.Centervalue - tempLeft);
}
return(mLeft, mRight);
}
private void StateChanged(object sender, XboxControllerStateChangedEventArgs e)
{
_gamepadState = Map(e.CurrentInputState);
var args = new GamepadEventArgs {
Gamepad = _gamepadState
};
// Fire event
GamepadStateChanged?.Invoke(this, args);
ProcessButtonsChangeEvents(_gamepadState, _previousState);
_previousState = _gamepadState;
}
public ControlsModel Process(GamepadModel gamepadState)
{
var lr = gamepadState.LeftThumbstick.X; //-255-255 left-right
var leftTrigger = gamepadState.LeftTrigger; //0-255 back
var rightTrigger = gamepadState.RightTrigger; //0-255 forward
short motorLeft = _config.Centervalue, motorRight = _config.Centervalue;
short tempLeft = 0, tempRight = 0;
checked
{
//process reverses
ProcessLeftRightReverse(lr, ref tempLeft, ref tempRight);
ProcessBackForwardReverse(leftTrigger, rightTrigger, out var tempFwd, out var tempBwd);
//process expo curve
ProcessExponentialLookup(ref tempLeft, ref tempRight, ref tempFwd, ref tempBwd);
ExponentialLookupProcessed?.Invoke(this, tempFwd);
//process expo curve (inifinite impulse response filter)
ProcessLowPassFilter(ref tempLeft, ref tempRight, ref tempFwd, ref tempBwd);
//constrain inputs
tempLeft = Utils.ConstrainNonnegative(tempLeft, 255);
tempRight = Utils.ConstrainNonnegative(tempRight, 255);
tempFwd = Utils.ConstrainNonnegative(tempFwd, 255);
tempBwd = Utils.ConstrainNonnegative(tempBwd, 255);
(motorLeft, motorRight) = ProcessMixing(gamepadState, tempFwd, tempBwd, tempLeft, tempRight);
//clamp values
motorLeft = Utils.ConstrainNonnegative(motorLeft, 510);
motorRight = Utils.ConstrainNonnegative(motorRight, 510);
//to -255 - 255 range
motorLeft -= 255;
motorRight -= 255;
//to -100 - 100 range
motorLeft = (short)((float)motorLeft / 2.55f);
motorRight = (short)((float)motorRight / 2.55f);
// constrain to user defined boundary
motorLeft = Utils.ConstrainSymetrical(motorLeft, _config.VelocityBoundPercentage);
motorRight = Utils.ConstrainSymetrical(motorRight, _config.VelocityBoundPercentage);
}
return(new ControlsModel(motorLeft, motorRight));
}
private static float GetDirection(GamepadModel model)
{
const float deadZone = 0.2f;
var leftRightStick = ScalingUtils.ShortToFloat(model.LeftThumbStick.Horizontal);
if (leftRightStick <= deadZone && leftRightStick >= -deadZone)
{
leftRightStick = 0;
}
leftRightStick *= 0.1f; // -0.1:0.1 range
return(ScalingUtils.SymmetricalConstrain(leftRightStick, 0.1f));
}
public GamepadService(ISteeringConfig config, int controllerIndex, int updateFrequency)
{
if (updateFrequency <= 0)
{
throw new ArgumentException("Update frequency should be positive");
}
_config = config;
//divide by 128 to get -255 <=> 255 range on thumbstick
_rangeConverter = new RangeConverter(128f, 255);
_gamepadModel = new GamepadModel();
var controller = XboxController.RetrieveController(controllerIndex);
controller.StateChanged += StateChanged;
XboxController.UpdateFrequency = updateFrequency;
_lowPassFilterTimer = new Timer(10);
_lowPassFilterTimer.Elapsed += LowPassFilterTimerOnElapsed;
}
protected abstract TResult DoMix(GamepadModel model);
public GamepadModel Filter(GamepadModel input)
{
input.LeftTrigger = PerformLookup(input.LeftTrigger);
input.RightTrigger = PerformLookup(input.RightTrigger);
return(input);
}
private void ProcessButtonsChangeEvents(GamepadModel state, GamepadModel previousState)
{
if (previousState is null)
{
return;
}
if (previousState.IsAPressed != state.IsAPressed)
{
AChanged?.Invoke(this, state.IsAPressed);
}
if (previousState.IsBPressed != state.IsAPressed)
{
BChanged?.Invoke(this, state.IsBPressed);
}
if (previousState.IsXPressed != state.IsXPressed)
{
XChanged?.Invoke(this, state.IsXPressed);
}
if (previousState.IsYPressed != state.IsYPressed)
{
YChanged?.Invoke(this, state.IsYPressed);
}
if (previousState.IsBackPressed != state.IsBackPressed)
{
BackChanged?.Invoke(this, state.IsBackPressed);
}
if (previousState.IsStartPressed != state.IsStartPressed)
{
StartChanged?.Invoke(this, state.IsStartPressed);
}
if (previousState.DPad.IsDownPressed != state.DPad.IsDownPressed)
{
DPadDownChanged?.Invoke(this, state.DPad.IsDownPressed);
}
if (previousState.DPad.IsUpPressed != state.DPad.IsUpPressed)
{
DPadUpChanged?.Invoke(this, state.DPad.IsUpPressed);
}
if (previousState.DPad.IsLeftPressed != state.DPad.IsLeftPressed)
{
DPadLeftChanged?.Invoke(this, state.DPad.IsLeftPressed);
}
if (previousState.DPad.IsRightPressed != state.DPad.IsRightPressed)
{
DPadRightChanged?.Invoke(this, state.DPad.IsRightPressed);
}
if (previousState.IsLeftThumbStickPressed != state.IsLeftThumbStickPressed)
{
LeftThumbStickPressedChanged?.Invoke(this, state.IsLeftThumbStickPressed);
}
if (previousState.IsRightThumbStickPressed != state.IsRightThumbStickPressed)
{
RightThumbStickPressedChanged?.Invoke(this, state.IsRightThumbStickPressed);
}
}
public TResult Process(GamepadModel input)
{
return(_mixer.Mix(input));
}
protected override ManipulatorProcessorResult DoMix(GamepadModel model)
{
return(new ManipulatorProcessorResult());
}
