/// <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; }