From c8b7e26b998b740a65e6a2c8cb446d32caad3aa1 Mon Sep 17 00:00:00 2001 From: Adam Vander Pas Date: Thu, 20 Apr 2023 18:33:38 -0500 Subject: [PATCH 1/7] Explore CAN class & protocol --- .../java/frc/robot/hardware/TinCANBus.java | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100644 src/main/java/frc/robot/hardware/TinCANBus.java diff --git a/src/main/java/frc/robot/hardware/TinCANBus.java b/src/main/java/frc/robot/hardware/TinCANBus.java new file mode 100644 index 00000000..9491b187 --- /dev/null +++ b/src/main/java/frc/robot/hardware/TinCANBus.java @@ -0,0 +1,38 @@ +// 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.wpilibj.CAN; + +/** Add your docs here. */ +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 -> I2C Bridge API + API Indices: + 1 -> Set I2C device + 2 -> Send Read I2C + 3 -> Send Write I2C + 4 -> Return last I2C read & status + 5 -> Periodic I2C Read + + Device Number/ID: 0-63 + */ + + public TinCANBus(int deviceId) { + super(deviceId); + } + + +} + From 52c4a611bed7a394395469dbd6868b2353881b7f Mon Sep 17 00:00:00 2001 From: Adam Vander Pas Date: Thu, 4 May 2023 18:01:34 -0500 Subject: [PATCH 2/7] Initial TinCANBus I2C setup --- .../java/frc/robot/hardware/TinCANBus.java | 32 +++++++++++++++++-- 1 file changed, 30 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/hardware/TinCANBus.java b/src/main/java/frc/robot/hardware/TinCANBus.java index 9491b187..3001effe 100644 --- a/src/main/java/frc/robot/hardware/TinCANBus.java +++ b/src/main/java/frc/robot/hardware/TinCANBus.java @@ -23,16 +23,44 @@ public class TinCANBus extends CAN { 1 -> Set I2C device 2 -> Send Read I2C 3 -> Send Write I2C - 4 -> Return last I2C read & status - 5 -> Periodic I2C Read + 4 -> Get last I2C read + 5 -> Status + 6 -> Periodic I2C Read Device Number/ID: 0-63 */ + private static int DEV_CONFIG_API_CLASS = 0; + + private static int SET_NEW_DEVICEID = 1; + + + private static int I2C_BRIDGE_API_CLASS = 1; + + private static int SET_DEVICE = 1; + private static int READ_I2C = 2; + private static int WRITE_I2C = 3; + private static int GET_READ = 4; + private static int STATUS = 5; + private static int PERIODIC_READ_I2C = 6; + public TinCANBus(int deviceId) { super(deviceId); } + private int getApiID(int apiClass, int apiIndex) { + return ((apiClass & 0x003F) << 6) + (apiIndex & 0x0F); + } + + public void setI2cDevice(byte i2cAddress) { + byte [] packet = {i2cAddress}; + writePacket(packet, getApiID(I2C_BRIDGE_API_CLASS, SET_DEVICE)); + } + + public void requestI2cRead(byte regAddress) { + + } + } From 0d9a46645e98b7963ea0f35decd194a6e0b39d17 Mon Sep 17 00:00:00 2001 From: Adam Vander Pas Date: Wed, 10 May 2023 15:00:04 -0500 Subject: [PATCH 3/7] Initial working TinCANBus code --- src/main/java/frc/robot/Robot.java | 20 +++++++---- .../java/frc/robot/hardware/TinCANBus.java | 36 +++++++++---------- 2 files changed, 29 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 59d5c66a..e5973cfe 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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(); diff --git a/src/main/java/frc/robot/hardware/TinCANBus.java b/src/main/java/frc/robot/hardware/TinCANBus.java index 3001effe..a79a0c4f 100644 --- a/src/main/java/frc/robot/hardware/TinCANBus.java +++ b/src/main/java/frc/robot/hardware/TinCANBus.java @@ -4,6 +4,7 @@ package frc.robot.hardware; +import edu.wpi.first.hal.CANData; import edu.wpi.first.wpilibj.CAN; /** Add your docs here. */ @@ -18,14 +19,11 @@ public class TinCANBus extends CAN { API Indices: 1 -> Set new deviceId - 1 -> I2C Bridge API + 1 -> Sensor Bridge API API Indices: - 1 -> Set I2C device - 2 -> Send Read I2C - 3 -> Send Write I2C - 4 -> Get last I2C read - 5 -> Status - 6 -> Periodic I2C Read + 1 -> Get last sensor read + 2 -> Status + 3 -> Periodic sensor Read Device Number/ID: 0-63 */ @@ -35,29 +33,27 @@ public class TinCANBus extends CAN { private static int SET_NEW_DEVICEID = 1; - private static int I2C_BRIDGE_API_CLASS = 1; + private static int SENSOR_BRIDGE_API_CLASS = 1; - private static int SET_DEVICE = 1; - private static int READ_I2C = 2; - private static int WRITE_I2C = 3; - private static int GET_READ = 4; - private static int STATUS = 5; - private static int PERIODIC_READ_I2C = 6; + private static int GET_READ = 1; + private static int STATUS = 2; + private static int PERIODIC_READ = 3; public TinCANBus(int deviceId) { super(deviceId); } private int getApiID(int apiClass, int apiIndex) { - return ((apiClass & 0x003F) << 6) + (apiIndex & 0x0F); + return ((apiClass & 0x003F) << 4) + (apiIndex & 0x0F); } - public void setI2cDevice(byte i2cAddress) { - byte [] packet = {i2cAddress}; - writePacket(packet, getApiID(I2C_BRIDGE_API_CLASS, SET_DEVICE)); - } - public void requestI2cRead(byte regAddress) { + public int lastSensorRead() { + CANData d = new CANData(); + readPacketLatest(getApiID(SENSOR_BRIDGE_API_CLASS, GET_READ), d); + return ((int)d.data[0]) & 0x00FF + ((((int)d.data[1]) & 0x00FF) << 8); + + } From 097be1a96a5f8d266c42de0d90a53717fb9147d9 Mon Sep 17 00:00:00 2001 From: Adam Vander Pas Date: Wed, 10 May 2023 15:41:45 -0500 Subject: [PATCH 4/7] Fancy enums to define API Classes add a helper function --- .../java/frc/robot/hardware/TinCANBus.java | 66 +++++++++++++++---- 1 file changed, 52 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/hardware/TinCANBus.java b/src/main/java/frc/robot/hardware/TinCANBus.java index a79a0c4f..b89c59bb 100644 --- a/src/main/java/frc/robot/hardware/TinCANBus.java +++ b/src/main/java/frc/robot/hardware/TinCANBus.java @@ -23,38 +23,76 @@ public class TinCANBus extends CAN { API Indices: 1 -> Get last sensor read 2 -> Status - 3 -> Periodic sensor Read Device Number/ID: 0-63 */ - private static int DEV_CONFIG_API_CLASS = 0; - private static int SET_NEW_DEVICEID = 1; + //TODO implement this API's class methods + private enum DEV_CONFIG_API_CLASS { + SET_NEW_DEVICEID(1); + private final int APICLASS = 0; + private int APIID; + + DEV_CONFIG_API_CLASS(int apiindex) { + this.APIID = getApiID(APICLASS, apiindex); + } - private static int SENSOR_BRIDGE_API_CLASS = 1; + } - private static int GET_READ = 1; - private static int STATUS = 2; - private static int PERIODIC_READ = 3; + private enum SENSOR_BRIDGE_API_CLASS { + GET_READ(1), + STATUS(2); - public TinCANBus(int deviceId) { - super(deviceId); + private final int APICLASS = 1; + private int APIID; + + SENSOR_BRIDGE_API_CLASS(int apiindex) { + this.APIID = getApiID(APICLASS, apiindex); + } } - private int getApiID(int apiClass, int 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 int lastSensorRead() { - CANData d = new CANData(); - readPacketLatest(getApiID(SENSOR_BRIDGE_API_CLASS, GET_READ), d); - return ((int)d.data[0]) & 0x00FF + ((((int)d.data[1]) & 0x00FF) << 8); + public TinCANBus(int deviceId) { + super(deviceId); + } + // 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; + } } From 2e3c040f24984b03de7411b6f16fe889d217a62f Mon Sep 17 00:00:00 2001 From: Adam Vander Pas Date: Wed, 10 May 2023 15:58:49 -0500 Subject: [PATCH 5/7] Fix APIID as final so it can't be reassigned --- src/main/java/frc/robot/hardware/TinCANBus.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/hardware/TinCANBus.java b/src/main/java/frc/robot/hardware/TinCANBus.java index b89c59bb..9ba2bd3b 100644 --- a/src/main/java/frc/robot/hardware/TinCANBus.java +++ b/src/main/java/frc/robot/hardware/TinCANBus.java @@ -33,7 +33,7 @@ private enum DEV_CONFIG_API_CLASS { SET_NEW_DEVICEID(1); private final int APICLASS = 0; - private int APIID; + private final int APIID; DEV_CONFIG_API_CLASS(int apiindex) { this.APIID = getApiID(APICLASS, apiindex); @@ -46,7 +46,7 @@ private enum SENSOR_BRIDGE_API_CLASS { STATUS(2); private final int APICLASS = 1; - private int APIID; + private final int APIID; SENSOR_BRIDGE_API_CLASS(int apiindex) { this.APIID = getApiID(APICLASS, apiindex); From 0b22fcb09bc393f462cfd5d7048b08d07d925298 Mon Sep 17 00:00:00 2001 From: Adam Vander Pas Date: Wed, 10 May 2023 16:46:57 -0500 Subject: [PATCH 6/7] Add docs and way to change device nums --- .../java/frc/robot/hardware/TinCANBus.java | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/hardware/TinCANBus.java b/src/main/java/frc/robot/hardware/TinCANBus.java index 9ba2bd3b..9bcb7748 100644 --- a/src/main/java/frc/robot/hardware/TinCANBus.java +++ b/src/main/java/frc/robot/hardware/TinCANBus.java @@ -7,7 +7,11 @@ import edu.wpi.first.hal.CANData; import edu.wpi.first.wpilibj.CAN; -/** Add your docs here. */ +/* + * 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 { /* @@ -29,7 +33,7 @@ public class TinCANBus extends CAN { //TODO implement this API's class methods - private enum DEV_CONFIG_API_CLASS { + private static enum DEV_CONFIG_API_CLASS { SET_NEW_DEVICEID(1); private final int APICLASS = 0; @@ -41,7 +45,7 @@ private enum DEV_CONFIG_API_CLASS { } - private enum SENSOR_BRIDGE_API_CLASS { + private static enum SENSOR_BRIDGE_API_CLASS { GET_READ(1), STATUS(2); @@ -73,6 +77,14 @@ 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() { From fd72a915c93d1c05c18692f6e102920cb05a9e76 Mon Sep 17 00:00:00 2001 From: Adam Vander Pas Date: Wed, 10 May 2023 16:48:01 -0500 Subject: [PATCH 7/7] Remove todo --- src/main/java/frc/robot/hardware/TinCANBus.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/hardware/TinCANBus.java b/src/main/java/frc/robot/hardware/TinCANBus.java index 9bcb7748..3345f38a 100644 --- a/src/main/java/frc/robot/hardware/TinCANBus.java +++ b/src/main/java/frc/robot/hardware/TinCANBus.java @@ -32,7 +32,6 @@ public class TinCANBus extends CAN { */ - //TODO implement this API's class methods private static enum DEV_CONFIG_API_CLASS { SET_NEW_DEVICEID(1);