Skip to content

Commit

Permalink
Kitbot ready!
Browse files Browse the repository at this point in the history
  • Loading branch information
suryatho committed Jan 15, 2024
1 parent 5f87912 commit d9b4c64
Show file tree
Hide file tree
Showing 9 changed files with 226 additions and 43 deletions.
8 changes: 6 additions & 2 deletions build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@ plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2024.1.1"
id "com.peterabeles.gversion" version "1.10"
id "com.diffplug.spotless" version "6.12.0"
id "com.diffplug.spotless" version "6.24.0"
}

java {
Expand Down Expand Up @@ -113,7 +113,11 @@ wpi.sim.addDriverstation()
// in order to make them all available at runtime. Also adding the manifest so WPILib
// knows where to look for our Robot Class.
jar {
from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } }
from {
configurations.runtimeClasspath.collect {
it.isDirectory() ? it : zipTree(it)
}
}
from sourceSets.main.allSource
manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
duplicatesStrategy = DuplicatesStrategy.INCLUDE
Expand Down
40 changes: 38 additions & 2 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,9 @@

package frc.robot;

import edu.wpi.first.wpilibj.RobotBase;
import frc.robot.util.Alert;

/**
* The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
* constants. This class should not be used for any other purpose. All constants should be declared
Expand All @@ -23,11 +26,30 @@
*/
public final class Constants {
public static final int loopPeriodMs = 20;
public static final Mode currentMode = Mode.SIM;
private static RobotType robotType = RobotType.KITBOT;
public static final boolean tuningMode = true;
public static final boolean characterizationMode = false;

public static enum Mode {
private static boolean invalidRobotAlertSent = false;

public static RobotType getRobot() {
if (RobotBase.isReal() && robotType == RobotType.SIMBOT) {
new Alert("Invalid Robot Selected, using COMPBOT as default", Alert.AlertType.ERROR)
.set(true);
invalidRobotAlertSent = true;
robotType = RobotType.COMPBOT;
}
return robotType;
}

public static Mode getMode() {
return switch (robotType) {
case KITBOT, COMPBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY;
case SIMBOT -> Mode.SIM;
};
}

public enum Mode {
/** Running on a real robot. */
REAL,

Expand All @@ -37,4 +59,18 @@ public static enum Mode {
/** Replaying from a log file. */
REPLAY
}

public enum RobotType {
SIMBOT,
KITBOT,
COMPBOT
}

/** Checks whether the robot the correct robot is selected when deploying. */
public static void main(String... args) {
if (robotType == RobotType.SIMBOT) {
System.err.println("Cannot deploy, invalid robot selected: " + robotType.toString());
System.exit(1);
}
}
}
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ public void robotInit() {
}

// Set up data receivers & replay source
switch (Constants.currentMode) {
switch (Constants.getMode()) {
case REAL:
// Running on a real robot, log to a USB stick ("/U/logs")
Logger.addDataReceiver(new WPILOGWriter());
Expand Down Expand Up @@ -117,7 +117,7 @@ public void robotInit() {
});

// Default to blue alliance in sim
if (Constants.currentMode == Constants.Mode.SIM) {
if (Constants.getMode() == Constants.Mode.SIM) {
DriverStationSim.setAllianceStationId(AllianceStationID.Blue1);
}

Expand Down
47 changes: 21 additions & 26 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@

package frc.robot;

import com.pathplanner.lib.path.*;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.Filesystem;
Expand All @@ -25,7 +24,6 @@
import frc.robot.commands.DriveCommands;
import frc.robot.commands.DriveTrajectory;
import frc.robot.commands.FeedForwardCharacterization;
import frc.robot.commands.autos.TestAutos;
import frc.robot.subsystems.drive.Drive;
import frc.robot.subsystems.drive.GyroIO;
import frc.robot.subsystems.drive.GyroIOPigeon2;
Expand Down Expand Up @@ -63,26 +61,24 @@ public class RobotContainer {

/** The container for the robot. Contains subsystems, OI devices, and commands. */
public RobotContainer() {
switch (Constants.currentMode) {
case REAL:
// Real robot, instantiate hardware IO implementations
drive =
new Drive(
new GyroIOPigeon2(false),
new ModuleIOSparkMax(0),
new ModuleIOSparkMax(1),
new ModuleIOSparkMax(2),
new ModuleIOSparkMax(3));
// drive = new Drive(
// new GyroIOPigeon2(true),
// new ModuleIOTalonFX(0),
// new ModuleIOTalonFX(1),
// new ModuleIOTalonFX(2),
// new ModuleIOTalonFX(3));
shooter = new KitbotShooter(new KitbotFlywheelIOSparkMax(), new KitbotFeederIOSparkMax());
break;

case SIM:
switch (Constants.getMode()) {
case REAL -> {
// Real robot, instantiate hardware IO implementations\
switch (Constants.getRobot()) {
default -> {
drive =
new Drive(
new GyroIOPigeon2(false),
new ModuleIOSparkMax(0),
new ModuleIOSparkMax(1),
new ModuleIOSparkMax(2),
new ModuleIOSparkMax(3));
shooter =
new KitbotShooter(new KitbotFlywheelIOSparkMax(), new KitbotFeederIOSparkMax());
}
}
}
case SIM -> {
// Sim robot, instantiate physics sim IO implementations
drive =
new Drive(
Expand All @@ -92,9 +88,8 @@ public RobotContainer() {
new ModuleIOSim(),
new ModuleIOSim());
shooter = new KitbotShooter(new KitbotFlywheelIOSim(), new KitbotFeederIOSim());
break;

default:
}
default -> {
// Replayed robot, disable IO implementations
drive =
new Drive(
Expand All @@ -104,7 +99,7 @@ public RobotContainer() {
new ModuleIO() {},
new ModuleIO() {});
shooter = new KitbotShooter(new KitbotFlywheelIO() {}, new KitbotFeederIO() {});
break;
}
}

autoChooser = new LoggedDashboardChooser<>("Auto Choices");
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/subsystems/drive/DriveConstants.java
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,9 @@ public final class DriveConstants {

// Replace with robots
public static TrajectoryConstants trajectoryConstants() {
return switch (Constants.currentMode) {
case REPLAY, REAL -> new TrajectoryConstants(6.0, 0.0, 8.0, 0.0);
case SIM -> new TrajectoryConstants(2.5, 0.0, 7.5, 0.0);
return switch (Constants.getRobot()) {
case COMPBOT, KITBOT -> new TrajectoryConstants(6.0, 0.0, 8.0, 0.0);
case SIMBOT -> new TrajectoryConstants(2.5, 0.0, 7.5, 0.0);
};
}

Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/drive/Module.java
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ public Module(ModuleIO io, int index) {

// Switch constants based on mode (the physics simulator is treated as a
// separate robot with different tuning)
switch (Constants.currentMode) {
switch (Constants.getMode()) {
case REAL:
case REPLAY:
driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13);
Expand Down
14 changes: 7 additions & 7 deletions src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java
Original file line number Diff line number Diff line change
Expand Up @@ -56,25 +56,25 @@ public class ModuleIOSparkMax implements ModuleIO {
public ModuleIOSparkMax(int index) {
switch (index) {
case 0:
driveSparkMax = new CANSparkMax(1, MotorType.kBrushless);
turnSparkMax = new CANSparkMax(2, MotorType.kBrushless);
driveSparkMax = new CANSparkMax(15, MotorType.kBrushless);
turnSparkMax = new CANSparkMax(11, MotorType.kBrushless);
turnAbsoluteEncoder = new AnalogInput(0);
absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
break;
case 1:
driveSparkMax = new CANSparkMax(3, MotorType.kBrushless);
turnSparkMax = new CANSparkMax(4, MotorType.kBrushless);
driveSparkMax = new CANSparkMax(12, MotorType.kBrushless);
turnSparkMax = new CANSparkMax(9, MotorType.kBrushless);
turnAbsoluteEncoder = new AnalogInput(1);
absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
break;
case 2:
driveSparkMax = new CANSparkMax(5, MotorType.kBrushless);
turnSparkMax = new CANSparkMax(6, MotorType.kBrushless);
driveSparkMax = new CANSparkMax(14, MotorType.kBrushless);
turnSparkMax = new CANSparkMax(10, MotorType.kBrushless);
turnAbsoluteEncoder = new AnalogInput(2);
absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
break;
case 3:
driveSparkMax = new CANSparkMax(7, MotorType.kBrushless);
driveSparkMax = new CANSparkMax(13, MotorType.kBrushless);
turnSparkMax = new CANSparkMax(8, MotorType.kBrushless);
turnAbsoluteEncoder = new AnalogInput(3);
absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED
Expand Down
147 changes: 147 additions & 0 deletions src/main/java/frc/robot/util/Alert.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,147 @@
package frc.robot.util;

// Copyright (c) 2023 FRC 6328
// http://github.com/Mechanical-Advantage
//
// Use of this source code is governed by an MIT-style
// license that can be found in the LICENSE file at
// the root directory of this project.

import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.ArrayList;
import java.util.Comparator;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.function.Predicate;

/** Class for managing persistent alerts to be sent over NetworkTables. */
public class Alert {
private static Map<String, SendableAlerts> groups = new HashMap<String, SendableAlerts>();

private final AlertType type;
private boolean active = false;
private double activeStartTime = 0.0;
private String text;

/**
* Creates a new Alert in the default group - "Alerts". If this is the first to be instantiated,
* the appropriate entries will be added to NetworkTables.
*
* @param text Text to be displayed when the alert is active.
* @param type Alert level specifying urgency.
*/
public Alert(String text, AlertType type) {
this("Alerts", text, type);
}

/**
* Creates a new Alert. If this is the first to be instantiated in its group, the appropriate
* entries will be added to NetworkTables.
*
* @param group Group identifier, also used as NetworkTables title
* @param text Text to be displayed when the alert is active.
* @param type Alert level specifying urgency.
*/
public Alert(String group, String text, AlertType type) {
if (!groups.containsKey(group)) {
groups.put(group, new SendableAlerts());
SmartDashboard.putData(group, groups.get(group));
}

this.text = text;
this.type = type;
groups.get(group).alerts.add(this);
}

/**
* Sets whether the alert should currently be displayed. When activated, the alert text will also
* be sent to the console.
*/
public void set(boolean active) {
if (active && !this.active) {
activeStartTime = Timer.getFPGATimestamp();
switch (type) {
case ERROR:
DriverStation.reportError(text, false);
break;
case WARNING:
DriverStation.reportWarning(text, false);
break;
case INFO:
System.out.println(text);
break;
}
}
this.active = active;
}

/** Updates current alert text. */
public void setText(String text) {
if (active && !text.equals(this.text)) {
switch (type) {
case ERROR:
DriverStation.reportError(text, false);
break;
case WARNING:
DriverStation.reportWarning(text, false);
break;
case INFO:
System.out.println(text);
break;
}
}
this.text = text;
}

private static class SendableAlerts implements Sendable {
public final List<Alert> alerts = new ArrayList<>();

public String[] getStrings(AlertType type) {
Predicate<Alert> activeFilter = (Alert x) -> x.type == type && x.active;
Comparator<Alert> timeSorter =
(Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime);
return alerts.stream()
.filter(activeFilter)
.sorted(timeSorter)
.map((Alert a) -> a.text)
.toArray(String[]::new);
}

@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Alerts");
builder.addStringArrayProperty("errors", () -> getStrings(AlertType.ERROR), null);
builder.addStringArrayProperty("warnings", () -> getStrings(AlertType.WARNING), null);
builder.addStringArrayProperty("infos", () -> getStrings(AlertType.INFO), null);
}
}

/** Represents an alert's level of urgency. */
public static enum AlertType {
/**
* High priority alert - displayed first on the dashboard with a red "X" symbol. Use this type
* for problems which will seriously affect the robot's functionality and thus require immediate
* attention.
*/
ERROR,

/**
* Medium priority alert - displayed second on the dashboard with a yellow "!" symbol. Use this
* type for problems which could affect the robot's functionality but do not necessarily require
* immediate attention.
*/
WARNING,

/**
* Low priority alert - displayed last on the dashboard with a green "i" symbol. Use this type
* for problems which are unlikely to affect the robot's functionality, or any other alerts
* which do not fall under "ERROR" or "WARNING".
*/
INFO
}
}
1 change: 1 addition & 0 deletions src/main/java/frc/robot/util/GeomUtil.java
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
package frc.robot.util;

// Copyright (c) 2023 FRC 6328
// http://github.com/Mechanical-Advantage
//
Expand Down

0 comments on commit d9b4c64

Please sign in to comment.