Dev splines test 2022

Util.java

@@ -1,5 +1,7 @@
 package org.usfirst.frc4904.standard;
 
+import java.util.concurrent.TimeUnit;
+
 import edu.wpi.first.hal.util.BoundaryException;
 
 /**
@@ -11,7 +13,7 @@ public class Util {
 	 * for floating point numbers, whose arithmetic operations tend to introduce
 	 * small errors.
 	 *
-
+	 *
 	 * @param value   The floating point number to be compared
 	 * @param epsilon The maximum magnitude of var such that it can be considered
 	 *                zero
@@ -21,7 +23,6 @@ public static boolean isZero(double value, double epsilon) {
 		return Math.abs(value) < epsilon;
 	}
 
-
 	/**
 	 * Returns true if {@code value} is less than {@code epsilon}. This is useful
 	 * for floating point numbers, whose arithmetic operations tend to introduce
@@ -87,4 +88,25 @@ public double limitValue(double value) {
 			return Math.max(Math.min(value, max), min);
 		}
 	}
+
+	/**
+	 * Computes the conversion factor between the first and second {@link TimeUnit}
+	 * given.
+	 *
+	 * @param from the source unit
+	 * @param to   the target unit
+	 * @return the conversion factor
+	 */
+	public static double timeConversionFactor(TimeUnit from, TimeUnit to) {
+		// TimeUnit.convert returns a long.
+		// If from >= to (in terms of units), we can simply use normal conversion, as it
+		// will scale up.
+		// Otherwise, invert the conversion and take the reciprocal.
+
+		if (from.compareTo(to) >= 0) {
+			return (double) to.convert(1, from);
+		} else {
+			return 1.0 / from.convert(1, to);
+		}
+	}
 }

commands/chassis/SimpleSplines.java

