Skip to content

Commit

Permalink
Merge branch 'main' into testing
Browse files Browse the repository at this point in the history
  • Loading branch information
ReeledWarrior14 authored Mar 7, 2024
2 parents 9454a93 + cc5a216 commit 41d08e7
Show file tree
Hide file tree
Showing 10 changed files with 121 additions and 71 deletions.
8 changes: 6 additions & 2 deletions .vscode/settings.json
Original file line number Diff line number Diff line change
@@ -1,4 +1,6 @@
{
"editor.insertSpaces": true,
"editor.tabSize": 2,
"java.configuration.updateBuildConfiguration": "automatic",
"java.server.launchMode": "Standard",
"files.exclude": {
Expand All @@ -18,9 +20,11 @@
{
"name": "WPIlibUnitTests",
"workingDirectory": "${workspaceFolder}/build/jni/release",
"vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ],
"vmargs": [
"-Djava.library.path=${workspaceFolder}/build/jni/release"
],
"env": {
"LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" ,
"LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release",
"DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release"
}
},
Expand Down
20 changes: 11 additions & 9 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -120,16 +120,18 @@ public static final class ShooterConstants {
public static final double kShooterSpeedTop = 0.75;
public static final double kShooterSpeedBottom = 0.9;
public static final double kShooterOff = 0;
}
}

public static class ClimberConstants {
public final static int leftForwardChannel = 0;
public final static int rightForwardChannel = 0;
public final static int leftReverseChannel = 1;
public final static int rightReverseChannel = 1;

public final static double minPressure = 50.0;
public final static double maxPressure = 120.0;
}
public final static int leftForwardChannel = 0;
public final static int rightForwardChannel = 0;
public final static int leftReverseChannel = 1;
public final static int rightReverseChannel = 1;

public final static double minPressure = 50.0;
public final static double maxPressure = 120.0;
}

public static final class VisionConstants {
// TODO: Update cam pose relative to center of bot
public static final Pose3d kCamPose = new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0));
Expand Down
69 changes: 40 additions & 29 deletions src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -4,18 +4,15 @@

package frc.robot;


import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.subsystems.ClimberSubsystem;

