diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index e9f3df6..939e5c2 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -16,7 +16,6 @@ import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.XboxController.Axis; import edu.wpi.first.wpilibj.XboxController.Button; -import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.math.util.Units; import static org.carlmontrobotics.Config.CONFIG; @@ -273,8 +272,8 @@ public static final class IntakeC { public static final int INTAKE_ID = 24; public static final int INTAKE_FOLLOWER_ID = 21; public static final int CONVEYOR_ID = 22; - public static final double INTAKE_SPEED = 3000; //TODO - public static final double CONVEYOR_SPEED = 0.9; //TODO + public static final double INTAKE_SPEED = 3000; + public static final double CONVEYOR_SPEED = 0.9; } public static final class OuttakeC { public static final int OUTTAKE_ID = 31; @@ -285,8 +284,8 @@ public static final class OuttakeC { public static final MotorConfig OUTTAKE_FOLLOWER_MOTOR_CONFIG = MotorConfig.NEO_VORTEX; public static final MotorConfig OUTTAKE_FEEDER_MOTOR_CONFIG = MotorConfig.NEO_VORTEX; - public static final double OUTTAKE_SHOOTING_RPM = 2450; //TODO - public static final double OUTTAKE_PASSING_RPM = 5000; //TODO + public static final double OUTTAKE_SHOOTING_RPM = 2450; + public static final double OUTTAKE_PASSING_RPM = 5000; public static final double kP = 0.0004; public static final double kI = 0; diff --git a/src/main/java/org/carlmontrobotics/Robot.java b/src/main/java/org/carlmontrobotics/Robot.java index 249c536..1cadfd5 100644 --- a/src/main/java/org/carlmontrobotics/Robot.java +++ b/src/main/java/org/carlmontrobotics/Robot.java @@ -4,10 +4,7 @@ package org.carlmontrobotics; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 7445107..c4ee17c 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -17,20 +17,13 @@ import org.carlmontrobotics.commands.ManipulatorCommands.RunConveyor; import org.carlmontrobotics.commands.ManipulatorCommands.ShootBalls; import org.carlmontrobotics.commands.ManipulatorCommands.SmartShoot; -import org.carlmontrobotics.commands.ManipulatorCommands.OuttakeFeeder; import static org.carlmontrobotics.Constants.OuttakeC.OUTTAKE_PASSING_RPM; import static org.carlmontrobotics.Constants.OuttakeC.OUTTAKE_SHOOTING_RPM; -import java.util.ArrayList; -import java.util.List; import java.util.Optional; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.wpilibj.DriverStation; @@ -38,46 +31,27 @@ import java.util.function.BooleanSupplier; -import javax.print.attribute.standard.MediaSize.NA; - -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; - //auton import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.path.Waypoint; import com.pathplanner.lib.auto.NamedCommands; //controllers import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Axis; -import edu.wpi.first.wpilibj.XboxController.Button; //commands import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.PrintCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; + //control bindings import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; //constants import org.carlmontrobotics.Constants.OI; import org.carlmontrobotics.Constants.OI.Driver; import org.carlmontrobotics.Constants.OI.Manipulator; -import org.carlmontrobotics.Constants.OuttakeC; -import org.carlmontrobotics.Constants.Drivetrainc.Autoc; + //smartdashboard/elastic import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -119,7 +93,6 @@ public RobotContainer() { setBindingsManipulator(); SmartDashboard.putBoolean("Baby Mode", Config.CONFIG.isBabyMode()); - // SmartDashboard.putData("Rotate Command",new RotateToTag(drivetrain, limelight)); SmartDashboard.putString("Alliance", DriverStation.getAlliance().toString()); SmartDashboard.putString("Location", DriverStation.getLocation().toString()); SmartDashboard.putBoolean("Connected to FMS?", DriverStation.isFMSAttached()); diff --git a/src/main/java/org/carlmontrobotics/commands/AutonCommands/CenterLeftNeutralAuto.java b/src/main/java/org/carlmontrobotics/commands/AutonCommands/CenterLeftNeutralAuto.java index 3194b6c..21cf82b 100644 --- a/src/main/java/org/carlmontrobotics/commands/AutonCommands/CenterLeftNeutralAuto.java +++ b/src/main/java/org/carlmontrobotics/commands/AutonCommands/CenterLeftNeutralAuto.java @@ -6,16 +6,13 @@ import static org.carlmontrobotics.Constants.IntakeC.CONVEYOR_SPEED; import static org.carlmontrobotics.Constants.IntakeC.INTAKE_SPEED; -import static org.carlmontrobotics.Constants.OuttakeC.OUTTAKE_FEEDER_VOLT_PERC; import static org.carlmontrobotics.Constants.OuttakeC.OUTTAKE_SHOOTING_RPM; import org.carlmontrobotics.subsystems.Drivetrain; import org.carlmontrobotics.subsystems.Intake; import org.carlmontrobotics.subsystems.Outtake; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ @@ -40,14 +37,6 @@ public CenterLeftNeutralAuto(Drivetrain dt, Outtake shooter, Intake intake) { this.dt = dt; timer = new Timer(); addRequirements(shooter, intake, dt); - // SmartDashboard.putNumber("startShoot", startShoot); - // SmartDashboard.putNumber("endshoot", endshoot); - // SmartDashboard.putNumber("endStrafe", endStrafe); - // SmartDashboard.putNumber("endFastDrive", endFastDrive); - // SmartDashboard.putNumber("endSlowDrive", endSlowDrive); - // SmartDashboard.putNumber("endRotation", endRotation); - // SmartDashboard.putNumber("endStrafeDrive", endStrafeDrive); - // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @@ -56,13 +45,6 @@ public void initialize() { dt.resetFieldOrientation(); dt.setFieldOriented(true); timer.restart(); - // startShoot = SmartDashboard.getNumber("startShoot", startShoot); - // endshoot = SmartDashboard.getNumber("endshoot", endshoot); - // endStrafe = SmartDashboard.getNumber("endStrafe", endStrafe); - // endFastDrive = SmartDashboard.getNumber("endFastDrive", endFastDrive); - // endSlowDrive = SmartDashboard.getNumber("endSlowDrive", endSlowDrive); - // endRotation = SmartDashboard.getNumber("endRotation", endRotation); - // endStrafeDrive= SmartDashboard.getNumber("endStrafeDrive", endStrafeDrive); } // Called every time the scheduler runs while the command is scheduled. @@ -108,7 +90,6 @@ public void end(boolean interrupted) { shooter.spinOuttakeFeeder(0); intake.stopConveyor(); dt.stop(); - //intake.stopIntake(); } // Returns true when the command should end. diff --git a/src/main/java/org/carlmontrobotics/commands/AutonCommands/CenterRightNeutralAuto.java b/src/main/java/org/carlmontrobotics/commands/AutonCommands/CenterRightNeutralAuto.java index e3724b0..8592ba0 100644 --- a/src/main/java/org/carlmontrobotics/commands/AutonCommands/CenterRightNeutralAuto.java +++ b/src/main/java/org/carlmontrobotics/commands/AutonCommands/CenterRightNeutralAuto.java @@ -6,16 +6,13 @@ import static org.carlmontrobotics.Constants.IntakeC.CONVEYOR_SPEED; import static org.carlmontrobotics.Constants.IntakeC.INTAKE_SPEED; -import static org.carlmontrobotics.Constants.OuttakeC.OUTTAKE_FEEDER_VOLT_PERC; import static org.carlmontrobotics.Constants.OuttakeC.OUTTAKE_SHOOTING_RPM; import org.carlmontrobotics.subsystems.Drivetrain; import org.carlmontrobotics.subsystems.Intake; import org.carlmontrobotics.subsystems.Outtake; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ @@ -40,14 +37,6 @@ public CenterRightNeutralAuto(Drivetrain dt, Outtake shooter, Intake intake) { this.dt = dt; timer = new Timer(); addRequirements(shooter, intake, dt); - // SmartDashboard.putNumber("startShoot", startShoot); - // SmartDashboard.putNumber("endshoot", endshoot); - // SmartDashboard.putNumber("endStrafe", endStrafe); - // SmartDashboard.putNumber("endFastDrive", endFastDrive); - // SmartDashboard.putNumber("endSlowDrive", endSlowDrive); - // SmartDashboard.putNumber("endRotation", endRotation); - // SmartDashboard.putNumber("endStrafeDrive", endStrafeDrive); - // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @@ -56,13 +45,6 @@ public void initialize() { dt.resetFieldOrientation(); dt.setFieldOriented(true); timer.restart(); - // startShoot = SmartDashboard.getNumber("startShoot", startShoot); - // endshoot = SmartDashboard.getNumber("endshoot", endshoot); - // endStrafe = SmartDashboard.getNumber("endStrafe", endStrafe); - // endFastDrive = SmartDashboard.getNumber("endFastDrive", endFastDrive); - // endSlowDrive = SmartDashboard.getNumber("endSlowDrive", endSlowDrive); - // endRotation = SmartDashboard.getNumber("endRotation", endRotation); - // endStrafeDrive= SmartDashboard.getNumber("endStrafeDrive", endStrafeDrive); } // Called every time the scheduler runs while the command is scheduled. @@ -108,7 +90,6 @@ public void end(boolean interrupted) { shooter.spinOuttakeFeeder(0); intake.stopConveyor(); dt.stop(); - //intake.stopIntake(); } // Returns true when the command should end. diff --git a/src/main/java/org/carlmontrobotics/commands/AutonCommands/LeftNeutralAuto.java b/src/main/java/org/carlmontrobotics/commands/AutonCommands/LeftNeutralAuto.java index 5fad0c7..4918cc2 100644 --- a/src/main/java/org/carlmontrobotics/commands/AutonCommands/LeftNeutralAuto.java +++ b/src/main/java/org/carlmontrobotics/commands/AutonCommands/LeftNeutralAuto.java @@ -4,18 +4,13 @@ package org.carlmontrobotics.commands.AutonCommands; -import static org.carlmontrobotics.Constants.IntakeC.CONVEYOR_SPEED; import static org.carlmontrobotics.Constants.IntakeC.INTAKE_SPEED; -import static org.carlmontrobotics.Constants.OuttakeC.OUTTAKE_FEEDER_VOLT_PERC; -import static org.carlmontrobotics.Constants.OuttakeC.OUTTAKE_SHOOTING_RPM; import org.carlmontrobotics.subsystems.Drivetrain; import org.carlmontrobotics.subsystems.Intake; import org.carlmontrobotics.subsystems.Outtake; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ @@ -40,14 +35,6 @@ public LeftNeutralAuto(Drivetrain dt, Outtake shooter, Intake intake) { this.dt = dt; timer = new Timer(); addRequirements(shooter, intake, dt); - // SmartDashboard.putNumber("startShoot", startShoot); - // SmartDashboard.putNumber("endshoot", endshoot); - // SmartDashboard.putNumber("endStrafe", endStrafe); - // SmartDashboard.putNumber("endFastDrive", endFastDrive); - // SmartDashboard.putNumber("endSlowDrive", endSlowDrive); - // SmartDashboard.putNumber("endRotation", endRotation); - // SmartDashboard.putNumber("endStrafeDrive", endStrafeDrive); - // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @@ -56,13 +43,6 @@ public void initialize() { dt.resetFieldOrientation(); dt.setFieldOriented(true); timer.restart(); - // startShoot = SmartDashboard.getNumber("startShoot", startShoot); - // endshoot = SmartDashboard.getNumber("endshoot", endshoot); - // endStrafe = SmartDashboard.getNumber("endStrafe", endStrafe); - // endFastDrive = SmartDashboard.getNumber("endFastDrive", endFastDrive); - // endSlowDrive = SmartDashboard.getNumber("endSlowDrive", endSlowDrive); - // endRotation = SmartDashboard.getNumber("endRotation", endRotation); - // endStrafeDrive= SmartDashboard.getNumber("endStrafeDrive", endStrafeDrive); } // Called every time the scheduler runs while the command is scheduled. @@ -94,7 +74,6 @@ public void end(boolean interrupted) { shooter.spinOuttakeFeeder(0); intake.stopConveyor(); dt.stop(); - //intake.stopIntake(); } // Returns true when the command should end. diff --git a/src/main/java/org/carlmontrobotics/commands/AutonCommands/RightNeutralAuto.java b/src/main/java/org/carlmontrobotics/commands/AutonCommands/RightNeutralAuto.java index 037e201..20c96cc 100644 --- a/src/main/java/org/carlmontrobotics/commands/AutonCommands/RightNeutralAuto.java +++ b/src/main/java/org/carlmontrobotics/commands/AutonCommands/RightNeutralAuto.java @@ -4,18 +4,13 @@ package org.carlmontrobotics.commands.AutonCommands; -import static org.carlmontrobotics.Constants.IntakeC.CONVEYOR_SPEED; import static org.carlmontrobotics.Constants.IntakeC.INTAKE_SPEED; -import static org.carlmontrobotics.Constants.OuttakeC.OUTTAKE_FEEDER_VOLT_PERC; -import static org.carlmontrobotics.Constants.OuttakeC.OUTTAKE_SHOOTING_RPM; import org.carlmontrobotics.subsystems.Drivetrain; import org.carlmontrobotics.subsystems.Intake; import org.carlmontrobotics.subsystems.Outtake; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ @@ -25,9 +20,6 @@ public class RightNeutralAuto extends Command { private final Intake intake; private final Timer timer; - private double startShoot = 0.5; - private double endshoot = 4; - private double endStrafe = 1; private double endFastDrive = 2.4; private double endSlowDrive = 0.1; private double endRotation = 0.6; @@ -40,14 +32,6 @@ public RightNeutralAuto(Drivetrain dt, Outtake shooter, Intake intake) { this.dt = dt; timer = new Timer(); addRequirements(shooter, intake, dt); - // SmartDashboard.putNumber("startShoot", startShoot); - // SmartDashboard.putNumber("endshoot", endshoot); - // SmartDashboard.putNumber("endStrafe", endStrafe); - // SmartDashboard.putNumber("endFastDrive", endFastDrive); - // SmartDashboard.putNumber("endSlowDrive", endSlowDrive); - // SmartDashboard.putNumber("endRotation", endRotation); - // SmartDashboard.putNumber("endStrafeDrive", endStrafeDrive); - // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @@ -56,13 +40,6 @@ public void initialize() { dt.resetFieldOrientation(); dt.setFieldOriented(true); timer.restart(); - // startShoot = SmartDashboard.getNumber("startShoot", startShoot); - // endshoot = SmartDashboard.getNumber("endshoot", endshoot); - // endStrafe = SmartDashboard.getNumber("endStrafe", endStrafe); - // endFastDrive = SmartDashboard.getNumber("endFastDrive", endFastDrive); - // endSlowDrive = SmartDashboard.getNumber("endSlowDrive", endSlowDrive); - // endRotation = SmartDashboard.getNumber("endRotation", endRotation); - // endStrafeDrive= SmartDashboard.getNumber("endStrafeDrive", endStrafeDrive); } // Called every time the scheduler runs while the command is scheduled. @@ -94,7 +71,6 @@ public void end(boolean interrupted) { shooter.spinOuttakeFeeder(0); intake.stopConveyor(); dt.stop(); - //intake.stopIntake(); } // Returns true when the command should end. diff --git a/src/main/java/org/carlmontrobotics/commands/DriveCommands/RotateToFieldRelativeAngle.java b/src/main/java/org/carlmontrobotics/commands/DriveCommands/RotateToFieldRelativeAngle.java index d3029f6..a795f18 100644 --- a/src/main/java/org/carlmontrobotics/commands/DriveCommands/RotateToFieldRelativeAngle.java +++ b/src/main/java/org/carlmontrobotics/commands/DriveCommands/RotateToFieldRelativeAngle.java @@ -8,7 +8,6 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.util.sendable.SendableRegistry; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; public class RotateToFieldRelativeAngle extends Command { diff --git a/src/main/java/org/carlmontrobotics/commands/DriveCommands/RotateToNearest60.txt b/src/main/java/org/carlmontrobotics/commands/DriveCommands/RotateToNearest60.txt deleted file mode 100644 index 9d2a4c9..0000000 --- a/src/main/java/org/carlmontrobotics/commands/DriveCommands/RotateToNearest60.txt +++ /dev/null @@ -1,49 +0,0 @@ -// 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.commands.DriveCommands; - -import org.carlmontrobotics.subsystems.Drivetrain; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Command; - -public class RotateToNearest60 extends Command { - - private final RotateToFieldRelativeAngle rotateCommand; - private final Drivetrain drivetrain; - - public RotateToNearest60(Drivetrain drivetrain) { - this.drivetrain = drivetrain; - Rotation2d targetAngle = getNearest60(drivetrain.getHeading()); - this.rotateCommand = new RotateToFieldRelativeAngle(targetAngle, drivetrain); - - addRequirements(drivetrain); - } - - @Override - public void initialize() { - rotateCommand.initialize(); - } - - @Override - public void execute() { - rotateCommand.execute(); - } - - @Override - public boolean isFinished() { - return rotateCommand.isFinished(); - } - - @Override - public void end(boolean interrupted) { - rotateCommand.end(interrupted); - } - - private Rotation2d getNearest60(double currentAngle) { - double modAngle = (Math.round(currentAngle / 60.0) * 60) % 360; - if (modAngle > 180) modAngle -= 360; // Keep angle within -180 to 180 - return Rotation2d.fromDegrees(modAngle); - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/DriveCommands/TeleopDrive.java b/src/main/java/org/carlmontrobotics/commands/DriveCommands/TeleopDrive.java index 0386341..a8f13b5 100644 --- a/src/main/java/org/carlmontrobotics/commands/DriveCommands/TeleopDrive.java +++ b/src/main/java/org/carlmontrobotics/commands/DriveCommands/TeleopDrive.java @@ -6,18 +6,15 @@ import java.util.function.DoubleSupplier; import org.carlmontrobotics.Constants; -import org.carlmontrobotics.Robot; import org.carlmontrobotics.subsystems.Drivetrain; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class TeleopDrive extends Command { - private static double robotPeriod = Robot.kDefaultPeriod; + //private static double robotPeriod = Robot.kDefaultPeriod; private final Drivetrain drivetrain; private DoubleSupplier fwd; private DoubleSupplier str; @@ -25,7 +22,7 @@ public class TeleopDrive extends Command { private BooleanSupplier slow; private double currentForwardVel = 0; private double currentStrafeVel = 0; - private double prevTimestamp; + //private double prevTimestamp; GenericHID manipulatorController; BooleanSupplier babyModeSupplier; @@ -46,33 +43,20 @@ public TeleopDrive(Drivetrain drivetrain, DoubleSupplier fwd, DoubleSupplier str // Called when the command is initially scheduled. @Override public void initialize() { - // SmartDashboard.putNumber("slow turn const", kSlowDriveRotation); - // SmartDashboard.putNumber("slow speed const", kSlowDriveSpeed); - // SmartDashboard.putNumber("normal turn const", kNormalDriveRotation); - // SmartDashboard.putNumber("normal speed const", kNormalDriveSpeed); - prevTimestamp = Timer.getFPGATimestamp(); + //prevTimestamp = Timer.getFPGATimestamp(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double currentTime = Timer.getFPGATimestamp(); - robotPeriod = currentTime - prevTimestamp; + //double currentTime = Timer.getFPGATimestamp(); + //robotPeriod = currentTime - prevTimestamp; if (!hasDriverInput()) { drivetrain.drive(0,0,0); } else { - double[] speeds = getRequestedSpeeds(); - // SmartDashboard.putNumber("Elapsed time", currentTime - prevTimestamp); - prevTimestamp = currentTime; - // kSlowDriveRotation = SmartDashboard.getNumber("slow turn const", kSlowDriveRotation); - // kSlowDriveSpeed = SmartDashboard.getNumber("slow speed const", kSlowDriveSpeed); - // kNormalDriveRotation = SmartDashboard.getNumber("normal turn const", kNormalDriveRotation); - // kNormalDriveSpeed = SmartDashboard.getNumber("normal speed const", kNormalDriveSpeed); - - // SmartDashboard.putNumber("fwd", speeds[0]); - // SmartDashboard.putNumber("strafe", speeds[1]); - // SmartDashboard.putNumber("turn", speeds[2]); + double[] speeds = getRequestedSpeeds(); + //prevTimestamp = currentTime; drivetrain.drive(speeds[0], speeds[1], speeds[2]); } } @@ -85,27 +69,17 @@ public double[] getRequestedSpeeds() { double forward = fwd.getAsDouble(); double strafe = str.getAsDouble(); double rotateClockwise = rcw.getAsDouble(); - // SmartDashboard.putNumber("fwdIN", forward); - // SmartDashboard.putNumber("strafeIN", strafe); - // SmartDashboard.putNumber("turnIN", rotateClockwise); - // System.out.println("fwd str rcw: "+forward+", "+strafe+", "+rotateClockwise); - boolean slow2 = slow.getAsBoolean(); forward *= maxForward; strafe *= maxStrafe; rotateClockwise *= maxRCW; - // System.out.println("teleopDrive ExtraSpeedMult%: "+drivetrain.extraSpeedMult); double driveMultiplier = (slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed); double rotationMultiplier = drivetrain.extraSpeedMult + (slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation); - // double driveMultiplier = (slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed); - // double rotationMultiplier = (slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation); if(babyModeSupplier.getAsBoolean()){ driveMultiplier = kBabyDriveSpeed; rotationMultiplier = kBabyDriveRotation; } - // double driveMultiplier = kNormalDriveSpeed; - // double rotationMultiplier = kNormalDriveRotation; forward *= driveMultiplier; strafe *= driveMultiplier; diff --git a/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/EjectBalls.java b/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/EjectBalls.java index b19fbf6..38631a1 100644 --- a/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/EjectBalls.java +++ b/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/EjectBalls.java @@ -6,11 +6,7 @@ import static org.carlmontrobotics.Constants.IntakeC.CONVEYOR_SPEED; import static org.carlmontrobotics.Constants.IntakeC.INTAKE_SPEED; -import static org.carlmontrobotics.Constants.OuttakeC.OUTTAKE_FEEDER_VOLT_PERC; - import org.carlmontrobotics.subsystems.Intake; -import org.carlmontrobotics.subsystems.Outtake; - import edu.wpi.first.wpilibj2.command.Command; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ diff --git a/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/IntakeBalls.java b/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/IntakeBalls.java index 06145a6..1d94341 100644 --- a/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/IntakeBalls.java +++ b/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/IntakeBalls.java @@ -8,7 +8,6 @@ import org.carlmontrobotics.subsystems.Intake; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; diff --git a/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/RunConveyor.java b/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/RunConveyor.java index 5519c56..0fe8a02 100644 --- a/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/RunConveyor.java +++ b/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/RunConveyor.java @@ -7,7 +7,6 @@ import static org.carlmontrobotics.Constants.IntakeC.*; import org.carlmontrobotics.subsystems.Intake; -import org.carlmontrobotics.subsystems.Outtake; import edu.wpi.first.wpilibj2.command.Command; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ diff --git a/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/SmartShoot.java b/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/SmartShoot.java index c2aa6fc..9ba2bb1 100644 --- a/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/SmartShoot.java +++ b/src/main/java/org/carlmontrobotics/commands/ManipulatorCommands/SmartShoot.java @@ -11,7 +11,6 @@ import org.carlmontrobotics.subsystems.Outtake; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -30,7 +29,6 @@ public class SmartShoot extends Command { /** Creates a new SmartShoot. */ public SmartShoot(Outtake outtake, Intake intake, double RPM) { - // Use addRequirements() here to declare subsystem dependencies. goalRPM = Math.min(RPM, 5500); this.outtake = outtake; this.intake = intake; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index b0a8fad..166a0a5 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -17,7 +17,7 @@ import static org.carlmontrobotics.Config.CONFIG; import edu.wpi.first.hal.SimDouble; - +import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; @@ -109,10 +109,7 @@ //Constants import org.carlmontrobotics.Constants; -import org.carlmontrobotics.Constants.Drivetrainc; -import org.carlmontrobotics.Constants.Drivetrainc.Autoc; -import org.carlmontrobotics.Robot; -import org.carlmontrobotics.subsystems.Limelight; + import org.carlmontrobotics.commands.DriveCommands.RotateToFieldRelativeAngle; import org.carlmontrobotics.commands.DriveCommands.TeleopDrive; import static org.carlmontrobotics.Constants.Drivetrainc.*; @@ -145,8 +142,6 @@ public class Drivetrain extends SubsystemBase { private SwerveModule moduleBR; private final Field2d field = new Field2d(); - private final Field2d odometryField = new Field2d(); - private final Field2d poseWithLimelightField = new Field2d(); public double ppKpDrive = 5.0; public double ppKiDrive = 0; @@ -166,9 +161,11 @@ public class Drivetrain extends SubsystemBase { public double extraSpeedMult = 0; private double lastSetX = 0, lastSetY = 0, lastSetTheta = 0; + double kP = 0; double kI = 0; double kD = 0; + public enum Mode { coast, brake, @@ -176,22 +173,11 @@ public enum Mode { } public final Limelight ll; + + public Drivetrain(Limelight ll) { this.ll = ll; AutoBuilder(); - //SmartDashboard.putNumber("Goal Velocity", 0); - //SmartDashboard.putNumber("kP", 0); - //SmartDashboard.putNumber("kI", 0); - //SmartDashboard.putNumber("kD", 0); - - // SmartDashboard.putNumber("Pose Estimator t x (m)", lastSetX); - // SmartDashboard.putNumber("Pose Estimator set y (m)", lastSetY); - // SmartDashboard.putNumber("Pose Estimator set rotation (deg)", - // lastSetTheta); - - // SmartDashboard.putNumber("pose estimator std dev x", STD_DEV_X_METERS); - // SmartDashboard.putNumber("pose estimator std dev y", STD_DEV_Y_METERS); - //SmartDashboard.putNumber("GoalPos", 0); // Calibrate Gyro { @@ -269,12 +255,7 @@ public Drivetrain(Limelight ll) { }; gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); } - SmartDashboard.putData("Module FL",moduleFL); - SmartDashboard.putData("Module FR",moduleFR); - SmartDashboard.putData("Module BL",moduleBL); - SmartDashboard.putData("Module BR",moduleBR); - SmartDashboard.putNumber("bigoal", 0); SparkMaxConfig driveConfig = new SparkMaxConfig(); driveConfig.openLoopRampRate(secsPer12Volts); @@ -304,10 +285,6 @@ public Drivetrain(Limelight ll) { coder.getVelocity().setUpdateFrequency(500); } - SmartDashboard.putData("Field", field); - //SmartDashboard.putData("Odometry Field", odometryField); - //martDashboard.putData("Pose with Limelight Field", poseWithLimelightField); - accelX = gyro.getWorldLinearAccelX(); // Acceleration along the X-axis accelY = gyro.getWorldLinearAccelY(); // Acceleration along the Y-axis accelXY = Math.sqrt(gyro.getWorldLinearAccelX() * gyro.getWorldLinearAccelX() + gyro.getWorldLinearAccelY() * gyro.getWorldLinearAccelY()); @@ -330,19 +307,9 @@ public Drivetrain(Limelight ll) { Rotation2d.fromDegrees(getHeading()), getModulePositions(), new Pose2d()); - - // Setup autopath builder - //configurePPLAutoBuilder(); - // SmartDashboard.putNumber("chassis speeds x", 0); - // SmartDashboard.putNumber("chassis speeds y", 0); - - // SmartDashboard.putNumber("chassis speeds theta", 0); - SmartDashboard.putData(this); // For seeing drivetrain data in SmartDashboard - } - public boolean isAtAngle(double desiredAngleDeg, double toleranceDeg){ for (SwerveModule module : modules) { if (!(Math.abs(MathUtil.inputModulus(module.getModuleAngle() - desiredAngleDeg, -90, 90)) < toleranceDeg)) @@ -466,15 +433,6 @@ public void periodic() { } //odometry.update(Rotation2d.fromDegrees(getHeading()), getModulePositions()); - // updateMT2PoseEstimator(); - - // double currSetX = - // SmartDashboard.getNumber("Pose Estimator set x (m)", lastSetX); - // double currSetY = - // SmartDashboard.getNumber("Pose Estimator set y (m)", lastSetY); - // double currSetTheta = SmartDashboard - // .getNumber("Pose Estimator set rotation (deg)", lastSetTheta); - // if (lastSetX != currSetX || lastSetY != currSetY // || lastSetTheta != currSetTheta) { // setPose(new Pose2d(currSetX, currSetY, @@ -484,33 +442,6 @@ public void periodic() { // setPose(new Pose2d(getPose().getTranslation().getX(), // getPose().getTranslation().getY(), // Rotation2d.fromDegrees(getHeading()))); - - - // SmartDashboard.putNumber("X position with limelight", getPoseWithLimelight().getX()); - // SmartDashboard.putNumber("Y position with limelight", getPoseWithLimelight().getY()); - SmartDashboard.putNumber("X position with gyro", getPose().getX()); - SmartDashboard.putNumber("Y position with gyro", getPose().getY()); - SmartDashboard.putData(CONFIG); - - //For finding acceleration of drivetrain for collision detector - SmartDashboard.putNumber("Accel X", accelX); - SmartDashboard.putNumber("Accel Y", accelY); - SmartDashboard.putNumber("2D Acceleration ", accelXY); - - // // // SmartDashboard.putNumber("Pitch", gyro.getPitch()); - // // // SmartDashboard.putNumber("Roll", gyro.getRoll()); - // SmartDashboard.putNumber("Raw gyro angle", gyro.getAngle()); - // SmartDashboard.putNumber("Robot Heading", getHeading()); - // // // SmartDashboard.putNumber("AdjRoll", gyro.getPitch() - initPitch); - // // // SmartDashboard.putNumber("AdjPitch", gyro.getRoll() - initRoll); - // SmartDashboard.putBoolean("Field Oriented", fieldOriented); - // SmartDashboard.putNumber("Gyro Compass Heading", gyro.getCompassHeading()); - // SmartDashboard.putNumber("Compass Offset", compassOffset); - // SmartDashboard.putBoolean("Current Magnetic Field Disturbance", gyro.isMagneticDisturbance()); - SmartDashboard.putNumber("front left encoder", moduleFL.getModuleAngle()); - SmartDashboard.putNumber("front right encoder", moduleFR.getModuleAngle()); - SmartDashboard.putNumber("back left encoder", moduleBL.getModuleAngle()); - SmartDashboard.putNumber("back right encoder", moduleBR.getModuleAngle()); } @Override @@ -571,10 +502,6 @@ public void setExtraSpeedMult(double set) { public void drive(double forward, double strafe, double rotation) { drive(getSwerveStates(forward, strafe, rotation)); } - public void logWhileDriving(SwerveModuleState[] moduleStates) { - SmartDashboard.putNumber("RequestedSpeed", moduleStates[1].speedMetersPerSecond); - drive(moduleStates); - } /** * Implements the provided SwerveStates for all 4 modules to get the wanted outcome @@ -585,9 +512,7 @@ public void drive(SwerveModuleState[] moduleStates) { double max = maxSpeed; SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, max); for (int i = 0; i < 4; i++) { - // SmartDashboard.putNumber("moduleIn" + Integer.toString(i), moduleStates[i].angle.getDegrees()); moduleStates[i].optimize(Rotation2d.fromDegrees(modules[i].getModuleAngle())); - // SmartDashboard.putNumber("moduleOT" + Integer.toString(i), moduleStates[i].angle.getDegrees()); modules[i].move(moduleStates[i].speedMetersPerSecond, moduleStates[i].angle.getDegrees()); } } @@ -605,7 +530,7 @@ public void AutoBuilder() { //Supplier robotRelativeSpeedsSupplier, this::getSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE //BiConsumer output, - (speeds) -> logWhileDriving(kinematics.toSwerveModuleStates(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards + (speeds) -> drive(kinematics.toSwerveModuleStates(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards //PathFollowingController controller, new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains new PIDConstants(4 @@ -739,27 +664,6 @@ public void setPose(Pose2d initialPose) { //odometry.resetPosition(Rotation2d.fromDegrees(getHeading()), getModulePositions(), initialPose); } - //This method will set the pose using limelight if it sees a tag and if not it is supposed to run like setPose() - public void setPoseWithLimelight(Pose2d backupPose){ //the pose will be set to backupPose if no tag is seen - // Rotation2d gyroRotation = gyro.getRotation2d(); - // Pose2d pose; - - // if (LimelightHelpers.getTV(REEF_LL)) { - - // pose = LimelightHelpers.getBotPose2d_wpiBlue(REEF_LL); - - // } else if (LimelightHelpers.getTV(CORAL_LL)) { - - // pose = LimelightHelpers.getBotPose2d_wpiBlue(CORAL_LL); - // } - // else { - // pose = backupPose; - // } - - // poseEstimator.resetPosition(gyroRotation, getModulePositions(), pose); - // simGyroOffset = pose.getRotation().minus(gyroRotation); - - } /** * Detects if the robot has experienced a collision based on acceleration thresholds. * @return true if a 2D acceleration greater than the {@link #COLLISION_ACCELERATION_THRESHOLD} false otherwise. @@ -1109,10 +1013,6 @@ private void sysIdSetup() { m_revs[i] = Rotation.mutable(0); m_revs_vel[i] = RotationsPerSecond.mutable(0); } - - // SmartDashboard.putNumber("Desired Angle", 0); - - // SmartDashboard.putNumber("kS", 0); } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Intake.java b/src/main/java/org/carlmontrobotics/subsystems/Intake.java index 6e9b2da..eb029e8 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Intake.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Intake.java @@ -40,18 +40,32 @@ public Intake() { pidC = intakeMotor.getClosedLoopController(); } + /** + * Spins intake using PID control + * @param intakeSpeed RPM speed + */ public void spinIntake(double intakeSpeed) { pidC.setSetpoint(intakeSpeed, ControlType.kVelocity); } + /** + * Spins conveyor using voltage percentage control + * @param conveyorSpeed voltage percentage: -1.0 to 1.0 + */ public void spinConveyor(double conveyorSpeed) { conveyorMotor.set(conveyorSpeed); } + /** + * Stops the intake + */ public void stopIntake(){ intakeMotor.set(0); } + /** + * Stops the conveyor + */ public void stopConveyor(){ conveyorMotor.set(0); } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index d8f43f3..a75df71 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -3,15 +3,12 @@ import static org.carlmontrobotics.Constants.LimeLightc.*; -import java.util.random.RandomGenerator.LeapableGenerator; - import org.carlmontrobotics.lib199.vendorLibs.LimelightHelpers; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Limelight extends SubsystemBase { @@ -136,7 +133,6 @@ public double getTX(String name) { return LimelightHelpers.getTX(name); } - //TODO: TEST WHICH ONE IS MORE ACCURATE public double getRotateAngleRadMT2(String limelightName) { Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(limelightName); // pose of the target