Skip to content

Commit

Permalink
feat: Mac address-based robot switcher (#54)
Browse files Browse the repository at this point in the history
* feat: switch constants based on mac address

* feat: printing unknown mac address

* docs: explain all the tools in MacAddressUtil

* docs: reword

* chore: input prototype bot Mac Address
  • Loading branch information
amb2127 authored Feb 10, 2024
1 parent 204c2d0 commit 2098868
Show file tree
Hide file tree
Showing 3 changed files with 117 additions and 5 deletions.
17 changes: 12 additions & 5 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import frc.utility.MacAddressUtil;
import frc.utility.MacAddressUtil.RobotIdentity;
import frc.utility.swerve.SwerveSetpointGenerator;
import frc.utility.swerve.SecondOrderKinematics;
import org.jetbrains.annotations.NotNull;
Expand All @@ -14,6 +16,10 @@

public final class Constants {
//TODO: reorganize this mess
public static RobotIdentity robotIdentity;
public static boolean isPrototype() {
return robotIdentity == RobotIdentity.PROTOTYPE_BOT;
}
public static final String LOG_DIRECTORY = "/home/lvuser/logs";
public static final boolean IS_PRACTICE = Files.exists(new File("/home/lvuser/practice").toPath());
public static final String VIRTUAL_MODE = "SIM";
Expand Down Expand Up @@ -107,7 +113,7 @@ public enum ElevatorPosition {
public static final double ELEVATOR_STALLING_CURRENT = 35;
public static final double MIN_ELEVATOR_HOME_TIME = 0.2;

public static final double SWERVE_DRIVE_P = 100;
public static final double SWERVE_DRIVE_P = isPrototype() ? 100 : 1;
public static final double SWERVE_DRIVE_D = 0.05;
public static final double SWERVE_DRIVE_I = 0.00;

Expand Down Expand Up @@ -138,10 +144,11 @@ public enum ElevatorPosition {
public static final double DRIVE_MOTOR_REDUCTION = 1 / 5.9;

// TODO: check accuracy of these numbers for new drive base. Ask CAD ppl?
public static final @NotNull Translation2d SWERVE_LEFT_FRONT_LOCATION = new Translation2d(Units.inchesToMeters(11.375), Units.inchesToMeters(11.375));
public static final @NotNull Translation2d SWERVE_LEFT_BACK_LOCATION = new Translation2d(Units.inchesToMeters(-11.375), Units.inchesToMeters(11.375));
public static final @NotNull Translation2d SWERVE_RIGHT_FRONT_LOCATION = new Translation2d(Units.inchesToMeters(11.375), Units.inchesToMeters(-11.375));
public static final @NotNull Translation2d SWERVE_RIGHT_BACK_LOCATION = new Translation2d(Units.inchesToMeters(-11.375), Units.inchesToMeters(-11.375));
public static final double wheelBaseInches = isPrototype() ? 22.75 : 27; // not real number, just example
public static final @NotNull Translation2d SWERVE_LEFT_FRONT_LOCATION = new Translation2d(Units.inchesToMeters(wheelBaseInches/2), Units.inchesToMeters(wheelBaseInches/2));
public static final @NotNull Translation2d SWERVE_LEFT_BACK_LOCATION = new Translation2d(Units.inchesToMeters(-wheelBaseInches/2), Units.inchesToMeters(wheelBaseInches/2));
public static final @NotNull Translation2d SWERVE_RIGHT_FRONT_LOCATION = new Translation2d(Units.inchesToMeters(wheelBaseInches/2), Units.inchesToMeters(-wheelBaseInches/2));
public static final @NotNull Translation2d SWERVE_RIGHT_BACK_LOCATION = new Translation2d(Units.inchesToMeters(-wheelBaseInches/2), Units.inchesToMeters(-wheelBaseInches/2));
public static final @NotNull Translation2d @NotNull [] SWERVE_MODULE_LOCATIONS = {
SWERVE_LEFT_FRONT_LOCATION,
SWERVE_LEFT_BACK_LOCATION,
Expand Down
15 changes: 15 additions & 0 deletions src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
import frc.utility.Controller;
import frc.utility.Controller.XboxButtons;
import frc.utility.ControllerDriveInputs;
import frc.utility.MacAddressUtil;
import frc.utility.net.editing.LiveEditableValue;
import org.littletonrobotics.junction.LogFileUtil;
import org.littletonrobotics.junction.LoggedRobot;
Expand All @@ -42,6 +43,7 @@

import java.io.File;
import java.io.IOException;
import java.net.SocketException;
import java.nio.file.Files;
import java.util.Arrays;
import java.util.NoSuchElementException;
Expand Down Expand Up @@ -83,6 +85,17 @@ public class Robot extends LoggedRobot {
// static Climber climber;

static Superstructure superstructure;
static byte[] mac;

static {
try {
mac = MacAddressUtil.getMacAddress();
} catch (SocketException e) {
System.out.println("Failed to get MAC address");
}

robotIdentity = MacAddressUtil.RobotIdentity.getRobotIdentity(mac);
}

/**
* This function is run when the robot is first started up and should be used
Expand All @@ -103,6 +116,8 @@ public void robotInit() {
case 1 -> Logger.recordMetadata("GitDirty", "Uncommitted changes");
default -> Logger.recordMetadata("GitDirty", "Unknown");
}
Logger.recordMetadata("Robot Identity", robotIdentity.toString());
Logger.recordMetadata("MAC Address", MacAddressUtil.macToString(mac));

if (!isReal() && Objects.equals(VIRTUAL_MODE, "REPLAY")) {
try {
Expand Down
90 changes: 90 additions & 0 deletions src/main/java/frc/utility/MacAddressUtil.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,90 @@
package frc.utility;

import frc.robot.Robot;

import java.net.NetworkInterface;
import java.net.SocketException;
import java.util.Arrays;
import java.util.Enumeration;

/**
* Utility class for getting the MAC address of the robot radio and determining the robot's identity.
*
* @author 2910 & 1678
*/
public class MacAddressUtil {
public static final byte[] PROTOTYPE_BOT = new byte[]{
(byte) 0x00, (byte) 0x80, (byte) 0x2f, (byte) 0x19, (byte) 0x21, (byte) 0xd9
};
public static final byte[] PRACTICE_BOT = new byte[]{
(byte) 0x00, (byte) 0x00, (byte) 0x00, (byte) 0x00, (byte) 0x00, (byte) 0x00
};
public static final byte[] COMPETITION_BOT = new byte[]{
(byte) 0x00, (byte) 0x00, (byte) 0x00, (byte) 0x00, (byte) 0x00, (byte) 0x00
};

/**
* Gets the MAC address of the robot's radio.
*
* @return the MAC address of the robot's radio
* @throws SocketException if no MAC address is found
*/
public static byte[] getMacAddress() throws SocketException {
Enumeration<NetworkInterface> networkInterface = NetworkInterface.getNetworkInterfaces();

NetworkInterface temp;

while (networkInterface.hasMoreElements()) {
temp = networkInterface.nextElement();

byte[] mac = temp.getHardwareAddress();
if (mac != null) {
return mac;
}
}
return null;
}

/**
* Converts a raw MAC address byte array to a string.
*
* @param mac the MAC address byte array
* @return the MAC address as a string
*/
public static String macToString(byte[] mac) {
StringBuilder sb = new StringBuilder();
for (int i = 0; i < mac.length; i++) {
sb.append(String.format("%02X%s", mac[i], (i < mac.length - 1) ? ":" : ""));
}
return sb.toString();
}

/**
* Enum representing the possible robot identities.
*/
public enum RobotIdentity {
PROTOTYPE_BOT,
PRACTICE_BOT,
COMPETITION_BOT;

/**
* Gets the robot identity based on the MAC address.
*
* @param mac the MAC address
* @return the robot identity
*/
public static RobotIdentity getRobotIdentity(byte[] mac) {
if (Arrays.compare(mac, MacAddressUtil.PROTOTYPE_BOT) == 0) {
return PROTOTYPE_BOT;
} else if (Arrays.compare(mac, MacAddressUtil.PRACTICE_BOT) == 0) {
return PRACTICE_BOT;
} else if (Arrays.compare(mac, MacAddressUtil.COMPETITION_BOT) == 0) {
return COMPETITION_BOT;
} else {
System.out.println("Unknown MAC Address: " + macToString(mac));
System.out.println("Assuming Prototype Bot");
return PROTOTYPE_BOT; // assume prototype bot for now, change this to comp later
}
}
}
}

0 comments on commit 2098868

Please sign in to comment.