From c1444ca79ad06ac3dae2edce7c9535bc8884802d Mon Sep 17 00:00:00 2001 From: notaroboticsenthusiast <91231142+notaroboticsenthusiast@users.noreply.github.com> Date: Sat, 28 Oct 2023 08:15:26 -0700 Subject: [PATCH] fixes forgot to commit --Gabe --- src/main/java/frc/robot/Constants.java | 79 +++++-------------- src/main/java/frc/robot/Robot.java | 20 ++--- src/main/java/frc/robot/RobotContainer.java | 6 -- .../java/frc/robot/positioning/AStar.java | 0 src/main/java/frc/robot/positioning/Node.java | 0 .../positioning/Team937TrajectoryUtils.java | 29 +++---- src/main/java/frc/robot/subsystems/Drive.java | 7 +- 7 files changed, 38 insertions(+), 103 deletions(-) delete mode 100644 src/main/java/frc/robot/positioning/AStar.java delete mode 100644 src/main/java/frc/robot/positioning/Node.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7c98ffb..037391a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -5,19 +5,13 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.positioning.Pose; -import frc.robot.positioning.Team937TrajectoryUtils; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.math.trajectory.Trajectory; -import frc.robot.positioning.Team937TrajectoryUtils; - - import frc.robot.positioning.Pose; +import frc.robot.positioning.Team937TrajectoryUtils; import java.util.ArrayList; import java.util.List; @@ -73,7 +67,7 @@ public static class Drive { new Translation2d(Units.feetToMeters(10 + (7 / 8)), Units.feetToMeters(8 + (5 / 8))); */ - public static final double OPEN_LOOP_RAMP_RATE = 1.0; + public static final double OPEN_LOOP_RAMP_RATE = 0.5; public static final double TRACK_WIDTH = Units.feetToMeters(20.5); @@ -210,9 +204,8 @@ public static class Arm { public static class Intake { public static final int ID_TALON_ARM_CLAW = 0; /* This is actually a Spark */ public static final double SPEED_ARM_CLAW = 1; - // TODO: TODO: TODO: this is prob wrong so this needs to be fixed - public static final double CUBE_IN_CONE_OUT_SPEED = 0.5; - public static final double CUBE_OUT_CONE_IN_SPEED = -0.5; + public static final double CUBE_IN_CONE_OUT_SPEED = 0.85; + public static final double CUBE_OUT_CONE_IN_SPEED = -0.85; } public static class ShoulderPID { @@ -265,12 +258,13 @@ public static class Limits { /** Holds constants for things not related to the robot */ public static class Game { public static class Field { - + /** * Half the length of the field relative to the origin. Positive for Max, Negative for Min. * Measured in cm. */ public static final int FIELD_X = 1654 / 2; + /** * Half the width of the field relative to the origin. Positive for Max, Negative for Min. * Measured in cm. @@ -358,25 +352,16 @@ public static class Cube { } } } - /** IDs for the controller buttons. */ - public static class ContollerButtons { - public static final int A_NUMBER = 1; - public static final int B_NUMBER = 2; - public static final int X_NUMBER = 3; - public static final int Y_NUMBER = 4; - public static final int LEFT_BUMPER_NUMBER = 5; - public static final int RIGHT_BUMPER_NUMBER = 6; - public static final int BACK_NUMBER = 7; - public static final int START_NUMBER = 8; - public static final int LEFT_STICK_NUMBER = 9; - public static final int RIGHT_STICK_NUMBER = 10; - } - + public static class Trajectories { - public static final Trajectory LOWER_COMMUNITY_TO_LOADING_ZONE = Team937TrajectoryUtils.generateTrajectory("/pathplanner/JSON/LC2LZ.wpilib.json"); - public static final Trajectory LOADING_ZONE_TO_LOWER_COMMUNITY = Team937TrajectoryUtils.generateTrajectory("/pathplanner/JSON/LZ2LC.wpilib.json"); - public static final Trajectory LOADING_ZONE_TO_UPPER_COMMUNITY = Team937TrajectoryUtils.generateTrajectory("/pathplanner/JSON/LZ2UC.wpilib.json"); - public static final Trajectory UPPER_COMMUNITY_TO_LOADING_ZONE = Team937TrajectoryUtils.generateTrajectory("/pathplanner/JSON/UC2LZ.wpilib.json"); + public static final Trajectory LOWER_COMMUNITY_TO_LOADING_ZONE = + Team937TrajectoryUtils.generateTrajectory("/pathplanner/JSON/LC2LZ.wpilib.json"); + public static final Trajectory LOADING_ZONE_TO_LOWER_COMMUNITY = + Team937TrajectoryUtils.generateTrajectory("/pathplanner/JSON/LZ2LC.wpilib.json"); + public static final Trajectory LOADING_ZONE_TO_UPPER_COMMUNITY = + Team937TrajectoryUtils.generateTrajectory("/pathplanner/JSON/LZ2UC.wpilib.json"); + public static final Trajectory UPPER_COMMUNITY_TO_LOADING_ZONE = + Team937TrajectoryUtils.generateTrajectory("/pathplanner/JSON/UC2LZ.wpilib.json"); } /** @@ -384,41 +369,15 @@ public static class Trajectories { * frc.robot.positioning.AStar}. */ public static class AStar { - /* + /* * Values are *2 as they are accessing half the length of the field * Values are /10 as they are in decimeters (/100 for centi and so on) */ - /** - * The full length of the field. - * Measured in decimeters. - */ + /** The full length of the field. Measured in decimeters. */ public static final int FIELD_X = Game.Field.FIELD_X * 2 / 10; - /** - * The full length of the field. - * Measured in decimeters. - */ - public static final int FIELD_Y = Game.Field.FIELD_Y * 2 / 10; - } - /** General constants for the drivetrain. Primarily used by {@link frc.robot.subsystems.Drive}. */ - public static class DriveConstants { - /* CAN IDs for the drivetrain motor controllers */ - public static final int ID_TALON_FRONT_LEFT = 0; - public static final int ID_TALON_FRONT_RIGHT = 1; - public static final int ID_TALON_REAR_LEFT = 2; - public static final int ID_TALON_REAR_RIGHT = 3; - } - - /** - * General constants for the AStar Subsystem. Primarily used by {@link - * frc.robot.positioning.AStar}. - */ - public static class AStar { - /** Half the length of the field. Positive for Max, Negative for Min. Measured in cm. */ - public static final int FIELD_X = 1654 / 2; - - /** Half the width of the field. Positive for Max, Negative for Min. Measured in cm. */ - public static final int FIELD_Y = 802 / 2; + /** The full length of the field. Measured in decimeters. */ + public static final int FIELD_Y = Game.Field.FIELD_Y * 2 / 10; } /** diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b87dab7..5dfe048 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,11 +8,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.positioning.ArmKinematics; -import frc.robot.positioning.Pose; -import frc.robot.positioning.AStarTrajectoryGenerator; -import frc.robot.positioning.Path; -import frc.robot.positioning.AStar; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -29,7 +24,7 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; - //private Path path = new Path(); + // private Path path = new Path(); /** * This function is run when the robot is first started up and should be used for any @@ -40,8 +35,8 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. - //AStar aStar = new AStar(1,1,10,10); - //this.path = aStar.generateAStarPath().get(); + // AStar aStar = new AStar(1,1,10,10); + // this.path = aStar.generateAStarPath().get(); m_robotContainer = new RobotContainer(); @@ -65,10 +60,11 @@ public void robotPeriodic() { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - //if (this.path!=null) { - //System.out.println("datPath " + AStarTrajectoryGenerator.generateTrajectory(path)); - //SmartDashboard.putString("Traj: ", AStarTrajectoryGenerator.generateTrajectory(path).toString()); - //} + // if (this.path!=null) { + // System.out.println("datPath " + AStarTrajectoryGenerator.generateTrajectory(path)); + // SmartDashboard.putString("Traj: ", + // AStarTrajectoryGenerator.generateTrajectory(path).toString()); + // } } /** This function is called once each time the robot enters Disabled mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index da984e2..30cfb8b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,12 +31,6 @@ import frc.robot.commands.TrackTrajectory; import frc.robot.commands.moveToPose.MoveToPose; import frc.robot.positioning.Pose; -import frc.robot.commands.ExampleCommand; -import frc.robot.commands.autotasks.ExampleAutoTask; -import frc.robot.subsystems.Drive; -import frc.robot.subsystems.ExampleSubsystem; -import frc.robot.subsystems.Limelight; -import frc.robot.subsystems.TaskScheduler; import frc.robot.subsystems.Camera; import frc.robot.subsystems.Drive; import frc.robot.subsystems.I2CManager; diff --git a/src/main/java/frc/robot/positioning/AStar.java b/src/main/java/frc/robot/positioning/AStar.java deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/robot/positioning/Node.java b/src/main/java/frc/robot/positioning/Node.java deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/robot/positioning/Team937TrajectoryUtils.java b/src/main/java/frc/robot/positioning/Team937TrajectoryUtils.java index d3bcd29..b896849 100644 --- a/src/main/java/frc/robot/positioning/Team937TrajectoryUtils.java +++ b/src/main/java/frc/robot/positioning/Team937TrajectoryUtils.java @@ -1,32 +1,22 @@ // 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 frc.robot.positioning; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.*; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.trajectory.*; import edu.wpi.first.math.trajectory.TrajectoryUtil; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; import frc.robot.Constants; -import frc.robot.Constants.Drive; -import frc.robot.Constants.Drive.HolonomicController; -import frc.robot.Constants.Drive.HolonomicController.ThetaController; -import frc.robot.Constants.Drive.HolonomicController.ThetaController.Constraints; - - - import java.util.ArrayList; -/** - * Static utility class for generating WPILib {@link Trajectory Trajectories} and more - */ +/** Static utility class for generating WPILib {@link Trajectory Trajectories} and more */ public class Team937TrajectoryUtils { /** @@ -65,18 +55,19 @@ public static Trajectory generateTrajectory( /** * Generates {@link Trajectory} out of Json File intended for pathweaver. + * * @param jsonFile - A filepath to a JSON file * @return A {@link Trajectory} */ public static Trajectory generateTrajectory(String jsonFile) { try { - return TrajectoryUtil.fromPathweaverJson(Filesystem.getDeployDirectory().toPath().resolve(jsonFile)); - } - catch (java.io.IOException e) { - DriverStation.reportError("Unable to open trajectory JSON file: " + jsonFile, e.getStackTrace()); + return TrajectoryUtil.fromPathweaverJson( + Filesystem.getDeployDirectory().toPath().resolve(jsonFile)); + } catch (java.io.IOException e) { + DriverStation.reportError( + "Unable to open trajectory JSON file: " + jsonFile, e.getStackTrace()); return null; } } - } diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index b830769..65d869d 100644 --- a/src/main/java/frc/robot/subsystems/Drive.java +++ b/src/main/java/frc/robot/subsystems/Drive.java @@ -8,12 +8,6 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.math.controller.HolonomicDriveController; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.estimator.MecanumDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.controller.RamseteController; import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -23,6 +17,7 @@ import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants;