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