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

added climber stuff and tweaked auto stuff (im tweakin)

Browse Source
This commit is contained in:
Abhishrek05
2024-03-09 14:31:26 -07:00
parent e1b0864c28
commit be29d00336
9 changed files with 136 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+8 -2
View File
@@ -171,8 +171,8 @@ public final class Constants {
public static final int LEFT_SHOOTER_ID = 15; // final
public static final int RIGHT_SHOOTER_ID = 16; // final
public static final double SHOOTER_SPEED = 0.50; // final
public static final double SHOOTER_IDLE = 0.35; // final
public static final double SHOOTER_IDLE_LIMELIGHT = 0.8;
public static final double SHOOTER_IDLE = 0.15; // final
public static final double SHOOTER_IDLE_LIMELIGHT = 0.20;
}
public static final class IntakeConstants {
@@ -189,6 +189,12 @@ public final class Constants {
}
}
public static final class ClimbConstants {
public static final int CLIMB_MOTOR_ID = 0;
public static final double CLIMB_OUT_SPEED = 1.0;
public static final double CLIMB_IN_SPEED = -1.0;
}
public static final class OIConstants {
public static final int XBOX_DRIVER_ID = 0;
public static final int XBOX_OPERATOR_ID = 1;
src/main/java/frc4388/robot/Robot.java
+2 -1
View File
@@ -43,7 +43,7 @@ public class Robot extends TimedRobot {
/**
* This function is called every robot packet, no matter the mode. Use
* this for items like diagnostics that you want ran during disabled,
* autonomous, teleoperated and test.
* autonomous, teleoperated and test.doubl
*
* <p>This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
@@ -51,6 +51,7 @@ public class Robot extends TimedRobot {
@Override
public void robotPeriodic() {
m_robotTime.updateTimes();
System.out.println(m_robotContainer.limelight.isNearSpeaker());
//mled.updateLED();
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
src/main/java/frc4388/robot/RobotContainer.java
+31 -12
View File
@@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.commands.Autos.AutoAlign;
import frc4388.robot.commands.Autos.PlaybackChooser;
@@ -32,8 +33,10 @@ import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Limelight;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.Climber;
import frc4388.robot.subsystems.Intake;
import frc4388.utility.DeferredBlock;
import frc4388.utility.configurable.ConfigurableString;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.VirtualController;
import frc4388.utility.controller.XboxController;
@@ -59,12 +62,14 @@ public class RobotContainer {
m_robotMap.gyro);
/* Limelight */
private final Limelight limelight = new Limelight();
public final Limelight limelight = new Limelight();
private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter);
private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter, limelight);
private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor);
private final Climber m_robotClimber = new Climber(m_robotMap.climbMotor);
//private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor);
/* Controllers */
@@ -115,7 +120,8 @@ public class RobotContainer {
private SequentialCommandGroup i = new SequentialCommandGroup(
intakeToShootStuff, intakeToShoot
intakeToShootStuff, intakeToShoot,
new InstantCommand(() -> m_robotShooter.idle())
);
private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup(
@@ -145,6 +151,7 @@ public class RobotContainer {
private Command startLeftMoveRight = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartLeftMoveRight.txt");
private Command startRightMoveLeft = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt");
private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", "defualt.auto");
//Help Simplify Shooting
// private SequentialCommandGroup pullInArmtoShoot = new SequentialCommandGroup(
@@ -316,6 +323,14 @@ public class RobotContainer {
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive));
new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5)
.onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))
.onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5)
.onTrue(new InstantCommand(() -> m_robotClimber.climbIn()))
.onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
// ! /* Auto Recording */
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
@@ -334,29 +349,29 @@ public class RobotContainer {
new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
.whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
"Nuetral_Center_1note_taxi.auto"))
() -> autoplaybackName.get()))
.onFalse(new InstantCommand());
new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
"Nuetral_Center_1note_taxi.auto",
autoplaybackName.get(),
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
true, false))
.onFalse(new InstantCommand());
// ! /* Speed */
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
// 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.Y_BUTTON)
.whileTrue(new InstantCommand(() ->
m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
new Translation2d(0, 0),
true), m_robotSwerveDrive));
// new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
// .whileTrue(new InstantCommand(() ->
// m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
// new Translation2d(0, 0),
// true), m_robotSwerveDrive));
//? /* Operator Buttons */
@@ -407,6 +422,10 @@ public class RobotContainer {
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_JOYSTICK_BUTTON)
.onTrue(emergencyRetract);
new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5)
.onTrue(new InstantCommand(() -> m_robotShooter.idle()))
.onFalse(new InstantCommand(() -> m_robotShooter.stop()));
}
src/main/java/frc4388/robot/RobotMap.java
+4
View File
@@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj.motorcontrol.Talon;
import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.ClimbConstants;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.subsystems.SwerveModule;
import frc4388.utility.RobotGyro;
@@ -74,6 +75,9 @@ public class RobotMap {
public final TalonFX intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID);
public final TalonFX pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID);
/* Climber Subsystem */
public final TalonFX climbMotor = new TalonFX(ClimbConstants.CLIMB_MOTOR_ID);
void configureLEDMotorControllers() {
}
src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java
+4 -2
View File
@@ -2,6 +2,7 @@ package frc4388.robot.commands.Swerve;
import java.io.FileInputStream;
import java.util.ArrayList;
import java.util.function.Supplier;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
@@ -13,15 +14,16 @@ 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 final Supplier<String> filenameGetter;
private final String filename;
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;
src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java
+12 -4
View File
@@ -2,6 +2,7 @@ package frc4388.robot.commands.Swerve;
import java.io.FileOutputStream;
import java.util.ArrayList;
import java.util.function.Supplier;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.XboxController;
@@ -15,18 +16,24 @@ 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 String filename;
private final Supplier<String> filenameGetter;
private long startTime = -1;
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) {
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier<String> filenameGetter) {
this.swerve = swerve;
this.controllers = controllers;
this.filename = filename;
this.filenameGetter = filenameGetter;
this.filename = "";
addRequirements(this.swerve);
}
public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) {
this(swerve, controllers, () -> filename);
}
@Override
public void initialize() {
frames.clear();
@@ -35,6 +42,7 @@ public class neoJoystickRecorder extends Command {
AutoRecordingFrame frame = new AutoRecordingFrame();
frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()};
frames.add(frame);
this.filename = this.filenameGetter.get();
}
src/main/java/frc4388/robot/subsystems/Climber.java
+39
View File
@@ -0,0 +1,39 @@
// 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.robot.subsystems;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants;
import frc4388.robot.Constants.ClimbConstants;
//! 6.5C Scoring Criteria for Stage
public class Climber extends SubsystemBase {
/** Creates a new Climber. */
TalonFX climbMotor;
public Climber(TalonFX climbMotor) {
this.climbMotor = climbMotor;
}
public void climbOut() {
climbMotor.set(Constants.ClimbConstants.CLIMB_OUT_SPEED);
}
public void climbIn() {
climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
}
public void stopClimb() {
climbMotor.set(0.d);
}
@Override
public void periodic() {
// This method will be called once per scheduler run
}
}
src/main/java/frc4388/robot/subsystems/Limelight.java
+1 -1
View File
@@ -71,7 +71,7 @@ public class Limelight extends SubsystemBase {
public void periodic() {
// This method will be called once per scheduler run
isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getBoolean(false);
isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0;
double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]);
if(newPose != cameraPose){
src/main/java/frc4388/robot/subsystems/Shooter.java
+35 -3
View File
@@ -11,6 +11,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.IntakeConstants;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.subsystems.Limelight;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.NeutralModeValue;
@@ -22,13 +24,24 @@ public class Shooter extends SubsystemBase {
private TalonFX leftShooter;
private TalonFX rightShooter;
private Limelight limelight;
private int spinMode = 0;
// 0 = Stop / Coast
// 1 = Idle
// 2 = Idle Near Speaker
// 3 = Spin
// 4 = SingleSpin
private double smartDashboardShooterSpeed;
/** Creates a new Shooter. */
public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX) {
public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight tmplimelight) {
leftShooter = leftTalonFX;
rightShooter = rightTalonFX;
limelight = tmplimelight;
leftShooter.setNeutralMode(NeutralModeValue.Coast);
rightShooter.setNeutralMode(NeutralModeValue.Coast);
SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
@@ -42,32 +55,43 @@ public class Shooter extends SubsystemBase {
public void singleSpin() {
leftShooter.set(1.0);
spinMode = 4;
}
public void singleSpin(double speed) {
leftShooter.set(speed);
spinMode = 4;
}
public void spin() {
spin(smartDashboardShooterSpeed);
spinMode = 3;
}
public void spin(double speed) {
leftShooter.set(-speed);
rightShooter.set(-speed);
spinMode = 3;
}
public void spin(double leftSpeed, double rightSpeed) {
leftShooter.set(leftSpeed);
rightShooter.set(-rightSpeed);
spinMode = 3;
}
public void stop() {
spin(0.d);
spinMode = 0;
}
public void idle() {
spin(ShooterConstants.SHOOTER_IDLE);
spin(ShooterConstants.SHOOTER_IDLE);
spinMode = 1;
}
public int getMode(){
return spinMode;
}
@Override
@@ -77,6 +101,14 @@ public class Shooter extends SubsystemBase {
//SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue());
smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
// If the robot is near the speaker, and is stopped, or idled, set to limelight idle speed.
// Else if the robot is not near the speaker, then set the speed back to idle.
if(limelight.isNearSpeaker() && (spinMode == 0 || spinMode == 1)){
leftShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT);
rightShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT);
spinMode = 2;
}else if(!limelight.isNearSpeaker() && spinMode == 2){
idle();
}
}
}