public void getDriveState()
{
if (Energy + ((Energy / 100) * 35) < eData.Energy)
{
currentDriveState = DriveState.ESCAPE;
}
else if (Energy > eData.Energy)
{
currentDriveState = DriveState.RAM;
}
else
{
currentDriveState = DriveState.AVOID;
}
}
/// <summary>
/// Initialize and validate startup DriveState
/// </summary>
private void InitializeState()
{
if (_state == null)
_state = new DriveState();
if (_state.LeftWheel == null)
_state.LeftWheel = new WheelConfiguration();
if (_state.RightWheel == null)
_state.RightWheel = new WheelConfiguration();
if (_state.PollingFrequencyMs == 0)
_state.PollingFrequencyMs = Contract.DefaultPollingFrequencyMs;
if (_state.LeftWheel.WheelDiameter == 0.0)
_state.LeftWheel.WheelDiameter = 0.055;
if (_state.RightWheel.WheelDiameter == 0.0)
_state.RightWheel.WheelDiameter = 0.055;
// Always initialize the runtime statistics when we start.
_state.RuntimeStatistics = new RuntimeStatistics();
_targetEncoderReachedPort[LEFT] = new Port<bool>();
_targetEncoderReachedPort[RIGHT] = new Port<bool>();
_state.Connected = false;
_genericState.IsEnabled = true;
_internalPendingDriveOperation = pxdrive.DriveRequestOperation.NotSpecified;
}
/// <summary>
/// Calculate the optimal power and target encoder degrees.
/// </summary>
/// <param name="drive">SetDriveRequest</param>
/// <param name="currentState">DriveState</param>
/// <param name="leftMotorPower"></param>
/// <param name="rightMotorPower"></param>
/// <param name="leftTargetEncoderDegrees"></param>
/// <param name="rightTargetEncoderDegrees"></param>
private void CalculatePowerAndTargetDegrees(
SetDriveRequest drive,
DriveState currentState,
out int leftMotorPower,
out int rightMotorPower,
out long leftTargetEncoderDegrees,
out long rightTargetEncoderDegrees)
{
leftTargetEncoderDegrees = Math.Abs(drive.LeftStopAtRotationDegrees);
rightTargetEncoderDegrees = Math.Abs(drive.RightStopAtRotationDegrees);
leftMotorPower = motor.NxtMotor.CalculateMaxMotorPower(leftTargetEncoderDegrees, drive.LeftPower, 0.0, 0.0);
rightMotorPower = motor.NxtMotor.CalculateMaxMotorPower(rightTargetEncoderDegrees, drive.RightPower, 0.0, 0.0);
// Adjust for Reverse Polarity
if (currentState.LeftWheel.ReversePolarity)
leftMotorPower *= -1;
if (currentState.RightWheel.ReversePolarity)
rightMotorPower *= -1;
// Adjust encoder sign to match power.
if (Math.Sign(leftMotorPower) != Math.Sign(leftTargetEncoderDegrees))
leftTargetEncoderDegrees *= -1;
if (Math.Sign(rightMotorPower) != Math.Sign(rightTargetEncoderDegrees))
rightTargetEncoderDegrees *= -1;
}
