Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-09 08:48:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'shooter-test' into AutoAlign

Browse Source
This commit is contained in:
Abhishrek05
2024-02-20 19:42:53 -07:00
parent e77d2d5a6c 3ec5525d5a
commit 0f71e0ec08
24 changed files with 969 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+8 -8
View File
@@ -168,19 +168,19 @@ public final class Constants {
public static final class ShooterConstants {
public static final int LEFT_SHOOTER_ID = 15; // final
public static final int RIGHT_SHOOTER_ID = 16; // final
public static final double SHOOTER_SPEED = 1.0; // final
public static final double SHOOTER_SPEED = 0.35; // final
public static final double SHOOTER_IDLE = 0.4; // final
public static final double SHOOTER_IDLE_LIMELIGHT = 0.8;
}
public static final class IntakeConstants {
public static final int INTAKE_MOTOR_ID = 18; //TODO:
public static final int PIVOT_MOTOR_ID = 17; //TODO:
public static final double INTAKE_SPEED = 0.75; //TODO:
public static final double INTAKE_OUT_SPEED = 0.1;
public static final double HANDOFF_SPEED = 0.4; //TODO:
public static final double PIVOT_SPEED = 0.2; //TODO:
public static final int INTAKE_MOTOR_ID = 18;
public static final int PIVOT_MOTOR_ID = 17;
public static final double INTAKE_SPEED = 0.75;
public static final double INTAKE_OUT_SPEED_UNPRESSED = 1.0;
public static final double INTAKE_OUT_SPEED_PRESSED = 0.5;
public static final double HANDOFF_SPEED = 0.4;
public static final double PIVOT_SPEED = 0.2;
public static final class ArmPID {
public static final Gains INTAKE_GAINS = new Gains(0.05, 0, 0, 0, 0, 1.0);
src/main/java/frc4388/robot/Robot.java
+12 -13
View File
@@ -38,7 +38,6 @@ public class Robot extends TimedRobot {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
CameraServer.startAutomaticCapture();
}
/**
@@ -84,20 +83,20 @@ public class Robot extends TimedRobot {
*/
@Override
public void autonomousInit() {
// m_autonomousCommand = m_robotContainer.getAutonomousCommand();
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
// /*
// * String autoSelected = SmartDashboard.getString("Auto Selector",
// * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
// * = new MyAutoCommand(); break; case "Default Auto": default:
// * autonomousCommand = new ExampleCommand(); break; }
// */
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// // schedule the autonomous command (example)
// if (m_autonomousCommand != null) {
// m_autonomousCommand.schedule();
// }
// m_robotTime.startMatchTime();
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
}
m_robotTime.startMatchTime();
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+197 -81
View File
@@ -7,12 +7,10 @@
package frc4388.robot;
import java.time.Instant;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -24,14 +22,16 @@ import frc4388.robot.commands.Autos.AutoAlign;
import frc4388.robot.commands.Autos.PlaybackChooser;
import frc4388.robot.commands.Swerve.JoystickPlayback;
import frc4388.robot.commands.Swerve.JoystickRecorder;
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
import frc4388.robot.commands.Swerve.neoJoystickRecorder;
import frc4388.robot.commands.Intake.ArmIntakeIn;
import frc4388.robot.commands.Intake.RotateIntakeToPosition;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Limelight;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.Intake;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.VirtualController;
import frc4388.utility.controller.XboxController;
/**
@@ -65,17 +65,19 @@ public class RobotContainer {
/* Controllers */
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
/* Autos */
// This shoud be in a SequentialCommandGroup vvv
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(2);
private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
private Command MoveToSpeaker = new JoystickPlayback(m_robotSwerveDrive, "MoveToSpeaker.txt");
/* Virtual Controllers */
private final VirtualController m_virtualDriver = new VirtualController(0);
private final VirtualController m_virtualOperator = new VirtualController(1);
private Limelight limelight = new Limelight();
private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter);
private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.pidIn()),
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin())
);
@@ -94,7 +96,7 @@ public class RobotContainer {
private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight);
private SequentialCommandGroup autoShoot = new SequentialCommandGroup(
MoveToSpeaker,
// MoveToSpeaker,
autoAlign,
new InstantCommand(() -> m_robotShooter.spin()),
new WaitCommand(3.0),
@@ -112,27 +114,88 @@ public class RobotContainer {
private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake)
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake)
);
private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
/* Autos */
private Command taxi = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
private Command startLeftMoveRight = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartLeftMoveRight.txt");
private Command startRightMoveLeft = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt");
private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup (
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new WaitCommand(1).asProxy(),
new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")
);
private SequentialCommandGroup oneNoteStartingSpeakerStationary = new SequentialCommandGroup (
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
private SequentialCommandGroup oneNoteStartingFromLeft = new SequentialCommandGroup(
startLeftMoveRight.asProxy(),
ejectToShoot.asProxy(),
taxi.asProxy()
);
private SequentialCommandGroup oneNoteStartingFromRight = new SequentialCommandGroup(
startRightMoveLeft.asProxy(),
ejectToShoot.asProxy(),
taxi.asProxy()
);
private SequentialCommandGroup twoNoteStartingFromSpeaker = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
intakeToShootStuff.asProxy(),
new WaitCommand(1).asProxy(),
new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt1.txt"),
intakeToShoot.asProxy(),
new WaitCommand(1).asProxy(),
new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"),
new WaitCommand(0.5).asProxy(),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
.addOption("Taxi Auto", new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
.addOption("Taxi Auto", taxi.asProxy())
.addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker)
.addOption("One Note Auto Starting in Front of Speaker, But Stay", oneNoteStartingSpeakerStationary)
.addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft)
.addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight)
.buildDisplay();
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
configureButtonBindings();
configureButtonBindings();
configureVirtualButtonBindings();
DriverStation.silenceJoystickConnectionWarning(true);
CameraServer.startAutomaticCapture();
/* Default Commands */
// drives the robot with a two-axis input from the driver controller
@@ -145,10 +208,6 @@ public class RobotContainer {
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
SmartDashboard.putNumber("Velocity Output", m_robotIntake.getVelocity());
// m_robotIntake.resetPostion();
}
/**
@@ -162,8 +221,6 @@ public class RobotContainer {
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
/* Auto Recording */
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
@@ -171,84 +228,131 @@ public class RobotContainer {
// () -> getDeadbandedDriverController().getLeftY(),
// () -> getDeadbandedDriverController().getRightX(),
// () -> getDeadbandedDriverController().getRightY(),
// "IntenseTaxi.txt"))
// () -> getDeadbandedOperatorController().getLeftBumper(),
// () -> getDeadbandedOperatorController().getRightBumper(),
// "TwoNotePrt1.txt"))
// .onFalse(new InstantCommand());
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
.onFalse(new InstantCommand());
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
// .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
// .onFalse(new InstantCommand());
new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
.whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
"2note.auto"))
.onFalse(new InstantCommand());
/* Speed */
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
"2note.auto",
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
true, false))
.onFalse(new InstantCommand());
// /* Speed */
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
// // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
/* Operator Buttons */
// new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor(), m_robotIntake))
// .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake));
// new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 360).execute(), m_robotIntake))
// .onFalse(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 0).execute(), m_robotShooter));
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.pidIn()))
.onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
.onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn()))
.onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.pidOut()))
.onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
// //Pull arm in
// new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotIntake.pidIn(), m_robotIntake))
// .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor(), m_robotIntake));
// //Pull arm out
// new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotIntake.pidOut(), m_robotIntake))
// .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor(), m_robotIntake));
// //Intake Note
// new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor(), m_robotIntake))
// .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake));
// //Outtake Note
// new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake))
// .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake));
//Spin Shooter Motors
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.talonPIDOut()))
.onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.talonSpinIntakeMotor()))
.onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors()));
// Override Intake Position encoder: out
new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-53), m_robotIntake));
// Override Intake Position encoder: in
new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(0), m_robotIntake));
//Spin Shooter Motors
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter))
.onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter));
// //Intake Note and ramp up shooter to 40%
// new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
// .onTrue(intakeToShoot);
// //Ramps up shooter to 100% to Shooter
// new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
// .onTrue(outtakeToShootFull);
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(ejectToShoot)
.onFalse(turnOffShoot);
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(i)
.onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn()));
//spins up shooter, no wind down
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter));
}
private void configureVirtualButtonBindings() {
/* Driver Buttons */
new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
/* Speed */
new JoystickButton(getVirtualDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
new JoystickButton(getVirtualDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
/* Operator Buttons */
new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.pidIn()))
.onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
new JoystickButton(getVirtualOperatorController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.pidOut()))
.onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
// Override Intake Position encoder: out
new JoystickButton(getVirtualOperatorController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake));
// Override Intake Position encoder: in
new JoystickButton(getVirtualOperatorController(), XboxController.START_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(0), m_robotIntake));
//Spin Shooter Motors
new JoystickButton(getVirtualOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter))
.onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter));
new JoystickButton(getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(ejectToShoot)
.onFalse(turnOffShoot);
new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(i)
.onFalse(new InstantCommand(() -> m_robotIntake.pidIn()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter));
}
/**
@@ -257,8 +361,12 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
//no auto
return playbackChooser.getCommand();
//no auto
return new neoJoystickPlayback(m_robotSwerveDrive,
"2note.auto",
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
true, false);
//return playbackChooser.getCommand();
}
/**
@@ -271,4 +379,12 @@ public class RobotContainer {
public DeadbandedXboxController getDeadbandedOperatorController() {
return this.m_operatorXbox;
}
public VirtualController getVirtualDriverController() {
return m_virtualDriver;
}
public VirtualController getVirtualOperatorController() {
return m_virtualOperator;
}
}
src/main/java/frc4388/robot/RobotMap.java
+2 -2
View File
@@ -70,8 +70,8 @@ public class RobotMap {
public final TalonFX rightShooter = new TalonFX(ShooterConstants.RIGHT_SHOOTER_ID);
/* Intake Subsystem */
public final CANSparkMax intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
public final CANSparkMax pivotMotor = new CANSparkMax(IntakeConstants.PIVOT_MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
public final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID);
public final TalonFX pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID);
void configureLEDMotorControllers() {
}
src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java
+6 -5
View File
@@ -42,6 +42,7 @@ public class PlaybackChooser {
m_playback = m_choosers.get(0);
nextChooser();
// ! This does not work, why?
Shuffleboard.getTab("Auto Chooser")
.add("Add Sequence", new InstantCommand(() -> nextChooser()))
.withPosition(4, 0);
@@ -68,13 +69,13 @@ public class PlaybackChooser {
String[] dirs = m_dir.list();
if(dirs == null){ // Fix funny error
return;
if(dirs != null){ // Fix funny error
for (String auto : dirs) {
chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
}
}
for (String auto : dirs) {
chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
}
for (var cmd_name : m_commandPool.keySet()) {
chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
}
src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt
+20
View File
@@ -0,0 +1,20 @@
AUTO file format
HEADER static size 0x5
0x00 BYTE NUM AXES: defualts to 6
0x01 BYTE NUM POV: defualts to 1
0x02 BYTE NUM CONTROLLERS: defualts to 2
0x03 SHORT FRAMES: any value greator or equal than one.
FRAME PER CONTROLLER: defualt size 0x34
0x00 DOUBLE AXES[NUM AXES]
0x30 SHORT BUTTONS
0x32 SHORT POVs[NUM POV]
FRAME: size varrys
FRAME PER CONTROLLER[NUM CONTROLLERS]
INT UNIXTIMESTAMP
FILE:
HEADER
FRAME[FRAMES]
src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java
+11 -8
View File
@@ -28,8 +28,8 @@ public class ArmIntakeIn extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
robotIntake.pidOut();
robotIntake.spinIntakeMotor();
robotIntake.talonPIDOut();
robotIntake.talonSpinIntakeMotor();
}
// Called once the command ends or is interrupted.
@@ -39,11 +39,14 @@ public class ArmIntakeIn extends Command {
// Returns true when the command should end.
@Override
public boolean isFinished() {
if(robotIntake.getIntakeLimitSwtichState() == true) {
return true;
} else {
return false;
}
return robotIntake.getTalonIntakeLimitSwitchState();
// if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0)))
// {
// return !true==true;
// }
// else
// {
// return !false==!(!(true));
// }
}
}
src/main/java/frc4388/robot/commands/Intake/RotateIntakeToPosition.java
-38
View File
@@ -1,38 +0,0 @@
// 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.robot.commands.Intake;
import com.revrobotics.CANSparkMax;
import edu.wpi.first.math.geometry.Translation2d;
import frc4388.robot.commands.PID;
import frc4388.robot.subsystems.SwerveDrive;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import frc4388.robot.subsystems.Intake;
public class RotateIntakeToPosition extends PID {
Intake intake;
double targetAngle;
/** Creates a new PIDSparkMax. */
public RotateIntakeToPosition(Intake intake, double targetAngle) {
super(0.3, 0.0, 0.0, 0.0, 1);
this.intake = intake;
this.targetAngle = targetAngle;
addRequirements(intake);
}
@Override
public double getError() {
return targetAngle - (((intake.getEncoder().getPosition()) * (360))%360);
}
@Override
public void runWithOutput(double output) {
intake.setVoltage(output / Math.abs(getError()));
}
}
src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java
+1 -1
View File
@@ -102,7 +102,7 @@ public class JoystickPlayback extends Command {
return;
}
lastIndex = i;
}
} // Why is this done rather than using the variable counter
TimedOutput lastOut = outputs.get(lastIndex - 1);
TimedOutput out = outputs.get(lastIndex);
src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java
+51 -1
View File
@@ -12,33 +12,66 @@ import java.util.function.Supplier;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.commands.Intake.ArmIntakeIn;
import frc4388.robot.subsystems.Intake;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.UtilityStructs.TimedOutput;
public class JoystickRecorder extends Command {
public final SwerveDrive swerve;
public final Shooter m_robotShooter;
public final Intake m_robotIntake;
public final Supplier<Double> leftX;
public final Supplier<Double> leftY;
public final Supplier<Double> rightX;
public final Supplier<Double> rightY;
public final Supplier<Boolean> OPLB;
public final Supplier<Boolean> OPRB;
private Command intakeToShootStuff;
private Command intakeToShoot;
private Command i;
private boolean lastOPLB;
private boolean lastOPRB;
private String filename;
public final ArrayList<TimedOutput> outputs = new ArrayList<>();
private long startTime = -1;
/** Creates a new JoystickRecorder. */
public JoystickRecorder(SwerveDrive swerve, Supplier<Double> leftX, Supplier<Double> leftY,
public JoystickRecorder(SwerveDrive swerve, Shooter m_robotShooter, Intake m_robotIntake,
Supplier<Double> leftX, Supplier<Double> leftY,
Supplier<Double> rightX, Supplier<Double> rightY,
Supplier<Boolean> OPLB, Supplier<Boolean> OPRB,
String filename)
{
this.swerve = swerve;
this.m_robotShooter = m_robotShooter;
this.m_robotIntake = m_robotIntake;
this.leftX = leftX;
this.leftY = leftY;
this.rightX = rightX;
this.rightY = rightY;
this.OPLB = OPLB;
this.OPRB = OPRB;
this.filename = filename;
intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter);
intakeToShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.pidIn()),
new InstantCommand(() -> m_robotShooter.spin())
);
i = new SequentialCommandGroup(
intakeToShootStuff, intakeToShoot
);
addRequirements(this.swerve);
}
@@ -60,6 +93,8 @@ public class JoystickRecorder extends Command {
inputs.leftY = leftY.get();
inputs.rightX = rightX.get();
inputs.rightY = rightY.get();
inputs.OPLB = OPLB.get();
inputs.OPRB = OPRB.get();
inputs.timedOffset = System.currentTimeMillis() - startTime;
outputs.add(inputs);
@@ -67,8 +102,23 @@ public class JoystickRecorder extends Command {
swerve.driveWithInput(new Translation2d(inputs.leftX, inputs.leftY),
new Translation2d(inputs.rightX, inputs.rightY),
true);
if(lastOPLB != inputs.OPLB && inputs.OPLB == true){
m_robotShooter.spin();
m_robotIntake.handoff();
}else if(lastOPLB != inputs.OPLB && inputs.OPLB == false){
}
if(lastOPRB != inputs.OPRB){
m_robotShooter.spin();
m_robotIntake.handoff();
}
System.out.println("RECORDING");
lastOPLB = inputs.OPLB;
lastOPRB = inputs.OPRB;
}
// Called once the command ends or is interrupted.
src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java
+145
View File
@@ -0,0 +1,145 @@
package frc4388.robot.commands.Swerve;
import java.io.FileInputStream;
import java.util.ArrayList;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.DataUtils;
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
import frc4388.utility.controller.VirtualController;
public class neoJoystickPlayback extends Command {
private final SwerveDrive swerve;
private final String filename;
private final VirtualController[] controllers;
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
private int frame_index = 0;
private long startTime = 0;
private long playbackTime = 0;
private boolean m_finished = false; // ! There is no better way.
private boolean m_shouldfree = false; // should free memory on ending
private byte m_numAxes = 0;
private byte m_numPOVs = 0;
private byte m_numControllers = 0;
private short m_numFrames = -1;
public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
this.swerve = swerve;
this.filename = filename;
this.controllers = controllers;
this.m_shouldfree = shouldfree;
if (instantload) loadAuto();
addRequirements(this.swerve);
}
public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) {
this(swerve, filename, controllers, true, false);
}
public boolean loadAuto() {
try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) {
if (m_numFrames != -1 && m_numFrames == frames.size()) {
System.out.println("AUTOPLAYBACK: Auto Already loaded.");
return true;
}
m_numAxes = stream.readNBytes(1)[0];
m_numPOVs = stream.readNBytes(1)[0];
m_numControllers = stream.readNBytes(1)[0];
m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2));
if (m_numControllers > controllers.length) {
System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers
+ " virtual controllers but only " + controllers.length + " were given");
return false;
}
for (int i = 0; i < m_numFrames; i++) {
AutoRecordingFrame frame = new AutoRecordingFrame();
for (int j = 0; j < m_numControllers; j++) {
AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
double[] axes = new double[m_numAxes];
for (int k = 0; k < m_numAxes; k++) { // we love third level for loops.
axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8));
}
short button = DataUtils.byteArrayToShort(stream.readNBytes(2));
short[] POV = new short[m_numPOVs];
for (int k = 0; k < m_numPOVs; k++) {
POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2));
}
controllerFrame.axes = axes;
controllerFrame.button = button;
controllerFrame.POV = POV;
frame.controllerFrames[j] = controllerFrame;
}
frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4));
frames.add(frame);
}
System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long");
return true;
} catch (Exception e) {
e.printStackTrace();
System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`');
return false;
}
}
public void unloadAuto() {
System.out.println("AUTOPLAYBACK: Auto unloaded");
frames.clear();
}
@Override
public void initialize() {
startTime = System.currentTimeMillis();
playbackTime = 0;
frame_index = 0;
m_finished = !loadAuto();
}
@Override
public void execute() {
if (frame_index >= m_numFrames) m_finished = true;
if (m_finished) return;
// if (frame_index == 0) {
// startTime = System.currentTimeMillis();
// playbackTime = 0;
// } else {
// playbackTime = System.currentTimeMillis() - startTime;
// }
AutoRecordingFrame frame = frames.get(frame_index);
for (int i = 0; i < controllers.length; i++) {
AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i];
controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV);
if (i == 0) {
this.swerve.driveWithInput(
new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)),
new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)),
true);
}
}
frame_index++;
}
@Override
public void end(boolean interrupted) {
for (VirtualController controller : controllers) controller.zeroControls();
swerve.stopModules();
if (m_shouldfree) frames.clear();
}
@Override
public boolean isFinished() {
return m_finished;
}
}
src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java
+105
View File
@@ -0,0 +1,105 @@
package frc4388.robot.commands.Swerve;
import java.io.FileOutputStream;
import java.util.ArrayList;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.DataUtils;
import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
import frc4388.utility.UtilityStructs.AutoRecordingFrame;
import frc4388.utility.controller.DeadbandedXboxController;
public class neoJoystickRecorder extends Command {
private final SwerveDrive swerve;
private final XboxController[] controllers;
private final String filename;
private long startTime = -1;
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) {
this.swerve = swerve;
this.controllers = controllers;
this.filename = filename;
addRequirements(this.swerve);
}
@Override
public void initialize() {
frames.clear();
this.startTime = System.currentTimeMillis();
AutoRecordingFrame frame = new AutoRecordingFrame();
frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()};
frames.add(frame);
}
@Override
public void execute() {
System.out.println("AUTORECORD: RECORDING");
AutoRecordingFrame frame = new AutoRecordingFrame();
frame.timeStamp = (int) (System.currentTimeMillis() - startTime);
for (int i = 0; i < controllers.length; i++) {
XboxController controller = controllers[i];
AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
double[] axes = {controller.getLeftX(), controller.getLeftY(),
controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(),
controller.getRightX(), controller.getRightY()};
short button = 0;
for (int j = 0; j < 10; j++)
if (controller.getRawButton(j+1))
button |= 1 << j;
short[] POV = {(short)(controller.getPOV())};
controllerFrame.axes = axes;
controllerFrame.button = button;
controllerFrame.POV = POV;
frame.controllerFrames[i] = controllerFrame;
}
frames.add(frame);
swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]),
new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]),
true); // Really jank way of doing this.
}
@Override
public void end(boolean interrupted) {
try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) {
// header: size of 0x5
// byte Number of axes per controller
// byte Number of POVs per controller
// byte Number of controllers
// short Number of frames
stream.write(new byte[]{6, 1, (byte) controllers.length});
stream.write(DataUtils.shortToByteArray((short) frames.size()));
// frame
// controller frame * number of controllers
// int unix time stamp.
for (AutoRecordingFrame frame : frames) {
// controller frame
// double axis * Number of axes per controller
// short button states
// short POV * Number of POVs per controller
for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) {
for (double axis: controllerFrame.axes) {
stream.write(DataUtils.doubleToByteArray(axis));
}
stream.write(DataUtils.shortToByteArray(controllerFrame.button));
for (short POV: controllerFrame.POV) {
stream.write(DataUtils.shortToByteArray(POV));
}
}
stream.write(DataUtils.intToByteArray(frame.timeStamp));
}
System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long.");
} catch (Exception e) {
e.printStackTrace();
}
}
}
src/main/java/frc4388/robot/subsystems/Intake.java
+131 -18
View File
@@ -6,6 +6,20 @@ package frc4388.robot.subsystems;
import java.util.function.BooleanSupplier;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.ctre.phoenix.motorcontrol.can.TalonSRXPIDSetConfiguration;
import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
import com.ctre.phoenix6.signals.ForwardLimitValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
import com.ctre.phoenix6.signals.ReverseLimitValue;
import com.revrobotics.CANSparkBase;
import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkLimitSwitch;
@@ -13,6 +27,7 @@ import com.revrobotics.SparkPIDController;
import com.revrobotics.RelativeEncoder;
import edu.wpi.first.wpilibj.CAN;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.motorcontrol.Talon;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants;
@@ -22,24 +37,34 @@ import frc4388.utility.Gains;
public class Intake extends SubsystemBase {
//NEO
private CANSparkMax intakeMotor;
private CANSparkMax pivot;
private SparkPIDController m_spedController;
private static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS;
private SparkLimitSwitch forwardLimit;
private SparkLimitSwitch reverseLimit;
private SparkLimitSwitch intakeforwardLimit;
private SparkLimitSwitch intakereverseLimit;
private Shooter shooter;
//Talon
private TalonFX talonIntake;
private TalonFX talonPivot;
private CANcoder encoder;
private HardwareLimitSwitchConfigs l;
TalonFXConfiguration doodooController = new TalonFXConfiguration();
public static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS;
private BooleanSupplier sup = () -> true;
private BooleanSupplier dup = () -> false;
private double smartDashboardOuttakeValue;
/** Creates a new Intake. */
//For NEO
public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) {
this.intakeMotor = intakeMotor;
this.pivot = pivot;
@@ -53,9 +78,6 @@ public class Intake extends SubsystemBase {
reverseLimit.enableLimitSwitch(true);
intakeMotor.restoreFactoryDefaults();
intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast);
intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
@@ -67,8 +89,79 @@ public class Intake extends SubsystemBase {
m_spedController.setP(armGains.kP);
m_spedController.setI(armGains.kI);
m_spedController.setD(armGains.kD);
SmartDashboard.putNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
}
//For Talon
public Intake(TalonFX talonIntake, TalonFX talonPivot) {
this.talonIntake = talonIntake;
this.talonPivot = talonPivot;
talonIntake.getConfigurator().apply(new TalonFXConfiguration());
talonPivot.getConfigurator().apply(new TalonFXConfiguration());
talonIntake.setNeutralMode(NeutralModeValue.Brake);
talonPivot.setNeutralMode(NeutralModeValue.Brake);
// talonPivot.getConfigurator().apply(new HardwareLimitSwitchConfigs());
// talonIntake.getConfigurator().apply(new HardwareLimitSwitchConfigs());
// doodooController.Slot0.kP = armGains.kP;
// doodooController.Slot1.kI = armGains.kI;
// doodooController.Slot2.kD = armGains.kD;
// in init function, set slot 0 gains
var slot0Configs = new Slot0Configs();
slot0Configs.kP = 0.05; // An error of 0.5 rotations results in 12 V output
slot0Configs.kI = 0.0; // no output for integrated error
slot0Configs.kD = 0.0; // A velocity of 1 rps results in 0.1 V output
talonPivot.getConfigurator().apply(slot0Configs);
}
// ! Talon Methods
public void talonPIDIn() {
PositionVoltage request = new PositionVoltage(0).withSlot(0);
talonPivot.setControl(request.withPosition(53)); //TODO: Find actual value
}
public void talonPIDOut() {
PositionVoltage request = new PositionVoltage(53).withSlot(53);
talonPivot.setControl(request.withPosition(0)); //TODO: Find actual value
}
public void talonHandoff() {
talonIntake.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
}
public void talonSpinIntakeMotor() {
talonIntake.set(IntakeConstants.INTAKE_SPEED);
}
public boolean getTalonIntakeLimitSwitchState() {
var r = talonIntake.getForwardLimit().getValue().value == 1;
return r;
}
public void talonSetPivotEncoderPosition(int val) {
talonPivot.setPosition(val);
}
public void talonStopIntakeMotors() {
talonIntake.set(0);
}
public void talonStopArmMotor() {
talonPivot.set(0);
}
// ! NEO Methods
//hanoff
public void spinIntakeMotor() {
intakeMotor.set(IntakeConstants.INTAKE_SPEED);
@@ -76,7 +169,6 @@ public class Intake extends SubsystemBase {
//Rotate robot in for handoff
public void rotateArmIn() {
//pivot.set(IntakeConstants.PIVOT_SPEED);
pivot.set(IntakeConstants.PIVOT_SPEED);
}
@@ -90,6 +182,10 @@ public class Intake extends SubsystemBase {
m_spedController.setReference(2.5, CANSparkMax.ControlType.kPosition);
//SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity());
}
public void pidOut() {
m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition);
}
public void limitNote() {
if (intakeforwardLimit.isPressed()) {
@@ -99,10 +195,6 @@ public class Intake extends SubsystemBase {
}
}
public void pidOut() {
m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition);
}
public void rotateArmOut2() {
if(reverseLimit.isPressed()){
stopArmMotor();
@@ -120,7 +212,15 @@ public class Intake extends SubsystemBase {
}
public void handoff() {
intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED);
intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
}
public void handoff2() {
if(intakeforwardLimit.isPressed()) {
intakeMotor.set(-smartDashboardOuttakeValue);
} else {
intakeMotor.set(-smartDashboardOuttakeValue);
}
}
public void stopIntakeMotors() {
@@ -155,13 +255,13 @@ public class Intake extends SubsystemBase {
return pivot.getEncoder().getVelocity();
}
public void resetPostion() {
pivot.getEncoder().setPosition(0);
public void setPivotEncoderPosition(int val) {
pivot.getEncoder().setPosition(val);
}
public void resetPosition1() {
public void resetPosition() {
if(forwardLimit.isPressed()) {
resetPostion();
setPivotEncoderPosition(0);
}
}
@@ -169,6 +269,10 @@ public class Intake extends SubsystemBase {
return pivot.getEncoder().getPosition();
}
public double getIntakeVelocity() {
return intakeMotor.getEncoder().getVelocity();
}
public void rotateArm() {
}
@@ -181,11 +285,20 @@ public class Intake extends SubsystemBase {
}
}
public void changeIntakeNeutralState() {
if(forwardLimit.isPressed()) {
intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast);
}
}
@Override
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Vel Output", getVelocity());
SmartDashboard.putNumber("Position", getPos());
resetPosition1();
resetPosition();
changeIntakeNeutralState();
smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
}
}
src/main/java/frc4388/robot/subsystems/LED.java
+1 -1
View File
@@ -33,7 +33,7 @@ public class LED extends SubsystemBase {
if(m_self != null)
return;
m_led = new AddressableLED(9);
m_ledBuffer = new AddressableLEDBuffer(130);
m_ledBuffer = new AddressableLEDBuffer(10);
m_led.setLength(m_ledBuffer.getLength());
m_led.setData(m_ledBuffer);
m_led.start();
src/main/java/frc4388/robot/subsystems/Shooter.java
+27 -20
View File
@@ -5,8 +5,10 @@
package frc4388.robot.subsystems;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.motorcontrol.Talon;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.Constants.ShooterConstants;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
@@ -20,15 +22,6 @@ public class Shooter extends SubsystemBase {
private TalonFX leftShooter;
private TalonFX rightShooter;
private Limelight limelight;
// 0 = Stop
// 1 = Idle, no limelight
// 2 = limelight
// 3 = Shooting
private int shooterMode;
/** Creates a new Shooter. */
public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight limelight) {
leftShooter = leftTalonFX;
@@ -38,16 +31,35 @@ public class Shooter extends SubsystemBase {
leftShooter.setNeutralMode(NeutralModeValue.Coast);
rightShooter.setNeutralMode(NeutralModeValue.Coast);
SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
}
public Shooter(TalonFX leftShooter) {
this.leftShooter = leftShooter;
leftShooter.setNeutralMode(NeutralModeValue.Coast);
}
public void singleSpin() {
leftShooter.set(1.0);
}
public void singleSpin(double speed) {
leftShooter.set(speed);
}
public void spin() {
shooterMode = 3;
spin(ShooterConstants.SHOOTER_SPEED);
spin(smartDashboardShooterSpeed);
}
public void spin(double speed) {
leftShooter.set(-speed);
rightShooter.set(speed);
rightShooter.set(-speed);
}
public void spin(double leftSpeed, double rightSpeed) {
leftShooter.set(leftSpeed);
rightShooter.set(-rightSpeed);
}
public void stop() {
@@ -68,15 +80,10 @@ public class Shooter extends SubsystemBase {
@Override
public void periodic() {
// This method will be called once per scheduler run
// SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue());
//SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue());
smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
if(limelight.isNearSpeaker() && shooterMode == 0){
shooterMode = 1;
spin(ShooterConstants.SHOOTER_IDLE_LIMELIGHT);
}
SmartDashboard.putNumber("Shooter Speed mode", shooterMode);
SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue());
SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue());
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+23 -2
View File
@@ -4,6 +4,7 @@
package frc4388.robot.subsystems;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -35,6 +36,7 @@ public class SwerveDrive extends SubsystemBase {
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
public double rotTarget = 0.0;
public Rotation2d orientRotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
/** Creates a new SwerveDrive. */
@@ -55,10 +57,11 @@ public class SwerveDrive extends SubsystemBase {
double rot = 0;
// ! drift correction
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
SmartDashboard.putBoolean("drift correction", false);
// SmartDashboard.putBoolean("drift correction", false);
stopped = false;
} else if(leftStick.getNorm() > 0.05) {
if (!stopped) {
@@ -66,7 +69,7 @@ public class SwerveDrive extends SubsystemBase {
stopped = true;
}
SmartDashboard.putBoolean("drift correction", true);
// SmartDashboard.putBoolean("drift correction", true);
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
@@ -84,6 +87,24 @@ public class SwerveDrive extends SubsystemBase {
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if(fieldRelative) {
double rot = 0;
if(rightStick.getNorm() > 0.5) {
orientRotTarget = rightStick.getAngle().minus(new Rotation2d(0, 1));
rot = orientRotTarget.minus(gyro.getRotation2d()).getRadians();
}
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
} else { // Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
}
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
}
/**
* Set each module of the swerve drive to the corresponding desired state.
* @param desiredStates Array of module states to set.