diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 07805f6..99507a5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -6,26 +6,14 @@ package frc4388.robot; import com.pathplanner.lib.PathPlanner; -import com.pathplanner.lib.PathPlannerTrajectory; -import com.pathplanner.lib.commands.PPSwerveControllerCommand; - -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.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -34,7 +22,6 @@ import frc4388.robot.Constants.*; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; -import frc4388.utility.controller.DeadbandedRawXboxController; import frc4388.utility.controller.DeadbandedXboxController; /** @@ -120,8 +107,6 @@ public class RobotContainer { public Command getAutonomousCommand() { // https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki - TrajectoryConfig config = new TrajectoryConfig(1.0, 1.0); - // PATH PLANNER TRAJECTORY IMPLEMENTATION // PathPlannerTrajectory testFirstPath = PathPlanner.loadPath("First Test Path", 1.0, 1.0); @@ -137,20 +122,20 @@ public class RobotContainer { // ); // WPILIB TRAJECTORY IMPLEMENTATION - Trajectory testFirstPath = PathPlanner.loadPath("First Test Path", 1.0, 1.0); + Trajectory testFirstPath = PathPlanner.loadPath("Move Forward", 1.0, 1.0); SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( - testFirstPath, + testFirstPath, m_robotSwerveDrive::getOdometry, m_robotSwerveDrive.m_kinematics, - new PIDController(0.5, 1.0, 1.0), - new PIDController(0.5, 1.0, 1.0), - new ProfiledPIDController(0.5, 1.0, 1.0, new TrapezoidProfile.Constraints(1.0, 1.0)), + new PIDController(0.0, 0.0, 0.0), + new PIDController(0.0, 0.0, 0.0), + new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(Math.PI, Math.PI)), m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive ); m_robotSwerveDrive.resetOdometry(testFirstPath.getInitialPose()); - return swerveControllerCommand.andThen(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, true)); + return swerveControllerCommand.andThen(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.0, true)); //return new InstantCommand(); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index e891d19..8880af7 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -4,9 +4,6 @@ package frc4388.robot.subsystems; -import com.ctre.phoenix.sensors.PigeonIMU; -import com.ctre.phoenix.sensors.PigeonIMUConfiguration; -import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; import com.ctre.phoenix.sensors.WPI_PigeonIMU; import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus; @@ -20,8 +17,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.OIConstants; diff --git a/src/main/java/frc4388/utility/DataLogging.java b/src/main/java/frc4388/utility/DataLogging.java new file mode 100644 index 0000000..730da4c --- /dev/null +++ b/src/main/java/frc4388/utility/DataLogging.java @@ -0,0 +1,12 @@ +// 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 frc4388.utility; + +/** Add your docs here. */ +public class DataLogging { + public DataLogging() { + + } +}