mirror of
https://github.com/Team4388/Robot-Essentials.git
synced 2026-06-08 16:28:02 -06:00
Add stuff for CAN testing
This commit is contained in:
Michael Mikovsky
@@ -7,7 +7,11 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import frc4388.utility.CanDevice;
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
|
||||
@@ -40,6 +44,8 @@ public final class Constants {
|
||||
public static final double FAST_SPEED = 0.5;
|
||||
public static final double TURBO_SPEED = 1.0;
|
||||
|
||||
// public static List<CanDevice> CAN_DEVICES = new ArrayList<>();
|
||||
|
||||
public static final class DefaultSwerveRotOffsets {
|
||||
public static final double FRONT_LEFT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
|
||||
public static final double FRONT_RIGHT_ROT_OFFSET = 0.0; //TODO: per robot swerve module offsets.
|
||||
@@ -48,23 +54,24 @@ public final class Constants {
|
||||
}
|
||||
|
||||
public static final class IDs {
|
||||
public static final int RIGHT_FRONT_WHEEL_ID = 2;
|
||||
public static final int RIGHT_FRONT_STEER_ID = 3;
|
||||
public static final int RIGHT_FRONT_ENCODER_ID = 10;
|
||||
public static final CanDevice RIGHT_FRONT_WHEEL = new CanDevice("RIGHT_FRONT_WHEEL", 2);
|
||||
public static final CanDevice RIGHT_FRONT_STEER = new CanDevice("RIGHT_FRONT_STEER", 3);
|
||||
public static final CanDevice RIGHT_FRONT_ENCODER = new CanDevice("RIGHT_FRONT_ENCODER", 10);
|
||||
|
||||
public static final int LEFT_FRONT_WHEEL_ID = 4;
|
||||
public static final int LEFT_FRONT_STEER_ID = 5;
|
||||
public static final int LEFT_FRONT_ENCODER_ID = 11;
|
||||
public static final CanDevice LEFT_FRONT_WHEEL = new CanDevice("LEFT_FRONT_WHEEL", 4);
|
||||
public static final CanDevice LEFT_FRONT_STEER = new CanDevice("LEFT_FRONT_STEER", 5);
|
||||
public static final CanDevice LEFT_FRONT_ENCODER = new CanDevice("LEFT_FRONT_ENCODER", 11);
|
||||
|
||||
public static final int LEFT_BACK_WHEEL_ID = 6;
|
||||
public static final int LEFT_BACK_STEER_ID = 7;
|
||||
public static final int LEFT_BACK_ENCODER_ID = 12;
|
||||
public static final CanDevice LEFT_BACK_WHEEL = new CanDevice("LEFT_BACK_WHEEL", 6);
|
||||
public static final CanDevice LEFT_BACK_STEER = new CanDevice("LEFT_BACK_STEER", 7);
|
||||
public static final CanDevice LEFT_BACK_ENCODER = new CanDevice("LEFT_BACK_ENCODER", 12);
|
||||
|
||||
public static final int RIGHT_BACK_WHEEL_ID = 8;
|
||||
public static final int RIGHT_BACK_STEER_ID = 9;
|
||||
public static final int RIGHT_BACK_ENCODER_ID = 13;
|
||||
public static final CanDevice RIGHT_BACK_WHEEL = new CanDevice("RIGHT_BACK_WHEEL", 8);
|
||||
public static final CanDevice RIGHT_BACK_STEER = new CanDevice("RIGHT_BACK_STEER", 9);
|
||||
public static final CanDevice RIGHT_BACK_ENCODER = new CanDevice("RIGHT_BACK_ENCODER", 13);
|
||||
|
||||
public static final int DRIVE_PIGEON_ID = 14;
|
||||
public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 4);
|
||||
public static final CanDevice e = new CanDevice("NONEXISTANT_CAN", 50);
|
||||
}
|
||||
|
||||
public static final class PIDConstants {
|
||||
|
||||
@@ -7,16 +7,19 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.util.Base64;
|
||||
import java.util.List;
|
||||
import java.util.logging.Level;
|
||||
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import frc4388.utility.CanDevice;
|
||||
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.robot.subsystems.LED;
|
||||
/**
|
||||
@@ -133,16 +136,39 @@ public class Robot extends TimedRobot {
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
|
||||
// Subsystems header
|
||||
System.out.println(new String(Base64.getDecoder().decode("IF9fICAgICAgIF8gICAgICAgICAgICAgICAgICAgXyAgICAgICAgICAgICAgICAgICAgIAovIF9cXyAgIF98IHxfXyAgX19fIF8gICBfIF9fX3wgfF8gX19fIF8gX18gX19fICBfX18gClwgXHwgfCB8IHwgJ18gXC8gX198IHwgfCAvIF9ffCBfXy8gXyBcICdfIGAgXyBcLyBfX3wKX1wgXCB8X3wgfCB8XykgXF9fIFwgfF98IFxfXyBcIHx8ICBfXy8gfCB8IHwgfCBcX18gXApcX18vXF9fLF98XyBfXy98X19fL1xfXywgfF9fXy9cX19cX19ffF98IHxffCB8X3xfX18vCiAgICAgICAgICAgICAgICAgICAgfF9fXy8gICAgICAgICAgICAgICAgICAgICAgICAgICA=")));
|
||||
|
||||
for(int i=0;i<m_robotContainer.subsystems.size();i++){
|
||||
|
||||
Subsystem subsystem = m_robotContainer.subsystems.get(i);
|
||||
subsystem.Log("Subsystem diagnostic report for " + subsystem.getName() + ":");
|
||||
System.out.println("** Subsystem diagnostic report for " + subsystem.getName() + ":");
|
||||
Status status = subsystem.diagnosticStatus();
|
||||
|
||||
for(int a=0;a<status.reports.size();a++){
|
||||
Report r = status.reports.get(i);
|
||||
Report r = status.reports.get(a);
|
||||
subsystem.Log(r.toString());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// CAN header
|
||||
System.out.println(new String(Base64.getDecoder().decode("ICAgX19fICAgXyAgICAgICAgX18gCiAgLyBfX1wgL19cICAgIC9cIFwgXAogLyAvICAgLy9fXFwgIC8gIFwvIC8KLyAvX19fLyAgXyAgXC8gL1wgIC8gIApcX19fXy9cXy8gXF8vXF9cIFwvICh0KQ==")));
|
||||
|
||||
|
||||
for(int i=0;i<CanDevice.devices.size();i++){
|
||||
|
||||
CanDevice device = CanDevice.devices.get(i);
|
||||
System.out.println("** CAN diagnostic report for " + device.name + ":");
|
||||
Status status = device.diagnosticStatus();
|
||||
|
||||
for(int a=0;a<status.reports.size();a++){
|
||||
Report r = status.reports.get(a);
|
||||
device.Log(r.toString());
|
||||
}
|
||||
}
|
||||
|
||||
System.out.println("Found CAN devices: " + new DeviceFinder().Find());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -22,7 +22,7 @@ import frc4388.utility.RobotGyro;
|
||||
* testing and modularization.
|
||||
*/
|
||||
public class RobotMap {
|
||||
private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON_ID);
|
||||
private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
|
||||
public RobotGyro gyro = new RobotGyro(m_pigeon2);
|
||||
|
||||
public SwerveModule leftFront;
|
||||
@@ -38,28 +38,28 @@ public class RobotMap {
|
||||
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
|
||||
/* Swreve Drive Subsystem */
|
||||
public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
|
||||
public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
|
||||
public final CANcoder leftFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
|
||||
public final TalonFX leftFrontWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL.id);
|
||||
public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER.id);
|
||||
public final CANcoder leftFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER.id);
|
||||
|
||||
|
||||
public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
|
||||
public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
|
||||
public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
|
||||
public final TalonFX rightFrontWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL.id);
|
||||
public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER.id);
|
||||
public final CANcoder rightFrontEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER.id);
|
||||
|
||||
public final TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
|
||||
public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
|
||||
public final CANcoder leftBackEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
|
||||
public final TalonFX leftBackWheel = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL.id);
|
||||
public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER.id);
|
||||
public final CANcoder leftBackEncoder = new CANcoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER.id);
|
||||
|
||||
public final TalonFX rightBackWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
|
||||
public final TalonFX rightBackSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
|
||||
public final CANcoder rightBackEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
|
||||
public final TalonFX rightBackWheel = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL.id);
|
||||
public final TalonFX rightBackSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER.id);
|
||||
public final CANcoder rightBackEncoder = new CANcoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER.id);
|
||||
|
||||
void configureDriveMotorControllers() {
|
||||
// initialize SwerveModules
|
||||
this.rightFront = new SwerveModule("Right Front Swerve Module", rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET);
|
||||
this.leftFront = new SwerveModule("Left Front Swerve Module", leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
|
||||
this.leftBack = new SwerveModule("Left Back Swerve Module", leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET);
|
||||
this.rightBack = new SwerveModule("Right Back Swerve Module", rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET);
|
||||
this.leftFront = new SwerveModule("Left Front Swerve Module", leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
|
||||
this.leftBack = new SwerveModule("Left Back Swerve Module", leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET);
|
||||
this.rightBack = new SwerveModule("Right Back Swerve Module", rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,51 @@
|
||||
package frc4388.utility;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import edu.wpi.first.hal.CANData;
|
||||
import edu.wpi.first.hal.can.CANJNI;
|
||||
import edu.wpi.first.wpilibj.CAN;
|
||||
import frc4388.utility.Status.Report;
|
||||
import frc4388.utility.Status.ReportLevel;
|
||||
|
||||
public class CanDevice {
|
||||
public static List<CanDevice> devices = new ArrayList<>();
|
||||
|
||||
public String name;
|
||||
public int id;
|
||||
|
||||
public CanDevice(String name, int id) {
|
||||
this.name = name;
|
||||
this.id = id;
|
||||
|
||||
devices.add(this);
|
||||
}
|
||||
|
||||
|
||||
private boolean isAlive() {
|
||||
return true; //TODO: Link this with Device Finder
|
||||
}
|
||||
|
||||
public void Log(String str){
|
||||
System.out.println("CAN ID " + this.id + " ( " + this.name + " ) " + str);
|
||||
}
|
||||
|
||||
public Status queryStatus() {
|
||||
Status s = new Status();
|
||||
|
||||
s.addReport(ReportLevel.INFO, "TODO");
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
public Status diagnosticStatus() {
|
||||
Status s = new Status();
|
||||
//TODO
|
||||
s.addReport(ReportLevel.INFO, "Add CAN magic here");
|
||||
boolean isAlive = isAlive();
|
||||
s.addReport(isAlive ? ReportLevel.INFO : ReportLevel.ERROR, "Is Alive: " + isAlive);
|
||||
|
||||
return s;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,161 @@
|
||||
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<String> deviceList = new ArrayList<String>();
|
||||
|
||||
public ArrayList<String> Find() {
|
||||
return deviceList;
|
||||
}
|
||||
|
||||
private boolean pdpIsPresent = false;
|
||||
private Set<Integer> srxs = new TreeSet<>();
|
||||
private Set<Integer> spxs = new TreeSet<>();
|
||||
private Set<Integer> 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<String> 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user