diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2cb1d6b..b619905 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -24,6 +24,8 @@ import frc4388.utility.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { + public static final String CANBUS_NAME = "rio"; + public static final class SwerveDriveConstants { public static final double MAX_ROT_SPEED = 3.5; @@ -33,8 +35,6 @@ public final class Constants { public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED; public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; - public static final String CANBUS_NAME = "IDK"; - public static final double CORRECTION_MIN = 10; public static final double CORRECTION_MAX = 50; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 1c3a04c..bb244ae 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -12,6 +12,9 @@ import java.util.Base64; import java.util.List; import java.util.logging.Level; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.CANBus.CANBusStatus; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -20,7 +23,6 @@ import frc4388.utility.DeferredBlock; import frc4388.utility.RobotTime; import frc4388.utility.Status; import frc4388.utility.Subsystem; -import frc4388.utility.DeviceFinder; import frc4388.utility.Status.Report; import frc4388.utility.Status.ReportLevel; //import frc4388.robot.subsystems.LED; @@ -162,6 +164,20 @@ public class Robot extends TimedRobot { // CAN header System.out.println(new String(Base64.getDecoder().decode("IOKWl+KWhOKWhOKWliDilpfiloTilpYg4paX4paWICDilpfilpYK4paQ4paMICAg4paQ4paMIOKWkOKWjOKWkOKWm+KWmuKWluKWkOKWjArilpDilowgICDilpDilpviloDilpzilozilpDilowg4pad4pac4paMCuKWneKWmuKWhOKWhOKWluKWkOKWjCDilpDilozilpDilowgIOKWkOKWjCh0KQ=="))); + CANBusStatus canInfo = CANBus.getStatus(Constants.CANBUS_NAME); + + System.out.println("CANInfo BusOffCount - " + canInfo.BusOffCount); + System.out.println("CANInfo BusUtilization - " + canInfo.BusUtilization); + System.out.println("CANInfo RX Errors count - " + canInfo.REC); + System.out.println("CANInfo TX Errors count - " + canInfo.TEC); + System.out.println("CANInfo Transmit buffer full count - " + canInfo.TxFullCount); + // Broken turniary operator + ReportLevel canReportLevel = canInfo.Status.isOK() ? (canInfo.Status.isWarning() ? ReportLevel.WARNING : ReportLevel.ERROR) : ReportLevel.INFO; + String canStatus = "CAN " + canReportLevel.name() + " - " + canInfo.Status.getName() + " (" + canInfo.Status.getDescription() + ")"; + if(canReportLevel == ReportLevel.ERROR) { + errors.add(canStatus); + } + System.out.println(canStatus); for(int i=0;i 0) { // Errors header diff --git a/src/main/java/frc4388/utility/DeviceFinder.java b/src/main/java/frc4388/utility/DeviceFinder.java deleted file mode 100644 index 99c4c87..0000000 --- a/src/main/java/frc4388/utility/DeviceFinder.java +++ /dev/null @@ -1,161 +0,0 @@ -package frc4388.utility; - -import java.nio.ByteBuffer; -import java.nio.ByteOrder; -import java.util.ArrayList; -import java.util.List; -import java.util.Set; -import java.util.TreeSet; - -import edu.wpi.first.hal.can.CANJNI; - -// https://github.com/FRC3620/FRC3620_2018_CoffeePi/blob/6ab2776ce0ee83cf8e127a7c50337ff17f01b249/FRC3620_2018_CoffeePi/src/org/usfirst/frc3620/misc/CANDeviceFinder.java# - -public class DeviceFinder { - - ArrayList deviceList = new ArrayList(); - - public ArrayList Find() { - return deviceList; - } - - private boolean pdpIsPresent = false; - private Set srxs = new TreeSet<>(); - private Set spxs = new TreeSet<>(); - private Set pcms = new TreeSet<>(); - - public DeviceFinder() { - super(); - find(); - } - - public boolean isPDPPresent() { - return pdpIsPresent; - } - - public boolean isSRXPresent(int i) { - return srxs.contains(i); - } - - public boolean isSPXPresent(int i) { - return spxs.contains(i); - } - - public boolean isPCMPresent(int i) { - return pcms.contains(i); - } - - public boolean isDevicePresent(String s) { - return deviceList.contains(s); - } - - /** - * - * @return ArrayList of strings holding the names of devices we've found. - */ - public List getDeviceList() { - return deviceList; - } - - /** - * polls for received framing to determine if a device is present. This is - * meant to be used once initially (and not periodically) since this steals - * cached messages from the robot API. - */ - void find() { - deviceList.clear(); - pdpIsPresent = false; - srxs.clear(); - pcms.clear(); - - /* get timestamp0 for each device */ - long pdp0_timeStamp0; // only look for PDP at '0' - long[] pcm_timeStamp0 = new long[63]; - long[] srx_timeStamp0 = new long[63]; - long[] spx_timeStamp0 = new long[63]; - - pdp0_timeStamp0 = checkMessage(0x08041400, 0); - for (int i = 0; i < 63; ++i) { - pcm_timeStamp0[i] = checkMessage(0x09041400, i); - srx_timeStamp0[i] = checkMessage(0x02041400, i); - spx_timeStamp0[i] = checkMessage(0x01041400, i); - } - - /* wait 200ms */ - try { - Thread.sleep(200); - } catch (InterruptedException e) { - e.printStackTrace(); - } - - /* get timestamp1 for each device */ - long pdp0_timeStamp1; // only look for PDP at '0' - long[] pcm_timeStamp1 = new long[63]; - long[] srx_timeStamp1 = new long[63]; - long[] spx_timeStamp1 = new long[63]; - - pdp0_timeStamp1 = checkMessage(0x08041400, 0); - for (int i = 0; i < 63; ++i) { - pcm_timeStamp1[i] = checkMessage(0x09041400, i); - srx_timeStamp1[i] = checkMessage(0x02041400, i); - spx_timeStamp1[i] = checkMessage(0x01041400, i); - } - - /* - * compare, if timestamp0 is good and timestamp1 is good, and they are - * different, device is healthy - */ - if (pdp0_timeStamp0 >= 0 && pdp0_timeStamp1 >= 0 - && pdp0_timeStamp0 != pdp0_timeStamp1) { - deviceList.add("PDP 0"); - pdpIsPresent = true; - } - - for (int i = 0; i < 63; ++i) { - if (pcm_timeStamp0[i] >= 0 && pcm_timeStamp1[i] >= 0 - && pcm_timeStamp0[i] != pcm_timeStamp1[i]) { - deviceList.add("PCM " + i); - pcms.add(i); - } - } - for (int i = 0; i < 63; ++i) { - if (srx_timeStamp0[i] >= 0 && srx_timeStamp1[i] >= 0 - && srx_timeStamp0[i] != srx_timeStamp1[i]) { - deviceList.add("SRX " + i); - srxs.add(i); - } - } - for (int i = 0; i < 63; ++i) { - if (spx_timeStamp0[i] >= 0 && spx_timeStamp1[i] >= 0 - && spx_timeStamp0[i] != spx_timeStamp1[i]) { - deviceList.add("SPX " + i); - spxs.add(i); - } - } - } - - private ByteBuffer targetID = ByteBuffer.allocateDirect(4); - private ByteBuffer timeStamp = ByteBuffer.allocateDirect(4); - - /** helper routine to get last received message for a given ID */ - private long checkMessage(int fullId, int deviceID) { - try { - targetID.clear(); - targetID.order(ByteOrder.LITTLE_ENDIAN); - targetID.asIntBuffer().put(0, fullId | deviceID); - - timeStamp.clear(); - timeStamp.order(ByteOrder.LITTLE_ENDIAN); - timeStamp.asIntBuffer().put(0, 0x00000000); - - CANJNI.FRCNetCommCANSessionMuxReceiveMessage( - targetID.asIntBuffer(), 0x1fffffff, timeStamp); - - long retval = timeStamp.getInt(); - retval &= 0xFFFFFFFF; /* undo sign-extension */ - return retval; - } catch (Exception e) { - return -1; - } - } -} \ No newline at end of file