Skip to content

Commit

Permalink
Merge pull request #234 from CyberCoyotes/event-ready
Browse files Browse the repository at this point in the history
Event ready
  • Loading branch information
JediScoy authored Mar 13, 2024
2 parents 1621707 + 508d2e0 commit fb79bac
Show file tree
Hide file tree
Showing 18 changed files with 75 additions and 70 deletions.
31 changes: 7 additions & 24 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -18,16 +18,14 @@
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.commands.IntakeCommandGroup;
import frc.robot.commands.IntakeIndex;
import frc.robot.commands.IntakeRevCommandGroup;
import frc.robot.commands.RevAndShootCommand;
import frc.robot.commands.RunShooter;
import frc.robot.commands.SetArmClimb;
import frc.robot.commands.SetIndex;
import frc.robot.commands.SetWinch;
import frc.robot.commands.ShootClose;
import frc.robot.commands.ShooterIndex;
import frc.robot.experimental.IntakeIndex;
import frc.robot.experimental.StopShooting;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.ArmSubsystem;
import frc.robot.subsystems.IndexSubsystem;
Expand Down Expand Up @@ -101,10 +99,10 @@ public RobotContainer() {
* Unintended side effect is this will create EVERY auton file from the RIO deploy folder.
* FTP into the the RIO to delete old auton options */
autoChooser = AutoBuilder.buildAutoChooser();
SmartDashboard.putData("Auto Chooser", autoChooser);
// SmartDashboard.putData("Auto Chooser", autoChooser);
Shuffleboard.getTab("Auton").add("Auto Chooser", autoChooser);

shooter = new ShooterSubsystemVelocity();
index.setDefaultCommand(index.run(() -> index.SetPower(BumperStatus(1))));

/* Configure the Button Bindings */
configureBindings();
Expand Down Expand Up @@ -163,7 +161,7 @@ private void configureBindings() {

};

/* Use for Debugging purposes */
/* Use for Debugging and diagnostics purposes */
public void DebugMethodSingle() {
// #region Driving

Expand All @@ -172,31 +170,16 @@ public void DebugMethodSingle() {

// Less useful logs that we still need to see for testing.

// var driverDiagnostics = Shuffleboard.getTab("Driver Diagnostics");
var driverDiagnostics = Shuffleboard.getTab("Driver Diagnostics");

driverDiagnostics.add(autoChooser);

driverDiagnostics.addBoolean("Note Detected", () -> index.HasCargo());
var driverDiagnostics = Shuffleboard.getTab("Diagnostics");

// driverDiagnostics.addDouble("Arm Rot", () -> arm.GetArmPos().getValueAsDouble());
// driverDiagnostics.addDouble("Arm Rot Deg", () -> arm.GetPositionDegrees());
// arm.showArmTelemetry("Driver Diagnostics");
// Shuffleboard.getTab("Arm").add("Arm Output", arm);

// #endregion Testing
}

/**
* Schizophrenic method to quickly get an int indicating driver bumper status. 1
* if RB,-1 if LB, 0 if both or none
*
* @param port controller port, 0 is driver, 1 is operaator
* @return an int indicating driver bumper status.
*/
private double BumperStatus(int port) {
return (Controllers[port].rightBumper().getAsBoolean() ? 1 : 0)
+ (Controllers[port].leftBumper().getAsBoolean() ? -1 : 0);
}

public Command getAutonomousCommand() {
return autoChooser.getSelected();
}
Expand Down
Original file line number Diff line number Diff line change
@@ -1,10 +1,8 @@
package frc.robot.experimental;
package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.Constants;
import frc.robot.commands.SetIndex;
import frc.robot.commands.SetIntake;
import frc.robot.subsystems.IndexSubsystem;
import frc.robot.subsystems.IntakeSubsystem;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ public class IntakeRevCommandGroup extends SequentialCommandGroup {
// LED to unloaded color
// Intake + Index in parallel.
// Note triggers the note sensor (ToF), turn off both intake and index,
// Set LED status to loaded (and shuffleboard)
// Set LED status to loaded and to dashboard

SetIntake runIntake;
IndexSubsystem index;
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/commands/RevAndShootCommand.java
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@

import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
import frc.robot.Constants;
import frc.robot.experimental.IncrementIndex;
// import frc.robot.experimental.IncrementIndex;
import frc.robot.subsystems.IndexSubsystem;
import frc.robot.subsystems.ShooterSubsystemVelocity;

Expand All @@ -23,14 +23,14 @@ public class RevAndShootCommand extends SequentialCommandGroup {
IndexSubsystem indexer;
ShooterSubsystemVelocity shooter;
// component Commands
private IncrementIndex outtaMyWay;
// private IncrementIndex outtaMyWay;
private Command revUpShooter;
private Command indexCommand;

public RevAndShootCommand(IndexSubsystem indexer) {
// Set up our subsystems
this.indexer = indexer;
outtaMyWay = new IncrementIndex(indexer);
// outtaMyWay = new IncrementIndex(indexer);
}


Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/commands/RevAndShootEndsCommand.java
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
import frc.robot.Constants;
import frc.robot.experimental.IncrementIndex;
// import frc.robot.experimental.IncrementIndex;
import frc.robot.subsystems.IndexSubsystem;
import frc.robot.subsystems.ShooterSubsystemVelocity;

Expand All @@ -23,14 +23,14 @@ public class RevAndShootEndsCommand extends SequentialCommandGroup {
IndexSubsystem indexer;
ShooterSubsystemVelocity shooter;
// component Commands
private IncrementIndex outtaMyWay;
// private IncrementIndex outtaMyWay;
private Command revUpShooter;
private Command indexCommand;

public RevAndShootEndsCommand(IndexSubsystem indexer) {
// Set up our subsystems
this.indexer = indexer;
outtaMyWay = new IncrementIndex(indexer);
// outtaMyWay = new IncrementIndex(indexer);
}

public RevAndShootEndsCommand(IndexSubsystem indexer, ShooterSubsystemVelocity shooter) {
Expand Down
Original file line number Diff line number Diff line change
@@ -1,7 +1,9 @@
package frc.robot.commands;
package frc.robot.experimental;

import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.commands.SetIndex;
import frc.robot.commands.SetIntake;
import frc.robot.subsystems.IndexSubsystem;
import frc.robot.subsystems.IntakeSubsystem;

Expand Down
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package frc.robot.subsystems;
package frc.robot.experimental;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

Expand All @@ -22,7 +23,8 @@ public void magSwitch() {
}
@Override
public void periodic(){
SmartDashboard.putBoolean("Mag Limit Switch", getSwitchState());
// Shuffleboard.getTab("Sensors").add("Mag Limit Switch", getSwitchState());

}


Expand Down
48 changes: 32 additions & 16 deletions src/main/java/frc/robot/subsystems/ArmSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.GravityTypeValue;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

Expand Down Expand Up @@ -102,10 +102,11 @@ public ArmSubsystem() {
m_arm.getConfigurator().apply(armCurrent0, 0.050);

/*
* Send info about the arm to the Smart Dashboard
* Send info about the arm to the Shuffleboard
* Defaults to percent output
* Only needed for diagnostics
*/
// SmartDashboard.putData("Arm Output", m_arm);
// Shuffleboard.getTab("Arm").add("Arm Output", m_arm);

} /* End of the class-method */

Expand All @@ -115,14 +116,12 @@ public StatusSignal<Double> getArmPos() {
}

public void setArmPose(double armPose) {
m_arm.setControl(m_armPose.withPosition(armPose));

// less effecient approach, shouldn't use this
// m_arm.setControl(new MotionMagicVoltage(armPose));

m_arm.setControl(

new MotionMagicVoltage(armPose));
/*
SmartDashboard.putNumber("Arm Position", m_arm.getPosition().getValue());
SmartDashboard.putNumber("Arm Stator", m_arm.getStatorCurrent().getValue());*/
showArmTelemetry();
// showArmTelemetry();

}

Expand All @@ -131,11 +130,13 @@ public void setArmPose(double armPose) {
// PositionVoltage armPose = new PositionVoltage(armPose);
public void setArmForClimb(double power) {
m_arm.setControl(new VoltageOut (power));
showArmTelemetry();
// showArmTelemetry();


}

/* I think this is all unused code */
/*
double targetPosition = 10;
public void moveArmToPositionAndStop(double targetPosition) {
Expand All @@ -149,24 +150,39 @@ public void moveArmToPositionAndStop(double targetPosition) {
// Stop the arm
m_arm.setControl(new PositionDutyCycle(0));
}
}
*/

/*
* Currently only being called in subsystem-command;
* inspite of my efforts it only appears once it's triggered at least once
*/
/*
public void showArmTelemetry() {
SmartDashboard.putNumber("Arm Position", m_arm.getPosition().getValue());
SmartDashboard.putNumber("Arm Stator", m_arm.getStatorCurrent().getValue());
SmartDashboard.putNumber("Arm Supply", m_arm.getSupplyCurrent().getValue());
SmartDashboard.putNumber("Arm Voltage", m_arm.getMotorVoltage().getValue());
Shuffleboard.getTab("Arm").add("Arm Position", m_arm.getPosition().getValue());
Shuffleboard.getTab("Arm").add("Arm Stator", m_arm.getStatorCurrent().getValue());
Shuffleboard.getTab("Arm").add("Arm Supply", m_arm.getSupplyCurrent().getValue());
Shuffleboard.getTab("Arm").add("Arm Voltage", m_arm.getMotorVoltage().getValue());
}
*/

public void stopArm(double power) {
m_arm.setControl(new VoltageOut(0));

}

@Override
public void periodic() {
/* Only needed for diagnostics */
/*
Shuffleboard.getTab("Arm").add("Arm Position", m_arm.getPosition().getValue());
Shuffleboard.getTab("Arm").add("Arm Stator", m_arm.getStatorCurrent().getValue());
Shuffleboard.getTab("Arm").add("Arm Supply", m_arm.getSupplyCurrent().getValue());
Shuffleboard.getTab("Arm").add("Arm Voltage", m_arm.getMotorVoltage().getValue());
*/
}

} // end of ArmSubsystem method

/***************************
Expand Down
32 changes: 18 additions & 14 deletions src/main/java/frc/robot/subsystems/NoteSensorSubsystem.java
Original file line number Diff line number Diff line change
@@ -1,22 +1,21 @@
package frc.robot.subsystems;

/* Subsystem class to primarily use a Time of Flight sensor from 'Playing with Fusion'.
* It will read the distance from the sensor to the 'note' and determine if the note is in a load position.
* It should return an actual distance reading to the SmartDashboard and a boolean for a method 'isNoteLoaded'
* This sensor data will change LED status and enable/disable intake and index motors
*/

import edu.wpi.first.wpilibj2.command.SubsystemBase;

import java.util.Optional;

import com.playingwithfusion.TimeOfFlight;
import com.playingwithfusion.TimeOfFlight.RangingMode;

import edu.wpi.first.hal.AllianceStationID;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;

/* Subsystem class to primarily use a Time of Flight sensor from 'Playing with Fusion'.
* It will read the distance from the sensor to the 'note' and determine if the note is in a load position.
* It should return an actual distance reading to and a boolean for a method 'isNoteLoaded'
* This sensor data will change LED status and enable/disable intake and index motors
*/

import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class NoteSensorSubsystem extends SubsystemBase {
Expand All @@ -40,7 +39,7 @@ public class NoteSensorSubsystem extends SubsystemBase {
/* Constructor */
public NoteSensorSubsystem() {
/*
* Initialize the sensor, and '.setRangingMode(RangingMode.Short)' for this
* Initialize the sensor, and '.setRangingMode(RangingMode.Short)' foDr this
* usage.
*
* | Sample value | Time |
Expand All @@ -50,8 +49,9 @@ public NoteSensorSubsystem() {
* | 3 | 50 ms |
* | 4 (default) | 100 ms |
* | 5 | 200 ms |
*****************************/
noteDistance.setRangingMode(RangingMode.Short, 1);
noteDistance.setRangingMode(RangingMode.Short, 4);
}

public double getNoteDistance() {
Expand Down Expand Up @@ -98,8 +98,12 @@ public void periodic() {
// ! Can be achieved with Shuffleboard call in
// ! Constructor, per
setLEDColor();
SmartDashboard.putNumber("Note Distance", noteDistance.getRange());
SmartDashboard.putBoolean("Note Loaded", isNoteLoaded());

// Only needed for diagnostics
// Shuffleboard.getTab("Note").add("Note Distance", noteDistance.getRange());
// FIXME: This is not working as expected. Code crashes saying .add title is already in use, probably because it's being called periodically.
// Shuffleboard.getTab("Sensors").add("Note Loaded 2", isNoteLoaded());

}
}
// end of class NoteSensorSubsystem
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
import com.ctre.phoenix6.signals.NeutralModeValue;

import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

Expand Down Expand Up @@ -148,8 +148,8 @@ public boolean isAtSetpoint() {
@Override
public void periodic() {
// System.out.println(m_main.getVelocity());
SmartDashboard.putNumber("Velocity", GetVelocity());
SmartDashboard.putBoolean("Velocity Achieved", isAtSetpoint());
// Shuffleboard.getTab("Shooter").add("Velocity", GetVelocity());
// Shuffleboard.getTab("Shooter").add("Velocity Achieved", isAtSetpoint());
}

}
Expand Down

0 comments on commit fb79bac

Please sign in to comment.