TinCANBus

src/main/java/frc/robot/Robot.java

@@ -18,8 +18,10 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.hardware.TinCANBus;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -47,18 +49,19 @@ public static enum LedEnum {
   public static final LedEnum ledSubSelect = LedEnum.NONE;
 
   public static final boolean cameraEnabled = false;
-  public static final boolean intakeEnabled = true;
-  public static final boolean tofEnabled = true;
-  public static final boolean swerveEnabled = true;
-  public static final boolean armEnabled = true;
+  public static final boolean intakeEnabled = false;
+  public static final boolean tofEnabled = false;
+  public static final boolean swerveEnabled = false;
+  public static final boolean armEnabled = false;
   public static final boolean buddyBalanceEnabled = false;
-  public static final boolean cubeapultEnabled = true;
+  public static final boolean cubeapultEnabled = false;
 
   private Command m_autonomousCommand;
   private DataLog loopCountlog = DataLogManager.getLog();
   private IntegerLogEntry loopCountEntry = new IntegerLogEntry(loopCountlog, "/robot/loopCount");
 
   private RobotContainer m_robotContainer;
+  private TinCANBus canbus;
 
   private int loopCount = 0;
 
@@ -90,6 +93,7 @@ public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+    canbus = new TinCANBus(0);
   }
 
   /**
@@ -108,6 +112,7 @@ public void robotPeriodic() {
     loopCount++;
     loopCountEntry.append(loopCount);
     CommandScheduler.getInstance().run();
+    SmartDashboard.putNumber("Time of Flight (mm)", canbus.lastSensorRead());
   }
 
   /** This function is called once each time the robot enters Disabled mode. */
@@ -116,8 +121,9 @@ public void disabledInit() {}
 
   @Override
   public void disabledPeriodic() {}
-
-  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+
+
+
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();

src/main/java/frc/robot/hardware/TinCANBus.java

@@ -0,0 +1,111 @@
+// 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.hardware;
+
+import edu.wpi.first.hal.CANData;
+import edu.wpi.first.wpilibj.CAN;
+
+/*
+ * Implements an API for a custom CAN device
+ * 
+ * See FRC CAN Bus Documentation: https://docs.wpilib.org/en/stable/docs/software/can-devices/can-addressing.html
+ */
+public class TinCANBus extends CAN {
+
+    /* 
+      Extended Identifier bits:
+        Device Type: 10 -> Miscellanious
+        Manufacture ID: 8 -> Team Use
+        API Class: 
+           0 -> Device Config 
+           API Indices:
+             1 -> Set new deviceId
+
+           1 -> Sensor Bridge API
+           API Indices:
+             1 -> Get last sensor read
+             2 -> Status
+
+        Device Number/ID: 0-63
+    */
+
+
+    private static enum DEV_CONFIG_API_CLASS {
+      SET_NEW_DEVICEID(1);
+
+      private final int APICLASS = 0;
+      private final int APIID;
+
+      DEV_CONFIG_API_CLASS(int apiindex) {
+        this.APIID = getApiID(APICLASS, apiindex);
+      }
+
+    }
+
+    private static enum SENSOR_BRIDGE_API_CLASS {
+      GET_READ(1),
+      STATUS(2);
+
+      private final int APICLASS = 1;
+      private final int APIID;
+
+      SENSOR_BRIDGE_API_CLASS(int apiindex) {
+        this.APIID = getApiID(APICLASS, apiindex);
+      }
+    }
+
+    // API ID is a 10 bit sequence 
+    // Bits 9-4 -> API Class 
+    // Bits 3-0 -> API Index 
+    private static int getApiID(int apiClass, int apiIndex) {
+        return ((apiClass & 0x003F) << 4) + (apiIndex & 0x0F);
+    }
+
+    private static int byteArrayToInt(byte[] array, int length) {
+      int ret = 0;
+      for (int i = 0; i < array.length && i < length && i < Integer.BYTES; i++) {
+        ret = (ret << 8) + (array[i] & 0x00FF);
+      }
+      return ret;
+    }
+
+
+    public TinCANBus(int deviceId) {
+        super(deviceId);
+    }
+
+    // Send packet to tell CAN device to change the device number it listens for. 
+    // Will be applied when the CAN device is next power cycled
+    // Only use when only the CAN device of this type you want to change is connected to the bus
+    public void setNewDeviceId(int deviceId) {
+      byte[] buff = {(byte)deviceId};
+      writePacket(buff, DEV_CONFIG_API_CLASS.SET_NEW_DEVICEID.APIID);
+    }
+
+    // Returns 2 bytes representing the last sensor read value 
+    // or -1 if packet error occurs
+    public int lastSensorRead() {
+        CANData d = new CANData();
+        if (readPacketLatest(SENSOR_BRIDGE_API_CLASS.GET_READ.APIID, d)) {
+          return byteArrayToInt(d.data, 2);
+        } else {
+          return -1;
+        }
+    }
+
+    // Return 1 byte status message (TBD)
+    // or -1 if packet error occurs
+    public int getSensorStatus() {
+        CANData d = new CANData();
+        if (readPacketLatest(SENSOR_BRIDGE_API_CLASS.STATUS.APIID, d)) {
+          return byteArrayToInt(d.data, 1);
+        } else {
+          return -1;
+        }
+    }
+
+
+}
+