Add stuff for CAN testing

src/main/java/frc4388/robot/Constants.java

@@ -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 CanDevice RIGHT_FRONT_WHEEL   = new CanDevice("RIGHT_FRONT_WHEEL", 2);
-            public static final int RIGHT_FRONT_STEER_ID = 3;
+            public static final CanDevice RIGHT_FRONT_STEER   = new CanDevice("RIGHT_FRONT_STEER", 3);
-            public static final int RIGHT_FRONT_ENCODER_ID = 10;
+            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 CanDevice LEFT_FRONT_WHEEL    = new CanDevice("LEFT_FRONT_WHEEL", 4);
-            public static final int LEFT_FRONT_STEER_ID = 5;
+            public static final CanDevice LEFT_FRONT_STEER    = new CanDevice("LEFT_FRONT_STEER", 5);
-            public static final int LEFT_FRONT_ENCODER_ID = 11;
+            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 CanDevice LEFT_BACK_WHEEL     = new CanDevice("LEFT_BACK_WHEEL", 6);
-            public static final int LEFT_BACK_STEER_ID = 7;
+            public static final CanDevice LEFT_BACK_STEER     = new CanDevice("LEFT_BACK_STEER", 7);
-            public static final int LEFT_BACK_ENCODER_ID = 12;
+            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 CanDevice RIGHT_BACK_WHEEL    = new CanDevice("RIGHT_BACK_WHEEL", 8);  
-            public static final int RIGHT_BACK_STEER_ID = 9;
+            public static final CanDevice RIGHT_BACK_STEER    = new CanDevice("RIGHT_BACK_STEER", 9);
-            public static final int RIGHT_BACK_ENCODER_ID = 13;
+            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 {

src/main/java/frc4388/robot/Robot.java

@@ -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());
   }
 }

src/main/java/frc4388/robot/RobotMap.java

@@ -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  leftFrontWheel    = new  TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL.id);
-    public final TalonFX leftFrontSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_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 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  rightFrontWheel   = new  TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL.id);
-    public final TalonFX rightFrontSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_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 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  leftBackWheel     = new  TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL.id);
-    public final TalonFX leftBackSteer = new TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_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 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  rightBackWheel    = new  TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL.id);
-    public final TalonFX rightBackSteer = new TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_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 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.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.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.rightBack  = new SwerveModule("Right Back Swerve Module",  rightBackWheel,  rightBackSteer,  rightBackEncoder,  SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET);
     }
 }

src/main/java/frc4388/utility/CanDevice.java

@@ -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;
+    }
+}

src/main/java/frc4388/utility/DeviceFinder.java

@@ -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;
+        }
+    }
+}