private JoystickOutput GetOutput_Recovery(HeliState s, long totalTicks, out ControlGoal control) { Actions |= Actions.Hover; Log("Crash imminent, trying to recover!"); control = new ControlGoal { HVelocity = 0, PitchAngle = 0, RollAngle = 0, HeadingAngle = 0 }; return new JoystickOutput { Throttle = 1.0f, Roll = Output.Roll(s.Degrees.RollAngle, control.RollAngle, totalTicks), Pitch = Output.Pitch(s.Degrees.PitchAngle, control.PitchAngle, totalTicks), Yaw = 0.0f }; }
public JoystickOutput GetHoverOutput(HeliState s, long totalTicks, out ControlGoal control) { Actions |= Actions.Hover; //Log("Hovering!"); control = new ControlGoal { HVelocity = 0, PitchAngle = 0, RollAngle = 0, HeadingAngle = 0 }; return new JoystickOutput { // TODO We need to store an initial s.Position.Y instead of using the most current one, so we need a "Hover" command that does this Throttle = Output.Throttle(s.Position.Y, s.Position.Y, totalTicks), Roll = Output.Roll(s.Degrees.RollAngle, control.RollAngle, totalTicks), Pitch = Output.Pitch(s.Degrees.PitchAngle, control.PitchAngle, totalTicks), Yaw = 0.0f }; }
/// <summary> /// /// </summary> /// <param name="userInput"></param> /// <param name="s"></param> /// <param name="totalTicks">TimeSpan ticks (100ns).</param> /// <param name="control"></param> /// <returns></returns> private JoystickOutput GetOutput_AssistedAutopilot(JoystickOutput userInput, HeliState s, long totalTicks, out ControlGoal control) { // Command the output controller based on the user input. // Joystick commands: // Forward/backward - Move forwards/backwards // Right/left - Move rightwards/leftwards // Throttle - Set the height above the ground to hold float heightAboveGroundToHold = MyMathHelper.Lerp(userInput.Throttle, 0, 1, 1, 10); float forwardsVelocity = MyMathHelper.Lerp(-userInput.Pitch, -1, 1, -10, 10); float rightwardsVelocity = MyMathHelper.Lerp(userInput.Roll, -1, 1, -10, 10); float headingDeg = s.Degrees.HeadingAngle - 15*userInput.Yaw; return Output.MoveRelatively(s, forwardsVelocity, rightwardsVelocity, headingDeg, heightAboveGroundToHold, totalTicks, out control); }
/// <summary> /// /// </summary> /// <param name="s"></param> /// <param name="totalTicks">TimeSpan ticks (100ns).</param> /// <param name="control"></param> /// <returns></returns> private JoystickOutput GetOutput_EnRoute(HeliState s, long totalTicks, out ControlGoal control) { return Output.MoveTowardsGoal(MaxHVelocity, out control, s, totalTicks, Task.HoldHeightAboveGround); }
/// <summary> /// /// </summary> /// <param name="userInput"></param> /// <param name="s"></param> /// <param name="ticks">TimeSpan ticks (100ns).</param> /// <param name="control"></param> /// <returns></returns> public JoystickOutput GetAssistedOutput(JoystickOutput userInput, HeliState s, long ticks, out ControlGoal control) { Navigation = NavigationState.AssistedAutopilot; JoystickOutput result = GetOutput_AssistedAutopilot(userInput, s, ticks, out control); ProcessNavigation(s, ticks); return result; }
/// <summary> /// /// </summary> /// <param name="s"></param> /// <param name="ticks">TimeSpan ticks (100ns).</param> /// <param name="control"></param> /// <returns></returns> public JoystickOutput GetOutput(HeliState s, long ticks, out ControlGoal control) { JoystickOutput result; // Re-fill the list of actions that are being performed in this input Actions = Actions.None; // float hDistanceToGoal = Vector2.Distance(VectorHelper.ToHorizontal(s.Position), // VectorHelper.ToHorizontal(CurrentWaypoint.Position)); // Simplified: Is the position in one second from now a crash? // If not, are we at the destination? // HeliState nextState = GetEstimatedState(s, TimeSpan.FromSeconds(1)); // Navigation = nextState.Position.Y < 0 // ? NavigationState.Recovery // : NavigationState.EnRoute; // TODO Insert some recovery code as well, commented out now because it triggers too often Navigation = NavigationState.EnRoute; // Navigation = (hDistanceToGoal < GoalDistanceTolerance) // ? NavigationState.AtDestination // : NavigationState.EnRoute; // if (CurrentWaypoint.Type != WaypointType.Start) //&& // if (CurrentWaypoint.Type == WaypointType.Land) // { // result = new JoystickInput(); // control = new ControlGoal(); // } // else { switch (Navigation) { case NavigationState.Recovery: Log("Nav: Recovery"); result = GetOutput_Recovery(s, ticks, out control); break; case NavigationState.EnRoute: Log("Nav: EnRoute"); result = GetOutput_EnRoute(s, ticks, out control); break; case NavigationState.AtDestination: Log("Nav: AtDestination"); result = GetHoverOutput(s, ticks, out control); break; default: throw new NotImplementedException("Should return before here."); } Log("Actions: " + ActionsToString(Actions)); Log(String.Format("Input: PRYT {0},{1},{2},{3}", (int) (result.Pitch*100), (int) (result.Roll*100), (int) (result.Yaw*100), (int) (result.Throttle*100))); } ProcessNavigation(s, ticks); return result; }