Skip to content

Commit

Permalink
added toggle bake mode
Browse files Browse the repository at this point in the history
  • Loading branch information
jack60612 committed Feb 5, 2024
1 parent 24bf893 commit 5a6ac87
Show file tree
Hide file tree
Showing 7 changed files with 135 additions and 62 deletions.
1 change: 1 addition & 0 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@ public static final class CANConstants {
}

public static final double MAX_SPEED = 0.8;
public static final double LIFTERSPEED = 0.5;

// USB Devices
public static final int CONTROLLERUSBINDEX = 0;
Expand Down
43 changes: 0 additions & 43 deletions src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -4,16 +4,11 @@

package frc.robot;

import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.cscore.UsbCamera;
import edu.wpi.first.cscore.VideoSink;
import edu.wpi.first.wpilibj.DataLogManager;
// import edu.wpi.first.cscore.VideoSource.ConnectionStrategy;
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;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import org.littletonrobotics.urcl.URCL;

/**
Expand All @@ -27,11 +22,6 @@ public class Robot extends TimedRobot {

private RobotContainer m_robotContainer;

private UsbCamera camera1;
private UsbCamera camera2;
private VideoSink mainCameraServer;
private int cameraCounter = 2;

/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
Expand All @@ -42,29 +32,10 @@ public void robotInit() {
// autonomous chooser on the dashboard.
m_robotContainer = new RobotContainer();

camera1 = CameraServer.startAutomaticCapture(0);
camera2 = CameraServer.startAutomaticCapture(1);
mainCameraServer = CameraServer.getServer();
// Tell both cameras to always stream.
// camera1.setConnectionStrategy(ConnectionStrategy.kKeepOpen);
// camera2.setConnectionStrategy(ConnectionStrategy.kKeepOpen);

// this is to put git info in the dashboard & Logs, uses new 2024 BuildConstants.java
String branchName = BuildConstants.GIT_BRANCH;
String commitHash = BuildConstants.GIT_SHA;
String commitTime = BuildConstants.GIT_DATE;
String buildTime = BuildConstants.BUILD_DATE;

System.out.println("Branch: " + branchName);
System.out.println("Commit: " + commitHash);
System.out.println("Commit Time: " + commitTime);
System.out.println("Build Time: " + buildTime);

SmartDashboard.putString("Branch", branchName);
SmartDashboard.putString("Commit", commitHash);
SmartDashboard.putString("CommitTime", commitTime);
SmartDashboard.putString("BuildTime", buildTime);

if (m_robotContainer.enableAutoProfiling) {
DataLogManager.start();
URCL.start();
Expand Down Expand Up @@ -121,20 +92,6 @@ public void teleopInit() {
if (m_autonomousCommand != null) {
m_autonomousCommand.cancel();
}
m_robotContainer
.getCameraButton()
.onTrue(
new InstantCommand(
() -> {
cameraCounter++;
if (cameraCounter % 2 == 0) {
System.out.println("Setting Camera 2");
mainCameraServer.setSource(camera2);
} else {
System.out.println("Setting Camera 1");
mainCameraServer.setSource(camera1);
}
}));
}

/** This function is called periodically during operator control. */
Expand Down
29 changes: 15 additions & 14 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,8 @@
import frc.robot.commands.ArmCommand;
import frc.robot.commands.BalanceCommand;
import frc.robot.commands.DefaultDrive;
import frc.robot.commands.LifterDownCommand;
import frc.robot.commands.LifterUpCommand;
import frc.robot.commands.ShooterCommand;
import frc.robot.commands.StraightCommand;
import frc.robot.commands.UltrasonicShooterCommand;
Expand Down Expand Up @@ -77,13 +79,16 @@ public class RobotContainer {
private final ArmCommand m_armCommand =
new ArmCommand(m_armSubsystem, m_shooterState, this::GetFlightStickY);
private final ShooterCommand m_shooterCommand = new ShooterCommand(m_shooterSubsytem);
private final LifterUpCommand m_lifterUpCommand = new LifterUpCommand(m_lifterSubsystem);
private final LifterDownCommand m_lifterDownCommand = new LifterDownCommand(m_lifterSubsystem);

private Command m_driveToSpeaker;
// Init Buttons
private Trigger m_switchCameraButton;
private Trigger m_balanceButton;
private Trigger m_straightButton;
private Trigger m_brakeButton;
private Trigger m_coastButton;
private Trigger m_toggleBrakeButton;
private Trigger m_lifterUpButton;
private Trigger m_lifterDownButton;
private JoystickButton m_aimButton;
private JoystickButton m_fireButton;
private Trigger m_driveToSpeakerButton;
Expand Down Expand Up @@ -125,12 +130,13 @@ public RobotContainer() {
*/
private void setupTriggers() {
// Controller buttons
m_switchCameraButton = m_controller1.x();
m_brakeButton = m_controller1.a();
m_coastButton = m_controller1.b();
m_toggleBrakeButton = m_controller1.x();
m_lifterUpButton = m_controller1.a();
m_lifterDownButton = m_controller1.b();
m_balanceButton = m_controller1.rightBumper();
m_straightButton = m_controller1.rightTrigger();
m_driveToSpeakerButton = m_controller1.y();

// Joystick buttons
m_aimButton = new JoystickButton(m_flightStick, Constants.AIMBUTTON);
m_fireButton = new JoystickButton(m_flightStick, Constants.FIREBUTTON);
Expand All @@ -143,9 +149,9 @@ private void bindCommands() {
m_aimButton.whileTrue(m_aimCommand);
m_fireButton.whileTrue(m_shooterCommand);
m_driveToSpeakerButton.whileTrue(m_driveToSpeaker);

m_brakeButton.whileTrue(new InstantCommand(() -> m_driveSubsystem.SetBrakemode()));
m_coastButton.whileTrue(new InstantCommand(() -> m_driveSubsystem.SetCoastmode()));
m_lifterUpButton.whileTrue(m_lifterUpCommand);
m_lifterDownButton.whileTrue(m_lifterDownCommand);
m_toggleBrakeButton.whileTrue(new InstantCommand(() -> m_driveSubsystem.SwitchBrakemode()));
}

private void bindDriveSysIDCommands() {
Expand Down Expand Up @@ -243,11 +249,6 @@ public DefaultDrive getDefaultDrive() {
return m_defaultDrive;
}

// to swap camera type.
public Trigger getCameraButton() {
return m_switchCameraButton;
}

// for future SmartDashboard uses.
public CommandXboxController getController1() {
return this.m_controller1;
Expand Down
51 changes: 51 additions & 0 deletions src/main/java/frc/robot/commands/LifterDownCommand.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
// 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 frc.robot.commands;

import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants;
import frc.robot.subsystems.LifterSubsystem;

/** Lifter Command using the Lifter Subsystem. */
public class LifterDownCommand extends Command {
@SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
private final LifterSubsystem m_lifterSubsystem;

private final double speed = Constants.LIFTERSPEED;

/**
* Creates a new LifterDownCommand.
*
* @param subsystem The subsystem used by this command.
*/
public LifterDownCommand(LifterSubsystem l_subsystem) {
m_lifterSubsystem = l_subsystem;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(l_subsystem);
}

// Called when the command is initially scheduled.
@Override
public void initialize() {}

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

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

// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
51 changes: 51 additions & 0 deletions src/main/java/frc/robot/commands/LifterUpCommand.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
// 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 frc.robot.commands;

import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants;
import frc.robot.subsystems.LifterSubsystem;

/** Lifter Command using the Lifter Subsystem. */
public class LifterUpCommand extends Command {
@SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
private final LifterSubsystem m_lifterSubsystem;

private final double speed = Constants.LIFTERSPEED;

/**
* Creates a new LifterUpCommand.
*
* @param l_subsystem The subsystem used by this command.
*/
public LifterUpCommand(LifterSubsystem l_subsystem) {
m_lifterSubsystem = l_subsystem;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(l_subsystem);
}

// Called when the command is initially scheduled.
@Override
public void initialize() {}

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

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

// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
13 changes: 13 additions & 0 deletions src/main/java/frc/robot/subsystems/DriveSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,9 @@ public class DriveSubsystem extends SubsystemBase {
private final SparkPIDController m_backLeftPIDController;
private final SparkPIDController m_backRightPIDController;

// Current Idle mode
private boolean isBrakeMode;

// motor feedforward
SimpleMotorFeedforward m_driveFeedForward =
new SimpleMotorFeedforward(
Expand Down Expand Up @@ -446,13 +449,23 @@ public void SetBrakemode() {
m_backRight.setIdleMode(CANSparkMax.IdleMode.kBrake);
m_frontLeft.setIdleMode(CANSparkMax.IdleMode.kBrake);
m_frontRight.setIdleMode(CANSparkMax.IdleMode.kBrake);
isBrakeMode = true;
}

public void SetCoastmode() {
m_backLeft.setIdleMode(CANSparkMax.IdleMode.kCoast);
m_backRight.setIdleMode(CANSparkMax.IdleMode.kCoast);
m_frontLeft.setIdleMode(CANSparkMax.IdleMode.kCoast);
m_frontRight.setIdleMode(CANSparkMax.IdleMode.kCoast);
isBrakeMode = false;
}

public void SwitchBrakemode() {
if (this.isBrakeMode) {
this.SetCoastmode();
} else {
this.SetBrakemode();
}
}

/**
Expand Down
9 changes: 4 additions & 5 deletions src/main/java/frc/robot/subsystems/LifterSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -11,27 +11,26 @@
public class LifterSubsystem extends SubsystemBase {
private final CANSparkMax m_left; // Motor for Left
private final CANSparkMax m_right; // Motor for right
private final double speed = 0.5;

/** Creates a new LifterSubsystem. */
public LifterSubsystem() {
m_left = new CANSparkMax(CANConstants.MOTORLIFTERLEFTID, CANSparkMax.MotorType.kBrushless);
m_right = new CANSparkMax(CANConstants.MOTORLIFTERRIGHTID, CANSparkMax.MotorType.kBrushless);
}

public void leftUp() {
public void leftUp(double speed) {
m_left.set(speed);
}

public void leftDown() {
public void leftDown(double speed) {
m_left.set(-speed);
}

public void rightUp() {
public void rightUp(double speed) {
m_right.set(speed);
}

public void rightDown() {
public void rightDown(double speed) {
m_right.set(-speed);
}

Expand Down

0 comments on commit 5a6ac87

Please sign in to comment.