/// <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; }
private void GamePadAxisUpdated(UpdateAxes update) { LogInfo("Right x: " + (update.Body.Rx*.001).ToString(CultureInfo.InvariantCulture)); LogInfo("Right y: " + (update.Body.Ry * .001).ToString(CultureInfo.InvariantCulture)); LogInfo("Right z: " + (update.Body.Rz * .001).ToString(CultureInfo.InvariantCulture)); LogInfo(" Left x: " + (update.Body.X * .001).ToString(CultureInfo.InvariantCulture)); LogInfo(" Left y: " + (update.Body.Y*.001).ToString(CultureInfo.InvariantCulture)); LogInfo(" Left z: " + (update.Body.Z * .001).ToString(CultureInfo.InvariantCulture)); var req = new SetDriveRequest {LeftPower = (update.Body.Rx*.0005), RightPower = (update.Body.Y*-.0005)}; drivePort.DriveDistance(req); }