/// <summary>
/// Calculates intermediate PWM using old InnerMotorData and target InnerMotorData
/// </summary>
/// <param name="oldData"></param>
/// <param name="targetData"></param>
/// <returns></returns>
private InnerMotorData IntermediatePwmCalculator(InnerMotorData oldData, InnerMotorData targetData)
{
InnerMotorData steppedData = oldData;
int allowedPwmChange = Config.MaxPWMChangePerKnock;
bool isMotorDirectionChanged = targetData.IsCcw != oldData.IsCcw;
int pwmStepped;
// Distinguish cases; do we need to change direction
if (isMotorDirectionChanged)
{
// Let's go down to zero
pwmStepped = oldData.Speed - allowedPwmChange;
if (pwmStepped < 0)
{
// If we hit zero, set to zero and change polarity
steppedData.IsCcw = !steppedData.IsCcw;
pwmStepped = 0;
}
}
else
{
// Is our change positive or negative
bool pwmChangeNegative = targetData.Speed < oldData.Speed;
if (pwmChangeNegative)
{
// Let's step down
pwmStepped = oldData.Speed - allowedPwmChange;
// If we stepped over goal, set to goal
pwmStepped = pwmStepped < targetData.Speed ? targetData.Speed : pwmStepped;
}
else
{
// Let's step up
pwmStepped = oldData.Speed + allowedPwmChange;
// If we stepped over goal, set to goal
pwmStepped = pwmStepped > targetData.Speed ? targetData.Speed : pwmStepped;
}
}
steppedData.Speed = pwmStepped;
return(steppedData);
}
public MotorData(InnerMotorData LeftMotor, InnerMotorData RightMotor)
{
this.LeftMotor = LeftMotor;
this.RightMotor = RightMotor;
}