@@ -23,21 +23,32 @@
 
 public class SimpleSplines extends SequentialCommandGroup {
   public SimpleSplines(SplinesDrive robotDrive, Pose2d init_pos, List<Translation2d> inter_points, Pose2d final_pos, double maxVoltage, Command nextCommand){
-    super(new RamseteCommand(
-        TrajectoryGenerator.generateTrajectory(init_pos, inter_points, final_pos, new TrajectoryConfig(robotDrive.getAutoConstants().kMaxSpeedMetersPerSecond,
-        robotDrive.getAutoConstants().kMaxAccelerationMetersPerSecondSquared)
-            .setKinematics(robotDrive.getDriveConstants().kDriveKinematics)
-            .addConstraint(new DifferentialDriveVoltageConstraint(
-              new SimpleMotorFeedforward(robotDrive.getDriveConstants().ksVolts, 
-              robotDrive.getDriveConstants().kvVoltSecondsPerMeter, 
-              robotDrive.getDriveConstants().kaVoltSecondsSquaredPerMeter), 
+    super(
+      new RamseteCommand(
+        TrajectoryGenerator.generateTrajectory(init_pos, inter_points, final_pos,
+          new TrajectoryConfig(
+            robotDrive.getAutoConstants().kMaxSpeedMetersPerSecond,
+            robotDrive.getAutoConstants().kMaxAccelerationMetersPerSecondSquared
+          ).setKinematics(robotDrive.getDriveConstants().kDriveKinematics
+          ).addConstraint(
+            new DifferentialDriveVoltageConstraint(
+              new SimpleMotorFeedforward(
+                robotDrive.getDriveConstants().ksVolts, 
+                robotDrive.getDriveConstants().kvVoltSecondsPerMeter, 
+                robotDrive.getDriveConstants().kaVoltSecondsSquaredPerMeter
+              ), 
               robotDrive.getDriveConstants().kDriveKinematics, 
-              maxVoltage))),
+              maxVoltage
+            )
+          )
+        ),
         robotDrive::getPose,
         new RamseteController(robotDrive.getAutoConstants().kRamseteB, robotDrive.getAutoConstants().kRamseteZeta),
-        new SimpleMotorFeedforward(robotDrive.getDriveConstants().ksVolts,
-        robotDrive.getDriveConstants().kvVoltSecondsPerMeter,
-        robotDrive.getDriveConstants().kaVoltSecondsSquaredPerMeter),
+        new SimpleMotorFeedforward(
+          robotDrive.getDriveConstants().ksVolts,
+          robotDrive.getDriveConstants().kvVoltSecondsPerMeter,
+          robotDrive.getDriveConstants().kaVoltSecondsSquaredPerMeter
+        ),
         robotDrive.getDriveConstants().kDriveKinematics,
         robotDrive::getWheelSpeeds,
         new PIDController(robotDrive.getDriveConstants().kPDriveVel, 0, 0),

custom/sensors/CANTalonEncoder.java

@@ -10,7 +10,7 @@
 import org.usfirst.frc4904.standard.custom.CustomPIDSourceType;
 
 /**
- * Encoder class for the Built-in Encoders on Talon Motor Controllers 
+ * Encoder class for the Built-in Encoders on Talon Motor Controllers
  * Works for both Falcons (CANTalonFX) and SRXs (CANTalonSRX)
  */
 public class CANTalonEncoder implements CustomEncoder {
@@ -123,13 +123,14 @@ public boolean getStopped() {
 		return Util.isZero(getRate());
 	}
 
-	public int getRevLimitSwitchClosed() {
-		return talon.isRevLimitSwitchClosed();
+	public boolean isRevLimitSwitchClosed() {
+		return talon.isRevLimitSwitchClosed() == 1;
 	}
-	public int getFwdLimitSwitchClosed() {
-		return talon.isFwdLimitSwitchClosed();
+
+	public boolean isFwdLimitSwitchClosed() {
+		return talon.isFwdLimitSwitchClosed() == 1;
 	}
-	
+
 	@Override
 	public double getRate() {
 		if (reverseDirection) {

subsystems/chassis/SplinesDrive.java

@@ -7,12 +7,10 @@
 
 package org.usfirst.frc4904.standard.subsystems.chassis;
 
-import javax.naming.InitialContext;
-
 import com.ctre.phoenix.sensors.CANCoder;
-import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder;
 
 import org.usfirst.frc4904.standard.commands.chassis.SimpleSplines;
+import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder;
 import org.usfirst.frc4904.standard.custom.sensors.IMU;
 import org.usfirst.frc4904.standard.subsystems.chassis.TankDrive;
 import org.usfirst.frc4904.standard.subsystems.motor.Motor;

subsystems/net/UDPSocket.java

@@ -0,0 +1,71 @@
+package org.usfirst.frc4904.standard.subsystems.net;
+
+import java.io.IOException;
+import java.net.SocketAddress;
+import java.nio.ByteBuffer;
+import java.nio.channels.DatagramChannel;
+
+import org.msgpack.core.MessageBufferPacker;
+import org.msgpack.core.MessagePack;
+import org.msgpack.core.MessageUnpacker;
+import org.msgpack.core.buffer.ByteBufferInput;
+import org.msgpack.core.buffer.MessageBuffer;
+import org.usfirst.frc4904.standard.LogKitten;
+import org.usfirst.frc4904.standard.subsystems.net.message.Packable;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public abstract class UDPSocket extends SubsystemBase {
+    private static final int RECV_BUFFER_SIZE = 1024;
+
+    private DatagramChannel channel;
+
+    public UDPSocket(SocketAddress address) throws IOException {
+        final DatagramChannel channel = DatagramChannel.open().bind(address);
+        channel.configureBlocking(false);
+
+        this.channel = channel;
+    }
+
+    protected void send(SocketAddress address, Packable message) throws IOException {
+        final MessageBufferPacker packer = MessagePack.newDefaultBufferPacker();
+
+        message.pack(packer);
+        packer.close();
+
+        final MessageBuffer messageBuffer = packer.toMessageBuffer();
+
+        channel.send(messageBuffer.sliceAsByteBuffer(), address);
+    }
+
+    protected abstract void receive(SocketAddress address, MessageUnpacker unpacker) throws IOException;
+
+    private void pollReceive() throws IOException {
+        final ByteBuffer buffer = ByteBuffer.allocate(RECV_BUFFER_SIZE);
+        final MessageUnpacker unpacker = MessagePack.newDefaultUnpacker(buffer);
+
+        while (true) {
+            final SocketAddress address = channel.receive(buffer);
+
+            if (address == null) {
+                break;
+            }
+
+            receive(address, unpacker);
+
+            buffer.clear();
+            unpacker.reset(new ByteBufferInput(buffer));
+        }
+
+        unpacker.close();
+    }
+
+    @Override
+    public void periodic() {
+        try {
+            pollReceive();
+        } catch (IOException ex) {
+            LogKitten.ex(ex);
+        }
+    }
+}

subsystems/net/message/Packable.java

@@ -0,0 +1,9 @@
+package org.usfirst.frc4904.standard.subsystems.net.message;
+
+import java.io.IOException;
+
+import org.msgpack.core.MessagePacker;
+
+public interface Packable {
+    void pack(MessagePacker packer) throws IOException;
+}

subsystems/net/message/Unpackable.java

@@ -0,0 +1,9 @@
+package org.usfirst.frc4904.standard.subsystems.net.message;
+
+import java.io.IOException;
+
+import org.msgpack.core.MessageUnpacker;
+
+public interface Unpackable {
+    void unpack(MessageUnpacker unpacker) throws IOException;
+}