From 1826557937adf9ef22b279fb3697f30f2a301508 Mon Sep 17 00:00:00 2001 From: DriverStationComputer Date: Sat, 8 Apr 2023 12:51:11 -0700 Subject: [PATCH] comp changes --- .../deploy/pathplanner/Mid Basic 5-1.path | 40 ++--------- src/main/deploy/pathplanner/Mid Basic.path | 8 +-- .../robotcode2023/RobotContainer.java | 9 ++- .../commands/DriveOverChargeStation.java | 70 +++++++++++++++++++ 4 files changed, 87 insertions(+), 40 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/robotcode2023/commands/DriveOverChargeStation.java diff --git a/src/main/deploy/pathplanner/Mid Basic 5-1.path b/src/main/deploy/pathplanner/Mid Basic 5-1.path index 96c95fa7..756a14b0 100644 --- a/src/main/deploy/pathplanner/Mid Basic 5-1.path +++ b/src/main/deploy/pathplanner/Mid Basic 5-1.path @@ -26,39 +26,12 @@ }, { "anchorPoint": { - "x": 2.826978549820152, - "y": 2.78 + "x": 3.673256635612798, + "y": 2.7733966368406673 }, "prevControl": { - "x": 2.320743920224466, - "y": 2.78 - }, - "nextControl": { - "x": 3.333213179415839, - "y": 2.78 - }, - "holonomicAngle": 180.0, - "isReversal": false, - "velOverride": null, - "isLocked": false, - "isStopPoint": true, - "stopEvent": { - "names": [ - "Cube Intake Pos." - ], - "executionBehavior": "parallel", - "waitBehavior": "none", - "waitTime": 0 - } - }, - { - "anchorPoint": { - "x": 6.394841434476022, - "y": 2.744509911273745 - }, - "prevControl": { - "x": 5.719761883274019, - "y": 2.744509911273745 + "x": 3.167022006017112, + "y": 2.7733966368406673 }, "nextControl": null, "holonomicAngle": 180.0, @@ -68,11 +41,12 @@ "isStopPoint": false, "stopEvent": { "names": [ - "Rotate 180" + "Cube Intake Pos.", + "DriveOverChargeStation" ], "executionBehavior": "parallel", "waitBehavior": "none", - "waitTime": 3.0 + "waitTime": 0 } } ], diff --git a/src/main/deploy/pathplanner/Mid Basic.path b/src/main/deploy/pathplanner/Mid Basic.path index d845b3ee..ad4d3ccd 100644 --- a/src/main/deploy/pathplanner/Mid Basic.path +++ b/src/main/deploy/pathplanner/Mid Basic.path @@ -53,12 +53,12 @@ }, { "anchorPoint": { - "x": 4.7481354877117745, - "y": 2.9274772386919947 + "x": 5.7628020469311085, + "y": 2.915865642157825 }, "prevControl": { - "x": 3.7481354877117745, - "y": 2.9274772386919947 + "x": 4.7628020469311085, + "y": 2.915865642157825 }, "nextControl": null, "holonomicAngle": 180.0, diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java b/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java index a20c464e..832efb21 100644 --- a/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java @@ -17,6 +17,7 @@ import org.carlmontrobotics.robotcode2023.Constants.Roller.RollerMode; import org.carlmontrobotics.robotcode2023.commands.AlignChargingStation; import org.carlmontrobotics.robotcode2023.commands.ArmTeleop; +import org.carlmontrobotics.robotcode2023.commands.DriveOverChargeStation; import org.carlmontrobotics.robotcode2023.commands.RotateToFieldRelativeAngle; import org.carlmontrobotics.robotcode2023.commands.RunRoller; import org.carlmontrobotics.robotcode2023.commands.SetArmWristGoalPreset; @@ -74,7 +75,7 @@ public RobotContainer() { // Command fakeArmCommand = new InstantCommand(() -> System.err.println("==============Store================="), arm); // eventMap.put("Stored Pos.", new SequentialCommandGroup(fakeArmCommand, new WaitCommand(2))); eventMap.put("Run Cube Intake", new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, () -> false, arm), new RunRoller(roller, RollerMode.INTAKE_CUBE))); - + eventMap.put("DriveOverChargeStation", new DriveOverChargeStation(drivetrain)); eventMap.put("Extend Arm High Cube", new SetArmWristGoalPreset(GoalPos.HIGH, () -> true, () -> false, arm)); eventMap.put("Extend Arm Mid Cube", new SetArmWristGoalPreset(GoalPos.MID, () -> true, () -> false, arm)); eventMap.put("Store Arm", new SetArmWristGoalPreset(GoalPos.STORED, () -> true, () -> false, arm)); @@ -116,8 +117,10 @@ public RobotContainer() { .getPathCommand(true, true).andThen( new PPRobotPath("Side Basic 2", drivetrain, false, eventMap).getPathCommand(false, true) ), - new PPRobotPath("Mid Basic 5-1", drivetrain, false, eventMap).getPathCommand(true, true).andThen( - new PPRobotPath("Mid Basic 5-2", drivetrain, false, eventMap).getPathCommand(false, true) + new SequentialCommandGroup( + new PPRobotPath("Mid Basic 5-1", drivetrain, false, eventMap).getPathCommand(true, true), + new ProxyCommand(new RotateToFieldRelativeAngle(new Rotation2d(Units.degreesToRadians(drivetrain.getHeadingDeg() + 180)), drivetrain)), + new PPRobotPath("Mid Basic 5-2", drivetrain, false, eventMap).getPathCommand(true, true) ) }; diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/commands/DriveOverChargeStation.java b/src/main/java/org/carlmontrobotics/robotcode2023/commands/DriveOverChargeStation.java new file mode 100644 index 00000000..facdbbf5 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/robotcode2023/commands/DriveOverChargeStation.java @@ -0,0 +1,70 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.robotcode2023.commands; + +import org.carlmontrobotics.robotcode2023.subsystems.Drivetrain; + +import static org.carlmontrobotics.robotcode2023.Constants.Drivetrain.*; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class DriveOverChargeStation extends CommandBase { + + private final Drivetrain drivetrain; + private boolean fwd, fieldOriented; + private boolean goingUp, goingDown, overChargeStation; + private int upSign; + + public DriveOverChargeStation(Drivetrain drivetrain) { + addRequirements(this.drivetrain = drivetrain); + } + + @Override + public void initialize() { + fwd = Math.abs(getRoll()) > Math.abs(getPitch()); + fieldOriented = drivetrain.getFieldOriented(); + drivetrain.setFieldOriented(false); + } + + @Override + public void execute() { + double roll = getRoll(); + double pitch = getPitch(); + double angle = fwd ? roll : pitch; + if (!goingUp) { + goingUp = Math.abs(angle) > 8; + upSign = angle >= 0 ? 1 : -1; + } + if (goingUp && !goingDown && upSign != 0) { + + goingDown = (upSign > 0 ? angle < 0 : angle > 0) && Math.abs(angle) > 8; + } + double forward = fwd ? drivetrain.getMaxSpeedMps() : 0; + double strafe = fwd ? 0 : drivetrain.getMaxSpeedMps(); + drivetrain.drive(forward, strafe, 0); + } + + @Override + public void end(boolean interrupted) { + drivetrain.stop(); + drivetrain.setFieldOriented(fieldOriented); + } + + @Override + public boolean isFinished() { + double roll = -getRoll(); + double pitch = getPitch(); + double angle = fwd ? roll : pitch; + return goingDown && Math.abs(angle) > 8; + } + + private double getPitch() { + return drivetrain.getPitch() - drivetrain.initPitch; + } + + private double getRoll() { + return drivetrain.getRoll() - drivetrain.initRoll; + } +}