public void ResetTest()
{
Assert.AreEqual(0, filter.Average);
//Initial population
filter.Add(10);
filter.Add(5);
filter.Add(3);
filter.Add(4);
Assert.AreEqual(5.5, filter.Average);
//Check reset removes all values
filter.Reset();
Assert.AreEqual(0, filter.Average);
//Check we can still add elements
filter.Add(4);
Assert.AreEqual(1, filter.Average);
}
// Update is called once per frame
void Update()
{
if (!enabled)
{
return;
}
// in [-1, 1] & inverted
float steeringAngle = -player.GetAxis("SteeringAngle");
// in [0,1]
float left = player.GetAxis("Backward");
float right = player.GetAxis("Forward");
//#else
var joystick = InputManager.Instance.GetControllerJoystick(true);
float steeringAngle = joystick.x;
// keeping "left"=backward and "right"=forward variables from the pedals implementation
float left = (joystick.y > 0) ? 0 : Mathf.Abs(joystick.y);
float right = (joystick.y < 0) ? 0 : Mathf.Abs(joystick.y);
leftWindow.Add(left);
rightWindow.Add(right);
// only change direction if pedals are in both deadzones
if (Mathf.Max(left, right) <= Mathf.Max(forwardDeadzone, backwardDeadzone))
{
canChangeDir = true;
leftWindow.Reset();
rightWindow.Reset();
//Debug.Log("reset");
}
//Debug.Log(leftWindow.IsFull + ", " + rightWindow.IsFull + "," + leftWindow.Buffer.Count + ", " + leftWindow.FilterSize);
if (leftWindow.IsFull && rightWindow.IsFull)
{
if (canChangeDir)
{
// go back if both pedals are pressed in more than the backward deadzone
if (leftWindow.Max() >= backwardDeadzone)
{
goingForward = false;
canChangeDir = false;
}
// go forward if one pedal is pressed more than the forward deadzone
else if (rightWindow.Max() >= forwardDeadzone)
{
goingForward = true;
canChangeDir = false;
}
}
}
else
{
left = 0;
right = 0;
}
// moving average filter pedal values
leftFilter.Add(left);
rightFilter.Add(right);
left = leftFilter.GetFiltered();
right = rightFilter.GetFiltered();
// [-1, 1] -- non-linear -> [-1, 1]
float mappedAngularVelocity = Mathf.Sign(steeringAngle) * Mathf.Clamp01(angularVelocityMap.Evaluate(Mathf.Abs(steeringAngle)));
// [-1, 1] -- non-linear -> [-1, 1]
float mappedVelocity;
float vel = goingForward ? right : left;
if (goingForward)
{
vel = Mathf.Clamp01(vel - forwardDeadzone) / (1 - forwardDeadzone);
mappedVelocity = Mathf.Clamp01(velocityMap.Evaluate(Mathf.Abs(vel)));
}
else
{
vel = Mathf.Clamp01(vel - backwardDeadzone) / (1 - backwardDeadzone);
mappedVelocity = -0.5f * Mathf.Clamp01(velocityMap.Evaluate(Mathf.Abs(vel)));
}
Vector2 direction = Vector2.Lerp(leftDrive.normalized,
rightDrive.normalized,
0.5f + 0.5f * mappedAngularVelocity);
// publish read only values
velocity = mappedVelocity * maxVelocity;
angularVelocity = direction.magnitude * maxAngularVelocity;
_output = maxAngularVelocity * direction + velocity * forwardDrive.normalized;
driveControl.V_L = output.x;
driveControl.V_R = output.y;
}