/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
* the package after creating this project, you must also update the build.gradle file in the
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the TimedRobot
* documentation. If you change the name of this class or the package after
* creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
Expand All @@ -25,47 +22,54 @@ public class Robot extends TimedRobot {

// public ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();

private Timer m_buttonTimer = new Timer();
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
* This function is run when the robot is first started up and should be used
* for any initialization code.
*/
@Override
public void robotInit() {
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
// autonomous chooser on the dashboard.
// Instantiate our RobotContainer. This will perform all our button bindings,
// and put our autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();
}

/**
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
* that you want ran during disabled, autonomous, teleoperated and test.
* This function is called every 20 ms, no matter the mode. Use this for items
* like diagnostics that you want ran during disabled, autonomous, teleoperated
* and test.
*
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
* <p>
* This runs after the mode specific periodic functions, but before LiveWindow
* and SmartDashboard integrated updating.
* </p>
*/
@Override
public void robotPeriodic() {
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// and running subsystem periodic() methods. This must be called from the robot's periodic
// block in order for anything in the Command-based framework to work.
// Runs the Scheduler. This is responsible for polling buttons, adding
// newly-scheduled commands, running already-scheduled commands, removing
// finished or interrupted commands, and running subsystem periodic() methods.
// This must be called from the robot's periodic block in order for anything in
// the Command-based framework to work.
CommandScheduler.getInstance().run();
}

/** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {}
public void disabledInit() {
}

@Override
public void disabledPeriodic() {
// if (RobotController.getUserButton() && m_buttonTimer.get() > 1) {
// m_climberSubsystem.toggleCompressor();
// m_buttonTimer.reset();
// m_climberSubsystem.toggleCompressor();
// m_buttonTimer.reset();
// }
}

/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
/**
* This autonomous runs the autonomous command selected by your
* {@link RobotContainer} class.
*/
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
Expand All @@ -78,7 +82,8 @@ public void autonomousInit() {

/** This function is called periodically during autonomous. */
@Override
public void autonomousPeriodic() {}
public void autonomousPeriodic() {
}

@Override
public void teleopInit() {
Expand All @@ -89,11 +94,14 @@ public void teleopInit() {
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}

m_robotContainer.resetAllSubsystems();
}

/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {}
public void teleopPeriodic() {
}

@Override
public void testInit() {
Expand All @@ -103,13 +111,16 @@ public void testInit() {

/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {}
public void testPeriodic() {
}

/** This function is called once when the robot is first started up. */
@Override
public void simulationInit() {}
public void simulationInit() {
}

/** This function is called periodically whilst in simulation. */
@Override
public void simulationPeriodic() {}
public void simulationPeriodic() {
}
}
32 changes: 20 additions & 12 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -5,21 +5,17 @@
package frc.robot;

import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.path.PathPlannerPath;
import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
import com.pathplanner.lib.util.PIDConstants;
import com.pathplanner.lib.util.ReplanningConfig;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.XboxController.Button;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
Expand All @@ -30,12 +26,11 @@
import frc.robot.commands.NoteIntakeCommand;
import frc.robot.commands.NoteOuttakeCommand;
import frc.robot.commands.ShooterSetSpeedCommand;
import frc.robot.subsystems.ClimberSubsystem;
import frc.robot.subsystems.DriveSubsystem;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.IntakeSubsystem.ArmPosition;
import frc.robot.subsystems.ShooterSubsystem.ShootSpeed;
import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.ShooterSubsystem.ShootSpeed;
import frc.robot.subsystems.VisionSubsystem;

/*
Expand All @@ -53,7 +48,6 @@ public class RobotContainer {
private final VisionSubsystem m_visionSubsystem = new VisionSubsystem();

private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort);

/**
* The container for the robot. Contains subsystems, IO devices, and commands.
Expand All @@ -67,8 +61,10 @@ public RobotContainer() {
new PIDConstants(5, 0.0, 0.0), // Translation PID constants
new PIDConstants(5, 0.0, 0.0), // Rotation PID constants
DriveConstants.kMaxSpeedMetersPerSecond, // Max module speed, in m/s
Math.hypot(DriveConstants.kTrackWidth, DriveConstants.kWheelBase), // Drive base radius in meters. Distance
// from robot center to furthest module.
Math.hypot(DriveConstants.kTrackWidth, DriveConstants.kWheelBase), // Drive base radius in
// meters. Distance
// from robot center to
// furthest module.
new ReplanningConfig(true, true)),
() -> false, m_robotDrive);

Expand Down Expand Up @@ -122,13 +118,16 @@ private void configureBindings() {
// DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5)));

new JoystickButton(m_driverController, Button.kX.value)
.onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting), new NoteOuttakeCommand(m_intakeSubsystem)))
.onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting),
new NoteOuttakeCommand(m_intakeSubsystem)))
.onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off));

// new JoystickButton(m_operatorController, Button.kA.value)
// .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_climberSubsystem));
// .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(),
// m_climberSubsystem));
// new JoystickButton(m_operatorController, Button.kB.value)
// .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_climberSubsystem));
// .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(),
// m_climberSubsystem));

new Trigger(() -> {
return m_driverController.getLeftTriggerAxis() > 0.5;
Expand All @@ -148,6 +147,15 @@ private void configureBindings() {
.onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted));
}

/**
* Reset all subsystems on teleop init
*/
public void resetAllSubsystems() {
m_intakeSubsystem.reset();
m_shooterSubsystem.reset();
m_robotDrive.reset();
}

/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
Expand Down
7 changes: 4 additions & 3 deletions src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants.ShooterConstants;
import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.ShooterSubsystem.ShootSpeed;

Expand Down Expand Up @@ -37,11 +36,13 @@ public void initialize() {

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}
public void execute() {
}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
public void end(boolean interrupted) {
}

// Returns true when the command should end.
@Override
Expand Down
16 changes: 8 additions & 8 deletions src/main/java/frc/robot/subsystems/ClimberSubsystem.java
Original file line number Diff line number Diff line change
@@ -1,17 +1,17 @@
package frc.robot.subsystems;

import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kOff;
import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.PneumaticsModuleType;
import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
import edu.wpi.first.wpilibj.PneumaticsModuleType;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.ClimberConstants;

import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kOff;
import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;

public class ClimberSubsystem extends SubsystemBase{
public class ClimberSubsystem extends SubsystemBase {

private final DoubleSolenoid m_leftSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH,
ClimberConstants.leftForwardChannel, ClimberConstants.leftReverseChannel);
Expand All @@ -28,7 +28,7 @@ public ClimberSubsystem() {
m_compressorEnabled = false;
solenoidOff();
toggleCompressor();
}
}

// Runs once every tick (~20ms)
public void periodic() {
Expand Down Expand Up @@ -59,7 +59,7 @@ public void reverse() {

/**
* Toggles the state of the compressor (on/off)
*/
*/
public void toggleCompressor() {
m_compressorEnabled = !m_compressorEnabled;
if (m_compressorEnabled) {
Expand Down
5 changes: 5 additions & 0 deletions src/main/java/frc/robot/subsystems/DriveSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -110,6 +110,11 @@ public void periodic() {
setModuleStates(swerveModuleStates);
}

public void reset() {
swerveModuleStates = new SwerveModuleState[] { new SwerveModuleState(), new SwerveModuleState(),
new SwerveModuleState(), new SwerveModuleState() };
}

/**
* Returns the currently-estimated pose of the robot.
*
Expand Down
12 changes: 10 additions & 2 deletions src/main/java/frc/robot/subsystems/IntakeSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,14 @@ public IntakeSubsystem() {
m_armSetpoint = m_armEncoder.getDistance();
}

public void reset() {
m_intakeMotor.set(0);
m_armMotor.set(0);

m_intakeSpeed = 0;
m_armSetpoint = getDistanceSensor();
}

public void setArmPosition(ArmPosition position) {
switch (position) {
case Amp:
Expand All @@ -65,7 +73,7 @@ public void setArmPosition(ArmPosition position) {
m_armPID.setSetpoint(m_armSetpoint);
}

public double getArmPosition(){
public double getArmPosition() {
return m_armSetpoint;
}

Expand Down Expand Up @@ -96,7 +104,7 @@ public double getDistanceSensor() {
public void periodic() {
haveNote = getDistanceSensor() < IntakeConstants.kDistanceSensorThreshold;

//Note: negative because encoder goes from 0 to -193 cuz weird
// Note: negative because encoder goes from 0 to -193 cuz weird
double setMotorSpeed = MathUtil.clamp(m_armPID.calculate(m_armEncoder.getDistance(), m_armSetpoint), -0.4, 0.4);
m_armMotor.set(setMotorSpeed);
m_intakeMotor.set(m_intakeSpeed);
Expand Down
Loading

0 comments on commit 41d08e7

Please sign in to comment.