From a84142389407df0df02ad87a4fc941adb2078f9d Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Mon, 28 Feb 2022 17:18:22 -0800 Subject: [PATCH 001/134] Removed comment --- udp/tests/TestUDPServer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/udp/tests/TestUDPServer.java b/udp/tests/TestUDPServer.java index 25a34d4a..8350458d 100644 --- a/udp/tests/TestUDPServer.java +++ b/udp/tests/TestUDPServer.java @@ -32,7 +32,6 @@ public class TestUDPServer extends Server { protected void decode(byte[] data) throws IOException{ MessageUnpacker unpacker = MessagePack.newDefaultUnpacker(data); - //unpacker.unp int firstInteger = unpacker.unpackInt(); String firstString = unpacker.unpackString(); // 1 int numberOfStuff = unpacker.unpackArrayHeader(); // 2 From da708650c1f9d30b3d3ddc677056a17270ffbd02 Mon Sep 17 00:00:00 2001 From: aemino Date: Tue, 22 Mar 2022 16:43:19 -0700 Subject: [PATCH 002/134] rewrite UDP implementation --- subsystems/net/UDPSocket.java | 71 +++++++++++++++++ subsystems/net/message/Packable.java | 9 +++ subsystems/net/message/Unpackable.java | 9 +++ udp/Client.java | 105 ------------------------- udp/Server.java | 93 ---------------------- udp/tests/TestUDPServer.java | 52 ------------ udp/tests/UDPTest.java | 85 -------------------- 7 files changed, 89 insertions(+), 335 deletions(-) create mode 100644 subsystems/net/UDPSocket.java create mode 100644 subsystems/net/message/Packable.java create mode 100644 subsystems/net/message/Unpackable.java delete mode 100644 udp/Client.java delete mode 100644 udp/Server.java delete mode 100644 udp/tests/TestUDPServer.java delete mode 100644 udp/tests/UDPTest.java diff --git a/subsystems/net/UDPSocket.java b/subsystems/net/UDPSocket.java new file mode 100644 index 00000000..725638b5 --- /dev/null +++ b/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); + } + } +} diff --git a/subsystems/net/message/Packable.java b/subsystems/net/message/Packable.java new file mode 100644 index 00000000..270c68ea --- /dev/null +++ b/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; +} diff --git a/subsystems/net/message/Unpackable.java b/subsystems/net/message/Unpackable.java new file mode 100644 index 00000000..be5acd5a --- /dev/null +++ b/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; +} diff --git a/udp/Client.java b/udp/Client.java deleted file mode 100644 index ac01a34e..00000000 --- a/udp/Client.java +++ /dev/null @@ -1,105 +0,0 @@ -package org.usfirst.frc4904.standard.udp; - -import java.nio.charset.StandardCharsets; - -import java.io.IOException; -import java.net.DatagramPacket; -import java.net.DatagramSocket; -import java.net.InetAddress; -import java.util.Arrays; -import java.util.HashMap; - -import java.io.IOException; - - -import org.msgpack.core.MessagePack; -import org.msgpack.core.MessagePack.PackerConfig; -import org.msgpack.core.MessagePack.UnpackerConfig; -import org.msgpack.core.MessageBufferPacker; -import org.msgpack.core.MessageFormat; -import org.msgpack.core.MessagePacker; -import org.msgpack.core.MessageUnpacker; -import org.msgpack.value.ArrayValue; -import org.msgpack.value.ExtensionValue; -import org.msgpack.value.FloatValue; -import org.msgpack.value.IntegerValue; -import org.msgpack.value.TimestampValue; -import org.msgpack.value.Value; -import org.usfirst.frc4904.standard.LogKitten; - -import java.io.File; -import java.io.FileInputStream; -import java.io.FileOutputStream; -import java.math.BigInteger; -import java.time.Instant; - -//import com.fasterxml.jackson.core.JsonProcessingException; -//import com.fasterxml.jackson.databind.ObjectMapper; - -public class Client { - private DatagramSocket socket; - private InetAddress address; - private byte[] header; - private int socketNum; - - private byte[] buf; - - public Client(String hostname, int sourcePort, int socketNum) { - try { - address = InetAddress.getByName(hostname); - socket = new DatagramSocket(sourcePort); - - this.socketNum = socketNum; - } catch (IOException e) { - e.printStackTrace(); - } - } - - public void sendEcho(String msg) { - //System.out.println("Sending Echo: " + "'" + msg + "'."); - DatagramPacket packet = null; - try { - byte[] buf = msg.getBytes("UTF-8"); - packet = new DatagramPacket(buf, buf.length, address, socketNum); - socket.send(packet); - // packet = new DatagramPacket(buf, buf.length); - // socket.receive(packet); - } catch (IOException e) { - LogKitten.wtf("Echo failed" + e.getMessage()); - e.printStackTrace(); - } - } - - public void sendGenericEcho(MessageBufferPacker map) { - byte[] convertedMap; - convertedMap = map.toByteArray(); - - System.out.println("Sending Echo: " + "'" + new String(convertedMap, StandardCharsets.US_ASCII) + "'."); - DatagramPacket packet = null; - try { - byte[] buf = convertedMap; - System.out.println(buf); - packet = new DatagramPacket(buf, buf.length, address, socketNum); - socket.send(packet); - } catch (IOException e) { - System.out.println("Echo failed"); - e.printStackTrace(); - } - } - - public String receiveData() { - DatagramPacket packet = new DatagramPacket(new byte[256], 256); - try { - socket.receive(packet); - String received = new String(packet.getData(), 0, packet.getLength()); - return received; - } catch (IOException e) { - e.printStackTrace(); - return null; - } - } - - public void close() { - socket.close(); - } -} diff --git a/udp/Server.java b/udp/Server.java deleted file mode 100644 index 13ab56f8..00000000 --- a/udp/Server.java +++ /dev/null @@ -1,93 +0,0 @@ -package org.usfirst.frc4904.standard.udp; - -import java.net.DatagramPacket; -import java.net.DatagramSocket; -import java.net.InetAddress; -import java.util.Arrays; -import java.nio.charset.StandardCharsets; - - - -import org.msgpack.core.MessagePack; -import org.msgpack.core.MessagePack.PackerConfig; -import org.msgpack.core.MessagePack.UnpackerConfig; -import org.msgpack.core.MessageBufferPacker; -import org.msgpack.core.MessageFormat; -import org.msgpack.core.MessagePacker; -import org.msgpack.core.MessageUnpacker; -import org.msgpack.value.ArrayValue; -import org.msgpack.value.ExtensionValue; -import org.msgpack.value.FloatValue; -import org.msgpack.value.IntegerValue; -import org.msgpack.value.TimestampValue; -import org.msgpack.value.Value; - -import java.io.File; -import java.io.FileInputStream; -import java.io.FileOutputStream; -import java.io.IOException; -import java.math.BigInteger; -import java.time.Instant; - -import com.fasterxml.jackson.databind.ObjectMapper; - -import java.io.*; - -/* -Messagepack generelization plan: - -Take in hashmap, put keys and stuff in a string header and deserialize accordingly on the other side. Serialize in by type. Or just use generics lmao. -*/ - -abstract public class Server extends Thread { - - protected DatagramSocket socket = null; - protected boolean running; - protected byte[] buf = new byte[256]; - protected String expectedString = "test"; - protected String serverHeader = "##SERVER"; - protected Boolean debug = true; - // TODO we shouldn't need a hostname here but I don't want to deal with it - public Server(int socketNum, String hostname) throws IOException { - socket = new DatagramSocket(socketNum); - } - - protected abstract void decode(byte[] message) throws IOException; - - public void run() { - System.out.println("Server running."); - running = true; - while (running) { - try { - DatagramPacket packet = new DatagramPacket(buf, buf.length); - socket.receive(packet); - byte[] received = packet.getData(); - byte[] data = Arrays.copyOfRange(received, 8, packet.getLength()); - String header = new String(Arrays.copyOfRange(received, 0, 8)); - System.out.println( - "Received: '" + data + "', length: " + data.length + ", from client: '" + header + "'."); - decode(data); - - InetAddress address = packet.getAddress(); - int port = packet.getPort(); - byte[] tempArr = new byte[buf.length]; - int index = 0; - for (byte byt : this.serverHeader.getBytes("UTF-8")) { - tempArr[index] = byt; - index++; - } - for (byte byt : data) { - tempArr[index] = byt; - index++; - } - // packet = new DatagramPacket(tempArr, tempArr.length, address, port); - // socket.send(packet); - buf = new byte[256]; - } catch (IOException e) { - e.printStackTrace(); - running = false; - } - } - socket.close(); - } -} \ No newline at end of file diff --git a/udp/tests/TestUDPServer.java b/udp/tests/TestUDPServer.java deleted file mode 100644 index ad5043d3..00000000 --- a/udp/tests/TestUDPServer.java +++ /dev/null @@ -1,52 +0,0 @@ -package org.usfirst.frc4904.standard.udp.tests; - -import java.io.IOException; - -import org.usfirst.frc4904.standard.udp.Server; - -import org.msgpack.core.MessagePack; -import org.msgpack.core.MessagePack.PackerConfig; -import org.msgpack.core.MessagePack.UnpackerConfig; -import org.msgpack.core.MessageBufferPacker; -import org.msgpack.core.MessageFormat; -import org.msgpack.core.MessagePacker; -import org.msgpack.core.MessageUnpacker; -import org.msgpack.value.ArrayValue; -import org.msgpack.value.ExtensionValue; -import org.msgpack.value.FloatValue; -import org.msgpack.value.IntegerValue; -import org.msgpack.value.TimestampValue; -import org.msgpack.value.Value; -import org.usfirst.frc4904.standard.udp.Client; -import org.usfirst.frc4904.standard.udp.Server; - -import java.io.*; -import java.util.HashMap; - - -public class TestUDPServer extends Server { - String testThreadGlobal; - TestUDPServer(int SocketNum, String hostname) throws IOException{ - super(SocketNum, hostname); - } - - protected void decode(byte[] data) throws IOException{ - MessageUnpacker unpacker = MessagePack.newDefaultUnpacker(data); - //unpacker.unp - int firstInteger = unpacker.unpackInt(); - String firstString = unpacker.unpackString(); // 1 - int numberOfStuff = unpacker.unpackArrayHeader(); // 2 - String[] terminalTextEditors = new String[numberOfStuff]; - for (int i = 0; i < numberOfStuff; ++i) { - terminalTextEditors[i] = unpacker.unpackString(); // terminalTextEditors = {"vim", "nano"} - } - unpacker.close(); - - System.out.println(String.format("Integer: %d String: %s ArrayElementOne: %s ArrayElementTwo: %s",firstInteger, firstString, terminalTextEditors[0], terminalTextEditors[1])); - if (terminalTextEditors[0] == "vim") { - System.out.println("It's a SUCCESS!"); - running = false; - } - testThreadGlobal = String.format("eeeeemaaacs and also %d", firstInteger); - } -} diff --git a/udp/tests/UDPTest.java b/udp/tests/UDPTest.java deleted file mode 100644 index 92dafa97..00000000 --- a/udp/tests/UDPTest.java +++ /dev/null @@ -1,85 +0,0 @@ -package org.usfirst.frc4904.standard.udp.tests; - - -//TODO fix imports -import org.msgpack.core.MessagePack; -import org.msgpack.core.MessagePack.PackerConfig; -import org.msgpack.core.MessagePack.UnpackerConfig; -import org.msgpack.core.MessageBufferPacker; -import org.msgpack.core.MessageFormat; -import org.msgpack.core.MessagePacker; -import org.msgpack.core.MessageUnpacker; -import org.msgpack.value.ArrayValue; -import org.msgpack.value.ExtensionValue; -import org.msgpack.value.FloatValue; -import org.msgpack.value.IntegerValue; -import org.msgpack.value.TimestampValue; -import org.msgpack.value.Value; -import org.usfirst.frc4904.standard.udp.Client; -import org.usfirst.frc4904.standard.udp.Server; -import java.util.concurrent.TimeUnit; - -import java.io.*; -import java.util.HashMap; - -public class UDPTest { - Client client; - TestUDPServer server; - private int sourcePort = 3375; - private int receivePort = 8765; - private int destinationPort = 4321; - - public void setup() { - System.out.println("Setting up the test on socket #" + receivePort + "."); - try { - server = new TestUDPServer(receivePort, "NUS12738-12-aksramks-MacBook-Pro.local"); - server.start(); - client = new Client("NUS12738-12-aksramks-MacBook-Pro.local", sourcePort, destinationPort); - } catch (IOException ex) { - System.out.println("ERR: IOException during setup. This error is from creating the Server."); - ex.printStackTrace(); - } - } - - public void encode() throws IOException { - MessageBufferPacker packer = MessagePack.newDefaultBufferPacker(); - packer - .packArrayHeader(2) - - .packArrayHeader(3) - - .packArrayHeader(2) - - - .packDouble(2.0) - .packArrayHeader(2) - .packDouble(1.0) - - .packDouble(1.0) - - .packArrayHeader(2) - - - .packDouble(2.0) - .packArrayHeader(2) - .packDouble(1.0) - - .packDouble(1.0) - - .packDouble(0.0) - .packDouble(0.0); - - - packer.close(); - client.sendGenericEcho(packer); - client.close(); - } - - public static void main(String[] args) throws IOException - { - UDPTest udpTest = new UDPTest(); - udpTest.setup(); - udpTest.encode(); - System.out.println(udpTest.server.testThreadGlobal); // This is a little bit sketchy since UDP is technically async - } -} \ No newline at end of file From 8a55e0e7acecc74448c7b8e74a4eec05594c7fa3 Mon Sep 17 00:00:00 2001 From: aemino Date: Fri, 25 Mar 2022 01:48:22 -0700 Subject: [PATCH 003/134] add time conversion factor utility --- Util.java | 26 ++++++++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/Util.java b/Util.java index 93f6797e..9edf7e3b 100644 --- a/Util.java +++ b/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); + } + } } From f5e51e2af719a44c5f4c016e8b758a623e18b731 Mon Sep 17 00:00:00 2001 From: aemino Date: Fri, 25 Mar 2022 02:08:32 -0700 Subject: [PATCH 004/134] improve CANTalonEncoder limit switch getter methods --- custom/sensors/CANTalonEncoder.java | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/custom/sensors/CANTalonEncoder.java b/custom/sensors/CANTalonEncoder.java index 265635cc..3283a6a6 100644 --- a/custom/sensors/CANTalonEncoder.java +++ b/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) { From 726bb4ce1fb552fcb0555a574bb219187bb56ff0 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 <102199775+roboticsteam4904-2@users.noreply.github.com> Date: Sun, 15 Jan 2023 10:10:46 -0800 Subject: [PATCH 005/134] init from driver station --- commands/RunFor.java | 6 ++++++ commands/motor/MotorConstant.java | 2 ++ 2 files changed, 8 insertions(+) diff --git a/commands/RunFor.java b/commands/RunFor.java index 84a72f5d..1a98ec63 100644 --- a/commands/RunFor.java +++ b/commands/RunFor.java @@ -1,6 +1,9 @@ package org.usfirst.frc4904.standard.commands; import edu.wpi.first.wpilibj2.command.Subsystem; + +import org.usfirst.frc4904.standard.LogKitten; + import edu.wpi.first.wpilibj2.command.CommandBase; public class RunFor extends CommandBase { @@ -54,8 +57,11 @@ public void initialize() { public boolean isFinished() { if (firstTick) { firstTick = false; + LogKitten.wtf(" runfor first tick check " + isTimedOut() + " " + !command.isScheduled()); return isTimedOut(); } + LogKitten.wtf(" runfor end check " + isTimedOut() + " " + !command.isScheduled()); + if (isTimedOut() || !command.isScheduled()) LogKitten.wtf("runfor endinggg"); return isTimedOut() || !command.isScheduled(); } diff --git a/commands/motor/MotorConstant.java b/commands/motor/MotorConstant.java index 6b08630b..86098f56 100644 --- a/commands/motor/MotorConstant.java +++ b/commands/motor/MotorConstant.java @@ -1,5 +1,6 @@ package org.usfirst.frc4904.standard.commands.motor; +import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.subsystems.motor.Motor; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -35,6 +36,7 @@ public MotorConstant(Motor motor, double motorSpeed) { @Override public void initialize() { motor.set(motorSpeed); + LogKitten.wtf("emacs"); } @Override From 7885cfb68f946b3823bfe0a7d39b3f196c4cbfc8 Mon Sep 17 00:00:00 2001 From: jac0rr Date: Mon, 16 Jan 2023 11:08:16 -0800 Subject: [PATCH 006/134] Added odometry to SimpleSplines and also added value for trackwidth --- commands/chassis/SimpleSplines.java | 1 + 1 file changed, 1 insertion(+) diff --git a/commands/chassis/SimpleSplines.java b/commands/chassis/SimpleSplines.java index 28443179..10949518 100644 --- a/commands/chassis/SimpleSplines.java +++ b/commands/chassis/SimpleSplines.java @@ -44,6 +44,7 @@ public SimpleSplines(SplinesDrive robotDrive, Pose2d init_pos, List inter_points, Pose2d final_pos, double maxVoltage){ From 97365a3c415d68b2fb3af15c0fa1441643c89f55 Mon Sep 17 00:00:00 2001 From: jac0rr Date: Mon, 16 Jan 2023 14:49:22 -0800 Subject: [PATCH 007/134] Now logs yaw on b press --- LogKitten.java | 1 + 1 file changed, 1 insertion(+) diff --git a/LogKitten.java b/LogKitten.java index 2fc96bd8..16bf4886 100644 --- a/LogKitten.java +++ b/LogKitten.java @@ -10,6 +10,7 @@ import java.text.SimpleDateFormat; import java.util.Date; import edu.wpi.first.hal.HAL; +import edu.wpi.first.wpilibj2.command.Command; public class LogKitten { private static BufferedOutputStream fileOutput; From 92a9a77d147b2275a6469ca5e1444beb918b30b2 Mon Sep 17 00:00:00 2001 From: NUS16474-9-eriwang Date: Fri, 20 Jan 2023 17:36:52 -0800 Subject: [PATCH 008/134] hopefully splines fixed --- subsystems/chassis/SensorDrive.java | 15 ++++++++------- subsystems/chassis/SplinesDrive.java | 3 ++- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/subsystems/chassis/SensorDrive.java b/subsystems/chassis/SensorDrive.java index 4c35b476..75134b3a 100644 --- a/subsystems/chassis/SensorDrive.java +++ b/subsystems/chassis/SensorDrive.java @@ -9,15 +9,16 @@ import com.ctre.phoenix.sensors.CANCoder; +import org.usfirst.frc4904.robot.RobotMap; import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.commands.chassis.SimpleSplines; import org.usfirst.frc4904.standard.custom.CustomPIDSourceType; -import org.usfirst.frc4904.standard.custom.sensors.IMU; import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; import org.usfirst.frc4904.standard.custom.sensors.PIDSensor; import org.usfirst.frc4904.standard.subsystems.chassis.TankDrive; import org.usfirst.frc4904.standard.subsystems.motor.Motor; import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder; +import org.usfirst.frc4904.standard.custom.sensors.NavX; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -31,7 +32,7 @@ public class SensorDrive implements Subsystem, PIDSensor { // Based largely on private final TankDrive driveBase; private final CANTalonEncoder leftEncoder; private final CANTalonEncoder rightEncoder; - private final IMU gyro; + private final NavX gyro; private boolean refresh = true; private final DifferentialDriveOdometry odometry; private CustomPIDSourceType sensorType; @@ -39,7 +40,7 @@ public class SensorDrive implements Subsystem, PIDSensor { // Based largely on /** * Creates a new DriveSubsystem. */ - public SensorDrive(TankDrive driveBase, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, IMU gyro, + public SensorDrive(TankDrive driveBase, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, NavX gyro, CustomPIDSourceType sensorType, Pose2d initialPose) { this.driveBase = driveBase; this.leftEncoder = leftEncoder; @@ -48,17 +49,17 @@ public SensorDrive(TankDrive driveBase, CANTalonEncoder leftEncoder, CANTalonEnc setCustomPIDSourceType(sensorType); resetEncoders(); - odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()), initialPose); + odometry = new DifferentialDriveOdometry(gyro.getRotation2d(), initialPose); CommandScheduler.getInstance().registerSubsystem(this); } - public SensorDrive(TankDrive driveBase, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, IMU gyro, Pose2d initialPose) { + public SensorDrive(TankDrive driveBase, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, NavX gyro, Pose2d initialPose) { this(driveBase, leftEncoder, rightEncoder, gyro, CustomPIDSourceType.kDisplacement, initialPose); } @Override public void periodic() { - odometry.update(Rotation2d.fromDegrees(getHeading()), leftEncoder.getDistance(), rightEncoder.getDistance()); + odometry.update(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance()); } @Override @@ -111,7 +112,7 @@ public DifferentialDriveWheelSpeeds getWheelSpeeds() { */ public void resetOdometry(Pose2d pose) { resetEncoders(); - odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading())); + odometry.resetPosition(pose, gyro.getRotation2d()); } /** diff --git a/subsystems/chassis/SplinesDrive.java b/subsystems/chassis/SplinesDrive.java index fcf37937..9ea1cca3 100644 --- a/subsystems/chassis/SplinesDrive.java +++ b/subsystems/chassis/SplinesDrive.java @@ -16,6 +16,7 @@ import org.usfirst.frc4904.standard.custom.sensors.IMU; import org.usfirst.frc4904.standard.subsystems.chassis.TankDrive; import org.usfirst.frc4904.standard.subsystems.motor.Motor; +import org.usfirst.frc4904.standard.custom.sensors.NavX; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -28,7 +29,7 @@ public class SplinesDrive extends SensorDrive { /** * Creates a new DriveSubsystem. */ - public SplinesDrive(TankDrive driveBase, SimpleSplines.AutoConstants autoConstants, SimpleSplines.DriveConstants driveConstants, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, IMU gyro, Pose2d initialPose) { + public SplinesDrive(TankDrive driveBase, SimpleSplines.AutoConstants autoConstants, SimpleSplines.DriveConstants driveConstants, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, NavX gyro, Pose2d initialPose) { super(driveBase,leftEncoder, rightEncoder, gyro, initialPose); this.autoConstants = autoConstants; this.driveConstants = driveConstants; From 39fff8fdb98ea15bf0497995a8a8188b0ba6340f Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Fri, 20 Jan 2023 20:37:50 -0600 Subject: [PATCH 009/134] Reverted extraneous logging --- build.xml | 2 +- custom/PCMPort.java | 1 - subsystems/chassis/TankDriveShifting.java | 2 -- 3 files changed, 1 insertion(+), 4 deletions(-) diff --git a/build.xml b/build.xml index 78b56376..4c0e0032 100644 --- a/build.xml +++ b/build.xml @@ -29,4 +29,4 @@ - \ No newline at end of file + diff --git a/custom/PCMPort.java b/custom/PCMPort.java index 6556e37c..e1f9b191 100644 --- a/custom/PCMPort.java +++ b/custom/PCMPort.java @@ -26,7 +26,6 @@ public PCMPort(int pcmID, PneumaticsModuleType pcmType, int forward, int reverse } public DoubleSolenoid buildDoubleSolenoid() { - LogKitten.wtf("built the Double Solenoid"); return new DoubleSolenoid(pcmID, pcmType, forward, reverse); } } diff --git a/subsystems/chassis/TankDriveShifting.java b/subsystems/chassis/TankDriveShifting.java index 9d40976a..5f449da6 100644 --- a/subsystems/chassis/TankDriveShifting.java +++ b/subsystems/chassis/TankDriveShifting.java @@ -1,6 +1,5 @@ package org.usfirst.frc4904.standard.subsystems.chassis; -import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.subsystems.motor.Motor; /** @@ -137,7 +136,6 @@ public TankDriveShifting(Motor leftWheel, Motor rightWheel, SolenoidShifters shi */ @Override public SolenoidShifters getShifter() { - LogKitten.wtf("Got the Shifter:" + String.valueOf(shifter)); return shifter; } } From cef99182cb35b5755bc67212017aaf55c32c8695 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 <102199775+roboticsteam4904-2@users.noreply.github.com> Date: Thu, 26 Jan 2023 16:25:09 -0800 Subject: [PATCH 010/134] reformat simple slines --- commands/chassis/SimpleSplines.java | 34 ++++++++++++++++++++--------- 1 file changed, 24 insertions(+), 10 deletions(-) diff --git a/commands/chassis/SimpleSplines.java b/commands/chassis/SimpleSplines.java index 10949518..e455cde5 100644 --- a/commands/chassis/SimpleSplines.java +++ b/commands/chassis/SimpleSplines.java @@ -23,16 +23,29 @@ public class SimpleSplines extends SequentialCommandGroup { public SimpleSplines(SplinesDrive robotDrive, Pose2d init_pos, List 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) + 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))), + .addConstraint( + new DifferentialDriveVoltageConstraint( + new SimpleMotorFeedforward( + robotDrive.getDriveConstants().ksVolts, + robotDrive.getDriveConstants().kvVoltSecondsPerMeter, + robotDrive.getDriveConstants().kaVoltSecondsSquaredPerMeter + ), + robotDrive.getDriveConstants().kDriveKinematics, + maxVoltage + ) + ) + ), robotDrive::getPose, new RamseteController(robotDrive.getAutoConstants().kRamseteB, robotDrive.getAutoConstants().kRamseteZeta), new SimpleMotorFeedforward(robotDrive.getDriveConstants().ksVolts, @@ -43,7 +56,8 @@ public SimpleSplines(SplinesDrive robotDrive, Pose2d init_pos, List Date: Thu, 26 Jan 2023 17:46:53 -0800 Subject: [PATCH 011/134] finish reformatting simple splines --- commands/chassis/SimpleSplines.java | 32 ++++++++++++++++++++++++++--- 1 file changed, 29 insertions(+), 3 deletions(-) diff --git a/commands/chassis/SimpleSplines.java b/commands/chassis/SimpleSplines.java index e455cde5..96238262 100644 --- a/commands/chassis/SimpleSplines.java +++ b/commands/chassis/SimpleSplines.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint; @@ -56,9 +57,34 @@ public SimpleSplines(SplinesDrive robotDrive, Pose2d init_pos, List inter_points, Pose2d final_pos, double maxVoltage){ From 972a042d0ceac20985a47fbc4d1ac2207da45cc5 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 26 Jan 2023 19:13:40 -0800 Subject: [PATCH 012/134] log intended and actual trajectory in simpleSplines --- commands/chassis/SimpleSplines.java | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/commands/chassis/SimpleSplines.java b/commands/chassis/SimpleSplines.java index 96238262..40b59b5f 100644 --- a/commands/chassis/SimpleSplines.java +++ b/commands/chassis/SimpleSplines.java @@ -18,7 +18,10 @@ import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.math.trajectory.Trajectory.State; import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -85,6 +88,27 @@ public SimpleSplines(SplinesDrive robotDrive, Pose2d init_pos, List { + // SmartDashboard.putNumber("Intended Trajectory elapsed_time", mmm.timeSeconds); + // SmartDashboard.putNumber("Intended Trajectory velocity", mmm.velocityMetersPerSecond); + // SmartDashboard.putNumber("Intended Trajectory poseX", mmm.poseMeters.getX()); + // SmartDashboard.putNumber("Intended Trajectory poseY", mmm.poseMeters.getY()); + // }); + + // actually, resample the trajectory to 20ms intervals to match the timed logging + for (double elapsed=0; elapsed inter_points, Pose2d final_pos, double maxVoltage){ From 7e409f275dd2337adb08afd3e36a8898b7bffd69 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Wed, 1 Feb 2023 01:09:18 -0800 Subject: [PATCH 013/134] upgrade to 2023 --- custom/controllers/CustomJoystick.java | 2 +- custom/controllers/TeensyController.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/custom/controllers/CustomJoystick.java b/custom/controllers/CustomJoystick.java index dbe46dfb..7deaa79b 100644 --- a/custom/controllers/CustomJoystick.java +++ b/custom/controllers/CustomJoystick.java @@ -71,7 +71,7 @@ public boolean active(int axis) { * @return */ public boolean connected() { - return DriverStation.getInstance().getStickButtonCount(port) > 0; + return DriverStation.getStickButtonCount(port) > 0; } /** diff --git a/custom/controllers/TeensyController.java b/custom/controllers/TeensyController.java index 38bea0df..d1a6009d 100644 --- a/custom/controllers/TeensyController.java +++ b/custom/controllers/TeensyController.java @@ -26,7 +26,7 @@ public TeensyController(int port, int numButtons) { * @return */ public boolean connected() { - return DriverStation.getInstance().getStickButtonCount(port) > 0; + return DriverStation.getStickButtonCount(port) > 0; } @Override From f21e07a765111ea6abc39758a384b4389387d72b Mon Sep 17 00:00:00 2001 From: Exr0n Date: Wed, 1 Feb 2023 02:35:13 -0800 Subject: [PATCH 014/134] HID triggers: delete CustomXBoxController; add CustomCommandJoystick use wpilibj2's CommandXBoxController instead --- custom/controllers/CustomButton.java | 64 -- custom/controllers/CustomCommandJoystick.java | 94 +++ custom/controllers/CustomJoystick.java | 91 --- custom/controllers/CustomXbox.java | 38 -- custom/controllers/TeensyController.java | 44 -- custom/controllers/XboxController.java | 621 ------------------ 6 files changed, 94 insertions(+), 858 deletions(-) delete mode 100644 custom/controllers/CustomButton.java create mode 100644 custom/controllers/CustomCommandJoystick.java delete mode 100644 custom/controllers/CustomJoystick.java delete mode 100644 custom/controllers/CustomXbox.java delete mode 100644 custom/controllers/TeensyController.java delete mode 100644 custom/controllers/XboxController.java diff --git a/custom/controllers/CustomButton.java b/custom/controllers/CustomButton.java deleted file mode 100644 index 9b7c9a65..00000000 --- a/custom/controllers/CustomButton.java +++ /dev/null @@ -1,64 +0,0 @@ -package org.usfirst.frc4904.standard.custom.controllers; - -import org.usfirst.frc4904.standard.commands.Cancel; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.CommandBase; - -/** - * A button with better toggle detection - * - */ -public class CustomButton extends JoystickButton { - protected boolean currentState; - - public CustomButton(GenericHID joystick, int buttonNumber) { - super(joystick, buttonNumber); - currentState = false; - } - - /** - * Returns true the first time the button is pressed and the function is - * called. - * - * @return whether the button was pressed since the last call to this function - */ - public boolean getFirstPressed() { - boolean buttonVal = super.get(); - if (currentState != buttonVal) { - currentState = buttonVal; - return buttonVal; - } - return false; - } - - /** - * Cancels a command when the button is released - * - * @param command The command to be cancelled. - */ - public void cancelWhenReleased(CommandBase command) { - whenReleased(new Cancel(command)); - } - - /** - * Run a command once when a button is held. - * - * @param command The command to be run - */ - public void onlyWhileHeld(CommandBase command) { - whenPressed(command); - cancelWhenReleased(command); - } - - /** - * Runs a command unless a button is held. - * - * @param command The command to be run. - */ - public void onlyWhileReleased(CommandBase command) { - command.schedule(); - whenReleased(command); - cancelWhenPressed(command); - } -} diff --git a/custom/controllers/CustomCommandJoystick.java b/custom/controllers/CustomCommandJoystick.java new file mode 100644 index 00000000..41f874f0 --- /dev/null +++ b/custom/controllers/CustomCommandJoystick.java @@ -0,0 +1,94 @@ +// THIS FILE IS TESTED post wpilibj2 + +package org.usfirst.frc4904.standard.custom.controllers; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +/** + * A joystick that implements the generic controller interface and the 2023 + * trigger interface. This allows us to use a joystick as a controller. This + * contains 12 buttons to reflect the joysticks we are typically using. + * + * TODO: should probably extend or be replaced with https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/wpilibj2/command/button/CommandJoystick.html + */ +public class CustomCommandJoystick extends Joystick implements Controller { + public static final int X_AXIS = 0; + public static final int Y_AXIS = 1; + public static final int SLIDER_AXIS = 3; + protected double deadzone; + protected final int port; + // Buttons + public final JoystickButton button1; + public final JoystickButton button2; + public final JoystickButton button3; + public final JoystickButton button4; + public final JoystickButton button5; + public final JoystickButton button6; + public final JoystickButton button7; + public final JoystickButton button8; + public final JoystickButton button9; + public final JoystickButton button10; + public final JoystickButton button11; + public final JoystickButton button12; + + public CustomCommandJoystick(int port) { + super(port); + this.port = port; + deadzone = 0; + button1 = new JoystickButton(this, 1); + button2 = new JoystickButton(this, 2); + button3 = new JoystickButton(this, 3); + button4 = new JoystickButton(this, 4); + button5 = new JoystickButton(this, 5); + button6 = new JoystickButton(this, 6); + button7 = new JoystickButton(this, 7); + button8 = new JoystickButton(this, 8); + button9 = new JoystickButton(this, 9); + button10 = new JoystickButton(this, 10); + button11 = new JoystickButton(this, 11); + button12 = new JoystickButton(this, 12); + } + + /** + * Returns true if a given axis is above the move threshold. + * + * @param axis + * @return + */ + public boolean active(int axis) { + if (axis == CustomCommandJoystick.X_AXIS) { + return Math.abs(super.getX()) > deadzone; + } else if (axis == CustomCommandJoystick.Y_AXIS) { + return Math.abs(super.getY()) > deadzone; + } else { + return false; + } + } + + /** + * Returns true if the joystick is actually connected. It determines this by + * counting the number of buttons (more than 0 means the joystick is connected). + * + * @return + */ + public boolean connected() { + return DriverStation.getStickButtonCount(port) > 0; + } + + /** + * Returns the value of the given axis. + */ + @Override + public double getAxis(int axis) { + if (Math.abs(super.getRawAxis(axis)) < deadzone) { + return 0.0; + } + return super.getRawAxis(axis); + } + + public void setDeadzone(double deadzone) { + this.deadzone = deadzone; + } +} diff --git a/custom/controllers/CustomJoystick.java b/custom/controllers/CustomJoystick.java deleted file mode 100644 index 7deaa79b..00000000 --- a/custom/controllers/CustomJoystick.java +++ /dev/null @@ -1,91 +0,0 @@ -// THIS FILE IS TESTED post wpilibj2 - -package org.usfirst.frc4904.standard.custom.controllers; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Joystick; - -/** - * A joystick that implements the generic controller interface. This allows us - * to use a joystick as a controller. This contains 12 buttons to reflect the - * joysticks we are typically using. - */ -public class CustomJoystick extends Joystick implements Controller { - public static final int X_AXIS = 0; - public static final int Y_AXIS = 1; - public static final int SLIDER_AXIS = 3; - protected double deadzone; - protected final int port; - // Buttons - public final CustomButton button1; - public final CustomButton button2; - public final CustomButton button3; - public final CustomButton button4; - public final CustomButton button5; - public final CustomButton button6; - public final CustomButton button7; - public final CustomButton button8; - public final CustomButton button9; - public final CustomButton button10; - public final CustomButton button11; - public final CustomButton button12; - - public CustomJoystick(int port) { - super(port); - this.port = port; - deadzone = 0; - button1 = new CustomButton(this, 1); - button2 = new CustomButton(this, 2); - button3 = new CustomButton(this, 3); - button4 = new CustomButton(this, 4); - button5 = new CustomButton(this, 5); - button6 = new CustomButton(this, 6); - button7 = new CustomButton(this, 7); - button8 = new CustomButton(this, 8); - button9 = new CustomButton(this, 9); - button10 = new CustomButton(this, 10); - button11 = new CustomButton(this, 11); - button12 = new CustomButton(this, 12); - } - - /** - * Returns true if a given axis is above the move threshold. - * - * @param axis - * @return - */ - public boolean active(int axis) { - if (axis == CustomJoystick.X_AXIS) { - return Math.abs(super.getX()) > deadzone; - } else if (axis == CustomJoystick.Y_AXIS) { - return Math.abs(super.getY()) > deadzone; - } else { - return false; - } - } - - /** - * Returns true if the joystick is actually connected. It determines this by - * counting the number of buttons (more than 0 means the joystick is connected). - * - * @return - */ - public boolean connected() { - return DriverStation.getStickButtonCount(port) > 0; - } - - /** - * Returns the value of the given axis. - */ - @Override - public double getAxis(int axis) { - if (Math.abs(super.getRawAxis(axis)) < deadzone) { - return 0.0; - } - return super.getRawAxis(axis); - } - - public void setDeadzone(double deadzone) { - this.deadzone = deadzone; - } -} diff --git a/custom/controllers/CustomXbox.java b/custom/controllers/CustomXbox.java deleted file mode 100644 index 1adeae14..00000000 --- a/custom/controllers/CustomXbox.java +++ /dev/null @@ -1,38 +0,0 @@ -// THIS FILE IS TESTED post wpilibj2 - -package org.usfirst.frc4904.standard.custom.controllers; - -/** - * An XboxController that implements the generic controller class. - * - */ -public class CustomXbox extends XboxController implements Controller { - public final static int LEFT_X_AXIS = 0; - public final static int LEFT_Y_AXIS = 1; - public final static int RIGHT_X_AXIS = 2; - public final static int RIGHT_Y_AXIS = 3; - - public CustomXbox(final int port) { - super(port); - } - - /** - * Axis 0: left joystick x Axis 1: left joystick y Axis 2: right joystick x Axis - * 2: right joystick y - */ - @Override - public double getAxis(int axis) { - switch (axis) { - case 0: - return leftStick.getX(); - case 1: - return leftStick.getY(); - case 2: - return rightStick.getX(); - case 3: - return rightStick.getY(); - default: - return super.getRawAxis(axis); - } - } -} diff --git a/custom/controllers/TeensyController.java b/custom/controllers/TeensyController.java deleted file mode 100644 index d1a6009d..00000000 --- a/custom/controllers/TeensyController.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.usfirst.frc4904.standard.custom.controllers; - -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Joystick; - -public class TeensyController extends Joystick implements Controller { - public int numButtons; - public final int port; - public CustomButton[] buttons; // array of all buttons - - public TeensyController(int port, int numButtons) { - super(port); - this.port = port; - this.numButtons = numButtons; - CustomButton[] buttons = new CustomButton[numButtons]; // array of any length - this.buttons = buttons; - for (int index = 0; index < numButtons; index++) { // sets each button in buttons - buttons[index] = new CustomButton(this, index + 1); - } - } - - /** - * Returns true if the joystick is actually connected. It determines this by - * counting the number of buttons (more than 0 means the joystick is connected). - * - * @return - */ - public boolean connected() { - return DriverStation.getStickButtonCount(port) > 0; - } - - @Override - public double getAxis(int axis) { // gets the value of the axis - return super.getRawAxis(axis); - } - - public double getNumButtons() { // returns number of buttons - return numButtons; - } - - public CustomButton getButton(int id) { // allows for indirect access to array - return buttons[id - 1]; - } -} diff --git a/custom/controllers/XboxController.java b/custom/controllers/XboxController.java deleted file mode 100644 index 06c867e3..00000000 --- a/custom/controllers/XboxController.java +++ /dev/null @@ -1,621 +0,0 @@ -package org.usfirst.frc4904.standard.custom.controllers; - -/* Imports */ -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj2.command.button.Button; - -/** - * [class] XboxController - * - * @author AJ Granowski & 4624 Owatonna Robotics - * @version 2015 - * - * This class wraps around the Joystick class in order to make working - * with Xbox360 controllers less of a pain. - * - * The values from this class can be used in two ways. One could either - * check each Button every cycle with .get(), or they could call - * commands directly from the Buttons with .whenPressed() - * - * USAGE: // Initialization myXboxController = new XboxController( - * ); - * myXboxController.leftStick.setThumbstickDeadZone( .2 ); // Optional. - * See code below for defaults. - * - * // Using buttons myXboxController.a.whenPressed( new MyCommand() ); - * myXboxController.lb.toggleWhenPressed( new MyCommand() ); - * myXboxController.rightStick.whenPressed( new MyCommand() ); - * - * // Getting values directly if( myXboxController.leftStick.getY() > - * .4 ) ... - * - * // Support of legacy methods (NOTE: These values are straight from - * the Joystick class. No deadzone stuff or anything) if( - * xboxController.getX() > .4 ) ... - * - * NOTES: Although I have confidence that this will work, not - * everything has been tested. This should work for the 2015 WPILib. - * The mappings of axis's and buttons may change in later years. I am - * not a good Java programmer. - */ -public class XboxController extends Joystick { - /* Default Values */ - protected static final double DEFAULT_THUMBSTICK_DEADZONE = 0.1; // Jiggle room for the thumbsticks - protected static final double DEFAULT_TRIGGER_DEADZONE = 0.01; // Jiggle room for the triggers - protected static final double DEFAULT_TRIGGER_SENSITIVITY = 0.6; // If the trigger is beyond this limit, say it has - // been pressed - /* Button Mappings */ - protected static final int A_BUTTON_ID = 1; - protected static final int B_BUTTON_ID = 2; - protected static final int X_BUTTON_ID = 3; - protected static final int Y_BUTTON_ID = 4; - protected static final int LB_BUTTON_ID = 5; - protected static final int RB_BUTTON_ID = 6; - protected static final int BACK_BUTTON_ID = 7; - protected static final int START_BUTTON_ID = 8; - protected static final int LEFT_THUMBSTIKC_BUTTON_ID = 9; - protected static final int RIGHT_THUMBSTICK_BUTTON_ID = 10; - /* Axis Mappings */ - protected static final int LEFT_THUMBSTICK_X_AXIS_ID = 0; - protected static final int LEFT_THUMBSTICK_Y_AXIS_ID = 1; - protected static final int LEFT_TRIGGER_AXIS_ID = 2; - protected static final int RIGHT_TRIGGER_AXIS_ID = 3; - protected static final int RIGHT_THUMBSTICK_X_AXIS_ID = 4; - protected static final int RIGHT_THUMBSTICK_Y_AXIS_ID = 5; - /* Instance Values */ - private final int port; - protected final Joystick controller; - public final Thumbstick leftStick; - public final Thumbstick rightStick; - public final Trigger lt; - public final Trigger rt; - public final DirectionalPad dPad; - public CustomButton a; - public CustomButton b; - public CustomButton x; - public CustomButton y; - public CustomButton lb; - public CustomButton rb; - public CustomButton back; - public CustomButton start; - - /** - * (Constructor #1) There are two ways to make an XboxController. With this - * constructor, you can specify which port you expect the controller to be on. - * - * @param port - */ - public XboxController(final int port) { - super(port); // Extends Joystick... - /* Initialize */ - this.port = port; - controller = new Joystick(this.port); // Joystick referenced by everything - leftStick = new Thumbstick(controller, HAND.LEFT); - rightStick = new Thumbstick(controller, HAND.RIGHT); - dPad = new DirectionalPad(controller); - lt = new Trigger(controller, HAND.LEFT); - rt = new Trigger(controller, HAND.RIGHT); - a = new CustomButton(controller, XboxController.A_BUTTON_ID); - b = new CustomButton(controller, XboxController.B_BUTTON_ID); - x = new CustomButton(controller, XboxController.X_BUTTON_ID); - y = new CustomButton(controller, XboxController.Y_BUTTON_ID); - lb = new CustomButton(controller, XboxController.LB_BUTTON_ID); - rb = new CustomButton(controller, XboxController.RB_BUTTON_ID); - back = new CustomButton(controller, XboxController.BACK_BUTTON_ID); - start = new CustomButton(controller, XboxController.START_BUTTON_ID); - } - - /** - * (Constructor #2) This is the other constructor. I would recommend using this - * one instead as it is unlikely that anything else but the XboxController will - * be connected. - */ - public XboxController() { - this(0); - } - - /** - * Rather than use an integer (which might not be what we expect) we use an enum - * which has a set amount of possibilities. - */ - public static enum HAND { - LEFT, RIGHT - } - - /** - * This is the relation of direction and number for .getPOV() used in the - * DirectionalPad class. - */ - public static enum DPAD { - UP(0), UP_RIGHT(45), RIGHT(90), DOWN_RIGHT(135), DOWN(180), DOWN_LEFT(225), LEFT(270), UP_LEFT(315); - - /* Instance Value */ - private int value; - - /** - * Constructor - * - * @param value - */ - DPAD(final int value) { - this.value = value; - } - - /** - * Convert integers to DPAD values - * - * @param value - * @return DPAD with matching angle - */ - public static DPAD getEnum(int angle) { - angle = Math.abs(angle); - angle %= 360; - angle = Math.round((float) angle / 45f) * 45; - DPAD[] all = DPAD.values(); - for (int i = 0; i < all.length; i++) { - if (all[i].value == angle) { - return all[i]; - } - } - System.out.println( - "[XboxController.DPAD.getEnum()] Angle supplied (" + angle + ") has no related DPad direction"); - return DPAD.UP; - } - } - - /** - * This class is used to represent the thumbsticks on the Xbox360 controller. - */ - public static class Thumbstick extends Button { - /* Instance Values */ - private final Joystick parent; - private final HAND hand; - private final int xAxisID; - private final int yAxisID; - private final int pressedID; - private double xDeadZone; - private double yDeadZone; - - /** - * Constructor - * - * @param parent - * @param hand - */ - Thumbstick(final Joystick parent, final HAND hand) { - /* Initialize */ - this.parent = parent; - this.hand = hand; - xDeadZone = XboxController.DEFAULT_THUMBSTICK_DEADZONE; - yDeadZone = XboxController.DEFAULT_THUMBSTICK_DEADZONE; - if (hand == HAND.LEFT) { - xAxisID = XboxController.LEFT_THUMBSTICK_X_AXIS_ID; - yAxisID = XboxController.LEFT_THUMBSTICK_Y_AXIS_ID; - pressedID = XboxController.LEFT_THUMBSTIKC_BUTTON_ID; - } else { // If right hand - xAxisID = XboxController.RIGHT_THUMBSTICK_X_AXIS_ID; - yAxisID = XboxController.RIGHT_THUMBSTICK_Y_AXIS_ID; - pressedID = XboxController.RIGHT_THUMBSTICK_BUTTON_ID; - } - } - - /** - * + = right - = left - * - * @return X but with a deadzone - */ - private double rawX() { - final double rawInput = parent.getRawAxis(xAxisID); - return XboxController.createDeadZone(rawInput, xDeadZone); - } - - /** - * + = up - = down - * - * @return Y but with a deadzone - */ - private double rawY() { - final double rawInput = -parent.getRawAxis(yAxisID); // -Y was up on our thumbsticks. Consider this a fix? - return XboxController.createDeadZone(rawInput, yDeadZone); - } - - /** - * magnitude - * - * @param x - * @param y - * @return Magnitude of thing - */ - private double magnitude(double x, double y) { - final double xSquared = Math.pow(x, 2); - final double ySquared = Math.pow(y, 2); - return Math.sqrt(xSquared + ySquared); - } - - /** - * angleToSquareSpace - * - * @param angle - * @return Number between 0 and PI/4 - */ - private double angleToSquareSpace(double angle) { - final double absAngle = Math.abs(angle); - final double halfPi = Math.PI / 2; - final double quarterPi = Math.PI / 4; - final double modulus = absAngle % halfPi; - return -Math.abs(modulus - quarterPi) + quarterPi; - } - - /** - * scaleMagnitude - * - * @param x - * @param y - * @return - */ - private double scaleMagnitude(double x, double y) { - final double magnitude = magnitude(x, y); - final double angle = Math.atan2(x, y); - final double newAngle = angleToSquareSpace(angle); - final double scaleFactor = Math.cos(newAngle); - return magnitude * scaleFactor; - } - - /* Extended Methods */ - @Override - public boolean get() { - return parent.getRawButton(pressedID); - } - - /* Get Methods */ - /** - * Used to see which side of the controller this thumbstick is - * - * @return Thumbstick hand - */ - public HAND getHand() { - return hand; - } - - /** - * getRawX - * - * @return X with a deadzone - */ - public double getX() { - return rawX(); - } - - /** - * getRawY - * - * @return Y with a deadzone - */ - public double getY() { - return rawY(); - } - - /** - * 0 = Up; 90 = Right; ... - * - * @return Angle the thumbstick is pointing - */ - public double getAngle() { - final double angle = Math.atan2(rawX(), rawY()); - return Math.toDegrees(angle); - } - - /** - * getMagnitude - * - * @return A number between 0 and 1 - */ - public double getMagnitude() { - double magnitude = scaleMagnitude(rawX(), rawY()); - if (magnitude > 1) { - magnitude = 1; // Prevent any errors that might arise - } - return magnitude; - } - - /** - * Get the adjusted thumbstick position (Magnitude <= 1) - * - * @return True thumbstick position - */ - public double getTrueX() { - final double x = rawX(); - final double y = rawY(); - final double angle = Math.atan2(x, y); - return scaleMagnitude(x, y) * Math.sin(angle); - } - - /** - * Get the adjusted thumbstick position (Magnitude <= 1) - * - * @return True thumbstick position - */ - public double getTrueY() { - final double x = rawX(); - final double y = rawY(); - final double angle = Math.atan2(x, y); - return scaleMagnitude(x, y) * Math.cos(angle); - } - - /* Set Methods */ - /** - * Set the X axis deadzone of this thumbstick - * - * @param number - */ - public void setXDeadZone(double number) { - xDeadZone = number; - } - - /** - * Set the Y axis deadzone of this thumbstick - * - * @param number - */ - public void setYDeadZone(double number) { - yDeadZone = number; - } - - /** - * Set both axis deadzones of this thumbstick - * - * @param number - */ - public void setDeadZone(double number) { - xDeadZone = number; - yDeadZone = number; - } - } - - /** - * This class is used to represent one of the two Triggers on an Xbox360 - * controller. - */ - public static class Trigger extends Button { - /* Instance Values */ - private final Joystick parent; - private final HAND hand; - private double deadZone; - private double sensitivity; - - /** - * Constructor - * - * @param joystick - * @param hand - */ - Trigger(final Joystick joystick, final HAND hand) { - /* Initialize */ - parent = joystick; - this.hand = hand; - deadZone = XboxController.DEFAULT_TRIGGER_DEADZONE; - sensitivity = XboxController.DEFAULT_TRIGGER_SENSITIVITY; - } - - /* Extended Methods */ - @Override - public boolean get() { - return getX() > sensitivity; - } - - /* Get Methods */ - /** - * getHand - * - * @return Trigger hand - * - * See which side of the controller this trigger is - */ - public HAND getHand() { - return hand; - } - - /** - * 0 = Not pressed 1 = Completely pressed - * - * @return How far its pressed - */ - public double getX() { - final double rawInput; - if (hand == HAND.LEFT) { - rawInput = parent.getRawAxis(XboxController.LEFT_TRIGGER_AXIS_ID); - } else { - rawInput = parent.getRawAxis(XboxController.RIGHT_TRIGGER_AXIS_ID); - } - return XboxController.createDeadZone(rawInput, deadZone); - } - - public double getY() { - return getX(); // Triggers have one dimensional movement. Use getX() instead - } - - /* Set Methods */ - /** - * Set the deadzone of this trigger - * - * @param number - */ - public void setTriggerDeadZone(double number) { - deadZone = number; - } - - /** - * How far you need to press this trigger to activate a button press - * - * @param number - */ - public void setTriggerSensitivity(double number) { - sensitivity = number; - } - } - - /** - * This is a weird object which is essentially just 8 buttons. - */ - public static class DirectionalPad extends Button { - /* Instance Values */ - private final Joystick parent; - public final Button up; - public final Button upRight; - public final Button right; - public final Button downRight; - public final Button down; - public final Button downLeft; - public final Button left; - public final Button upLeft; - - /** - * Constructor - * - * @param parent - */ - DirectionalPad(final Joystick parent) { - /* Initialize */ - this.parent = parent; - up = new DPadButton(this, DPAD.UP); - upRight = new DPadButton(this, DPAD.UP_RIGHT); - right = new DPadButton(this, DPAD.RIGHT); - downRight = new DPadButton(this, DPAD.DOWN_RIGHT); - down = new DPadButton(this, DPAD.DOWN); - downLeft = new DPadButton(this, DPAD.DOWN_LEFT); - left = new DPadButton(this, DPAD.LEFT); - upLeft = new DPadButton(this, DPAD.UP_LEFT); - } - - /** - * This class is used to represent each of the 8 values a dPad has as a button. - */ - public static class DPadButton extends Button { - /* Instance Values */ - private final DPAD direction; - private final DirectionalPad parent; - - /** - * Constructor - * - * @param parent - * @param dPad - */ - DPadButton(final DirectionalPad parent, final DPAD dPadDirection) { - /* Initialize */ - direction = dPadDirection; - this.parent = parent; - } - - /* Extended Methods */ - @Override - public boolean get() { - return parent.getAngle() == direction.value; - } - } - - private int angle() { - return parent.getPOV(); - } - - /* Extended Methods */ - @Override - public boolean get() { - return angle() != -1; - } - - /* Get Methods */ - /** - * UP 0; UP_RIGHT 45; RIGHT 90; DOWN_RIGHT 135; DOWN 180; DOWN_LEFT 225; LEFT - * 270; UP_LEFT 315; - * - * @return A number between 0 and 315 indicating direction - */ - public int getAngle() { - return angle(); - } - - /** - * Just like getAngle, but returns a direction instead of an angle - * - * @return A DPAD direction - */ - public DPAD getDirection() { - return DPAD.getEnum(angle()); - } - } - - /** - * Creates a deadzone, but without clipping the lower values. turns this - * |--1--2--3--4--5--| into this ______|-1-2-3-4-5-| - * - * @param input - * @param deadZoneSize - * @return adjusted_input - */ - private static double createDeadZone(double input, double deadZoneSize) { - final double negative; - double deadZoneSizeClamp = deadZoneSize; - double adjusted; - if (deadZoneSizeClamp < 0 || deadZoneSizeClamp >= 1) { - deadZoneSizeClamp = 0; // Prevent any weird errors - } - negative = input < 0 ? -1 : 1; - adjusted = Math.abs(input) - deadZoneSizeClamp; // Subtract the deadzone from the magnitude - adjusted = adjusted < 0 ? 0 : adjusted; // if the new input is negative, make it zero - adjusted = adjusted / (1 - deadZoneSizeClamp); // Adjust the adjustment so it can max at 1 - return negative * adjusted; - } - - /* Get Methods */ - /** - * @return The port of this XboxController - */ - public int getPort() { - return port; - } - - /** - * @return The Joystick of this XboxController - */ - public Joystick getJoystick() { - return controller; - } - - /* - * Set both axis deadzones of both thumbsticks - * - * @param number - */ - public void setDeadZone(double number) { - leftStick.setDeadZone(number); - rightStick.setDeadZone(number); - } - - /* Set Methods */ - - /** - * Make the controller vibrate - * - * @param hand The side of the controller to rumble - * @param intensity How strong the rumble is - */ - public void setRumble(HAND hand, double intensity) { - if (hand == HAND.LEFT) { - controller.setRumble(RumbleType.kLeftRumble, intensity); - } else { - controller.setRumble(RumbleType.kRightRumble, intensity); - } - } - - /** - * Make the controller vibrate - * - * @param intensity How strong the rumble is - */ - public void setRumble(double intensity) { - controller.setRumble(RumbleType.kLeftRumble, intensity); - controller.setRumble(RumbleType.kRightRumble, intensity); - } - -} \ No newline at end of file From cb14ea6f809578c4933a760c159de451621d134f Mon Sep 17 00:00:00 2001 From: Exr0n Date: Wed, 1 Feb 2023 02:54:10 -0800 Subject: [PATCH 015/134] TO UNDO: jankily delete broken things would fix it but blocked by SensorDrive usage of NavX instead of IMU --- LogKitten.java | 3 +- commands/InjectedCommandGroup.java | 44 ------------------------------ 2 files changed, 2 insertions(+), 45 deletions(-) delete mode 100644 commands/InjectedCommandGroup.java diff --git a/LogKitten.java b/LogKitten.java index 16bf4886..e0f91a7f 100644 --- a/LogKitten.java +++ b/LogKitten.java @@ -145,7 +145,8 @@ public static void setPrintMute(boolean mute) { * @param errorString */ private static void reportErrorToDriverStation(String details, String errorMessage, KittenLevel logLevel) { - HAL.sendError(true, logLevel.getSeverity(), false, errorMessage, details, "", false); + // HAL.sendError(true, logLevel.getSeverity(), false, errorMessage, details, "", false); + // TODO: this is now broken. commenting out causes silent failure. how do you fix it } public static synchronized void logMessage(Object message, KittenLevel level, boolean override) { diff --git a/commands/InjectedCommandGroup.java b/commands/InjectedCommandGroup.java deleted file mode 100644 index 73f60c12..00000000 --- a/commands/InjectedCommandGroup.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.usfirst.frc4904.standard.commands; - -import edu.wpi.first.wpilibj2.command.CommandGroupBase; - -public abstract class InjectedCommandGroup extends CommandGroupBase { - private final CommandGroupBase previous; - - public InjectedCommandGroup(String name, CommandGroupBase previous) { - super(); - setName(name); - this.previous = previous; - } - - public InjectedCommandGroup(CommandGroupBase previous) { - this("InjectedCommandGroup", previous); - } - - public final void initialize() { - if (previous != null && previous.isScheduled()) { - previous.cancel(); - } - onInitialize(); - } - - protected final void interrupted() { - onInterrupted(); - if (previous != null && !previous.isScheduled()) { - previous.schedule(); - } - } - - protected final void end() { - onEnd(); - if (previous != null && !previous.isScheduled()) { - previous.schedule(); - } - } - - protected abstract void onInitialize(); - - protected abstract void onInterrupted(); - - protected abstract void onEnd(); -} From 9d7311bcc5c6f83d065943c4234af405c0a0625c Mon Sep 17 00:00:00 2001 From: Exr0n Date: Fri, 3 Feb 2023 16:32:21 -0800 Subject: [PATCH 016/134] fix sensordrive --- subsystems/chassis/SensorDrive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/subsystems/chassis/SensorDrive.java b/subsystems/chassis/SensorDrive.java index 75134b3a..69c89fd9 100644 --- a/subsystems/chassis/SensorDrive.java +++ b/subsystems/chassis/SensorDrive.java @@ -49,7 +49,7 @@ public SensorDrive(TankDrive driveBase, CANTalonEncoder leftEncoder, CANTalonEnc setCustomPIDSourceType(sensorType); resetEncoders(); - odometry = new DifferentialDriveOdometry(gyro.getRotation2d(), initialPose); + odometry = new DifferentialDriveOdometry(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), initialPose); CommandScheduler.getInstance().registerSubsystem(this); } @@ -112,7 +112,7 @@ public DifferentialDriveWheelSpeeds getWheelSpeeds() { */ public void resetOdometry(Pose2d pose) { resetEncoders(); - odometry.resetPosition(pose, gyro.getRotation2d()); + odometry.resetPosition(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), pose); } /** From 124321e612b69676a1bc72f3bcf7fc631b08ebb7 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 6 Feb 2023 20:29:58 -0800 Subject: [PATCH 017/134] remove RunAllParallel, RunAllSequential, RunFor; use wpilib --- commands/RunAllParallel.java | 26 ------------ commands/RunAllSequential.java | 25 ------------ commands/RunFor.java | 73 ---------------------------------- 3 files changed, 124 deletions(-) delete mode 100644 commands/RunAllParallel.java delete mode 100644 commands/RunAllSequential.java delete mode 100644 commands/RunFor.java diff --git a/commands/RunAllParallel.java b/commands/RunAllParallel.java deleted file mode 100644 index 45e95110..00000000 --- a/commands/RunAllParallel.java +++ /dev/null @@ -1,26 +0,0 @@ -package org.usfirst.frc4904.standard.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; - -import java.util.Arrays; -import java.util.stream.Collectors; - -public class RunAllParallel extends ParallelCommandGroup { - /** - * Run a variable number of commands in parallel. This is a convenience class - * for simple use of CommandGroup. For example, if you want to raise an elevator - * an blare an airhorn at the same time, use: new RunAllParallel(new - * SetElevatorHeight(Elevator.MAX_HEIGHT), new BlareAirhorn()) If you will be - * using the same RunAllParallel more than once, make a new class and extend - * CommandGroup. - * - * @param commands The commands to be run in parallel - */ - public RunAllParallel(CommandBase... commands) { - super(); - setName("RunAllParallel[" + Arrays.stream(commands).map((CommandBase command) -> command.getName()) - .collect(Collectors.joining(" ")) + "]"); - addCommands(commands); - } -} \ No newline at end of file diff --git a/commands/RunAllSequential.java b/commands/RunAllSequential.java deleted file mode 100644 index 888cf6bd..00000000 --- a/commands/RunAllSequential.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.usfirst.frc4904.standard.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -import java.util.Arrays; -import java.util.stream.Collectors; - -public class RunAllSequential extends SequentialCommandGroup { - /** - * Run a variable number of commands in sequence. This is a convenience class - * for simple use of CommandGroup. For example, if you want to go 10 miles north - * and then 5 miles east, use: new RunAllParallel(new GoNorth(10), new - * GoEast(5)) If you will be using the same RunAllSequential more than once, - * make a new class and extend CommandGroup. - * - * @param commands The commands to be run in sequence - */ - public RunAllSequential(CommandBase... commands) { - super(); - setName("RunAllSequential[" + Arrays.stream(commands).map((CommandBase command) -> command.getName()) - .collect(Collectors.joining(" ")) + "]"); - addCommands(commands); - } -} \ No newline at end of file diff --git a/commands/RunFor.java b/commands/RunFor.java deleted file mode 100644 index 1a98ec63..00000000 --- a/commands/RunFor.java +++ /dev/null @@ -1,73 +0,0 @@ -package org.usfirst.frc4904.standard.commands; - -import edu.wpi.first.wpilibj2.command.Subsystem; - -import org.usfirst.frc4904.standard.LogKitten; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class RunFor extends CommandBase { - protected final double duration; - protected final CommandBase command; - protected boolean firstTick; - protected long startMillis; - - /** - * Run a command for a given amount of time, in seconds. The command will be - * cancelled at the end. For example, if you want to go forward for 3 seconds, - * use: new RunFor(new GoForward(), 3) - * - * @param name The name of this RunFor - * @param command The command to be run for the duration - * @param duration A duration in seconds - * @param interruptible Whether this command should be interruptible - */ - public RunFor(String name, CommandBase command, double duration) { - super(); - setName(name); - this.duration = duration; - this.command = command.withTimeout(duration); - firstTick = true; - } - - /** - * Run a command for a given amount of time, in seconds. The command will be - * cancelled at the end. For example, if you want to go forward for 3 seconds, - * use: new RunFor(new GoForward(), 3) - * - * @param command The command to be run for the duration - * @param duration A duration in seconds - * @param interruptible Whether this command should be interruptible - */ - public RunFor(CommandBase command, double duration) { - this("RunFor[" + command.getName() + "]", command, duration); - } - - public boolean isTimedOut() { - return System.currentTimeMillis() - startMillis > duration * 1000; - } - - @Override - public void initialize() { - startMillis = System.currentTimeMillis(); - command.schedule(); - } - - @Override - public boolean isFinished() { - if (firstTick) { - firstTick = false; - LogKitten.wtf(" runfor first tick check " + isTimedOut() + " " + !command.isScheduled()); - return isTimedOut(); - } - LogKitten.wtf(" runfor end check " + isTimedOut() + " " + !command.isScheduled()); - if (isTimedOut() || !command.isScheduled()) LogKitten.wtf("runfor endinggg"); - return isTimedOut() || !command.isScheduled(); - } - - @Override - public void end(boolean inturrupted) { - command.cancel(); - firstTick = true; - } -} From cd3397ae953fa8f2ed54e4e79c39b4de05297ec1 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 6 Feb 2023 22:02:21 -0800 Subject: [PATCH 018/134] custom MotorController interface to force better setVoltage --- custom/motioncontrollers/CANTalonFX.java | 5 +++-- custom/motioncontrollers/CANTalonSRX.java | 7 +++++-- subsystems/motor/MotorController.java | 22 ++++++++++++++++++++++ 3 files changed, 30 insertions(+), 4 deletions(-) create mode 100644 subsystems/motor/MotorController.java diff --git a/custom/motioncontrollers/CANTalonFX.java b/custom/motioncontrollers/CANTalonFX.java index 901d5194..7fe4921b 100644 --- a/custom/motioncontrollers/CANTalonFX.java +++ b/custom/motioncontrollers/CANTalonFX.java @@ -4,12 +4,13 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import org.usfirst.frc4904.standard.subsystems.motor.BrakeableMotor; - -import edu.wpi.first.wpilibj.motorcontrol.MotorController; +import org.usfirst.frc4904.standard.subsystems.motor.MotorController; public class CANTalonFX extends WPI_TalonFX implements MotorController, BrakeableMotor { protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; + // TODO: implement setVoltage with native APIs + public CANTalonFX(int deviceNumber, NeutralMode mode) { super(deviceNumber); setNeutralMode(mode); diff --git a/custom/motioncontrollers/CANTalonSRX.java b/custom/motioncontrollers/CANTalonSRX.java index 5ff81121..1d07626b 100644 --- a/custom/motioncontrollers/CANTalonSRX.java +++ b/custom/motioncontrollers/CANTalonSRX.java @@ -6,10 +6,13 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import org.usfirst.frc4904.standard.subsystems.motor.BrakeableMotor; +import org.usfirst.frc4904.standard.subsystems.motor.MotorController; -public class CANTalonSRX extends WPI_TalonSRX implements BrakeableMotor { +public class CANTalonSRX extends WPI_TalonSRX implements MotorController, BrakeableMotor { protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; + // TODO: implement setVoltage with native APIs + public CANTalonSRX(int deviceNumber, NeutralMode mode) { super(deviceNumber); setNeutralMode(mode); @@ -22,7 +25,7 @@ public CANTalonSRX(int deviceNumber) { public void setCoastMode() { setNeutralMode(NeutralMode.Coast); } - + public void setBrakeMode() { setNeutralMode(NeutralMode.Brake); } diff --git a/subsystems/motor/MotorController.java b/subsystems/motor/MotorController.java new file mode 100644 index 00000000..c20707b9 --- /dev/null +++ b/subsystems/motor/MotorController.java @@ -0,0 +1,22 @@ + +// THIS FILE IS TESTED post wpilibj2 + +package org.usfirst.frc4904.standard.subsystems.motor; + +/** + * An abstract motor interface that replaces the WPILib motor interface + * - to force custom implementation of setVoltage (WPILib's uses RobotController.getBatteryVoltage() which is slow, cite @zbuster05) + */ +public interface MotorController extends edu.wpi.first.wpilibj.motorcontrol.MotorController { + + /** + * Implementations should implement this using a better voltage reading than + * RobotController.getBatteryVoltage(), preferably a native interface. + * + * NOTE FROM BASE CLASS: This function *must* be called regularly in order + * for voltage compensation to work properly - unlike the ordinary set + * function, it is not "set it and forget it." + */ + @Override + public abstract void setVoltage(double voltage); +} From 22279804d88d68475e7e78c63885799840b4a93f Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 6 Feb 2023 22:12:48 -0800 Subject: [PATCH 019/134] new MotorSubsystem replaces Motor.java and commands --- commands/motor/MotorConstant.java | 3 + commands/motor/MotorIdle.java | 4 +- subsystems/motor/MotorSubsystem.java | 230 +++++++++++++++++++++++++++ 3 files changed, 236 insertions(+), 1 deletion(-) create mode 100644 subsystems/motor/MotorSubsystem.java diff --git a/commands/motor/MotorConstant.java b/commands/motor/MotorConstant.java index 86098f56..c4c09815 100644 --- a/commands/motor/MotorConstant.java +++ b/commands/motor/MotorConstant.java @@ -5,8 +5,11 @@ import edu.wpi.first.wpilibj2.command.CommandBase; /** + * DEPRECATED: use MotorSubsystem.c_setPowerHold() + * * Runs a motor at a constant speed until interrupted. */ +@Deprecated public class MotorConstant extends CommandBase { protected final double motorSpeed; protected final Motor motor; diff --git a/commands/motor/MotorIdle.java b/commands/motor/MotorIdle.java index dc54252b..84190efb 100644 --- a/commands/motor/MotorIdle.java +++ b/commands/motor/MotorIdle.java @@ -5,9 +5,11 @@ import edu.wpi.first.wpilibj2.command.CommandBase; /** + * DEPRECATED: use MotorSubsystem.c_idle() + * * Idles the motor (sets speed to 0). - * */ +@Deprecated public class MotorIdle extends CommandBase { protected final Motor motor; diff --git a/subsystems/motor/MotorSubsystem.java b/subsystems/motor/MotorSubsystem.java new file mode 100644 index 00000000..bf582c69 --- /dev/null +++ b/subsystems/motor/MotorSubsystem.java @@ -0,0 +1,230 @@ +package org.usfirst.frc4904.standard.subsystems.motor; + +import org.usfirst.frc4904.standard.LogKitten; +import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; +import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; + +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** + * Wraps MotorControllers to represent as a subsystem. Should be used as the + * atomic, primary motor class for subsystem compositions. + * + * Does not include inversion logic, instead, AbstractMotors/MotorControllers + * should be inverted if needed before constructing this class. + * + * Replaces Motor.java in pre-2023 standard, except without CTRE voltage comp by + * default and without inversion logic. + */ +public class MotorSubsystem extends SubsystemBase { + protected final MotorController[] motors; + protected final SpeedModifier speedModifier; // NOTE: maybe change to be called PowerModifier + protected final String name; + protected double prevPower; + + /** + * A class that wraps around a variable number of MotorController objects to + * give them Subsystem functionality. Can also modify their power with a + * SpeedModifier for things like scaling or brownout protection. + * + * @param name The name for the motor + * @param speedModifier A SpeedModifier changes the input to every motor based + * on some factor. The default is an IdentityModifier, + * which does not affect anything. + * @param motors The MotorControllers in this subsystem. Can be a single + * MotorController or multiple MotorControllers. + */ + public MotorSubsystem(String name, SpeedModifier speedModifier, MotorController... motors) { + super(); + setName(name); + this.name = name; + this.speedModifier = speedModifier; + this.motors = motors; + prevPower = 0; + for (MotorController motor : motors) { + motor.set(0); + } + setDefaultCommand(this.c_idle()); + } + + /** + * A class that wraps around a variable number of MotorController objects to + * give them Subsystem functionality. Can also modify their speed with a + * SpeedModifier for things like scaling or brownout protection. + * + * @param name The name for the motor + * @param motors The MotorControllers in this subsystem. Can be a single + * MotorController or multiple MotorControllers. + */ + public MotorSubsystem(String name, MotorController... motors) { + this(name, new IdentityModifier(), motors); + } + + /** + * A class that wraps around a variable number of MotorController objects to + * give them Subsystem functionality. Can also modify their speed with a + * SpeedModifier for things like scaling or brownout protection. + * + * @param speedModifier A SpeedModifier changes the input to every motor based + * on some factor. The default is an IdentityModifier, + * which does not affect anything. + * @param motors The MotorControllers in this subsystem. Can be a single + * MotorController or multiple MotorControllers. + */ + public MotorSubsystem(SpeedModifier speedModifier, MotorController... motors) { + this("Motor", speedModifier, motors); + } + + /** + * A class that wraps around a variable number of MotorController objects to + * give them Subsystem functionality. Can also modify their speed with a + * SpeedModifier for things like scaling or brownout protection. + * + * @param motors The MotorControllers in this subsystem. Can be a single + * MotorController or multiple MotorControllers. + */ + public MotorSubsystem(MotorController... motors) { + this("Motor", motors); + } + + /// METHODS + /** + * Disables the motor with underlying .disable() + */ + public void disable() { + for (MotorController motor : motors) { + motor.disable(); + } + } + + /** + * Stops the motor with underlying stopMotor() + * + * In theory this should stop the motor without disabling, but wpilib seems + * to just call disable under the hood. + */ + public void stopMotor() { + for (MotorController motor : motors) { + motor.stopMotor(); + } + } + + /** + * Get the most recently set power. If the mostly called set was a + * setVoltage, return the estimated power. + * + * @return The most recently set power between -1.0 and 1.0. + */ + public double get() { + return prevPower; + } + + /** + * Set the motor power. Passes through SpeedModifier. + * + * @param power The power to set. Value should be between -1.0 and 1.0. + */ + public void set(double power) { + LogKitten.v("Motor " + getName() + " @ " + power); + double newPower = speedModifier.modify(power); + prevPower = newPower; + for (MotorController motor : motors) { + motor.set(newPower); + } + } + + /** + * Identical to set(). Set motor power, passing through SpeedModifier. + * + * @param power The power to set. Value should be between -1.0 and 1.0. + */ + public void setPower(double power) { + set(power); + } + + /** + * Set the motor voltages with the underlying setVoltage(). + * + * @param voltage The voltage to set. Future calls to get() will return the + * ratio of this voltage to the current battery voltage. + */ + public void setVoltage(double voltage) { + LogKitten.v("Motor " + getName() + " @ " + voltage + "v"); + prevPower = voltage / RobotController.getBatteryVoltage(); // TODO: use something with less latency than RobotController.getBatteryVoltage(); cite @zbuster05 + } + + + /// COMMANDS + // TODO: remove the "Replaces *.java in pre-2023 standard" and "eg. button1.onTrue(motor.c_idle())" when they become unnecessary. + + /** + * Disable motor using the underlying .disable(); command version of .disable(). + * + * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_idle()) + */ + public Command c_disable() { return this.run(() -> this.disable()); } + + /** + * Stop motor using the underlying .stopMotor(); command version of .stopMotor(). + * + * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_idle()) + */ + public Command c_stopMotor() { return this.run(() -> this.stopMotor()); } + + /** + * Set motor power to zero. Replaces MotorIdle.java in pre-2023 standard. + * + * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_idle()) + */ + public Command c_idle() { return this.runOnce(() -> setPower(0)); } + + /** + * Set motor power to power. Command version of setPower(). + * + * @param power The power to set, in the range [-1, 1]. + * + * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_idle()) + */ + public Command c_setPower(double power) { return this.runOnce(() -> this.setPower(power)); } + + /** + * Repeatedly motor power to power until inturrupted. Replaces MotorConstant.java in pre-2023 standard. + * + * @param power The power to set, in the range [-1, 1]. + * + * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_idle()) + */ + public Command c_setPowerHold(double power) { return this.run(() -> this.setPower(power)); } + + /** + * Set motor voltage. Command version of setVoltagePower(). In case .get() + * is later called, estimates and remembers the power by dividing by battery + * voltage. + * + * @param voltage The voltage to set. + * + * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_idle()) + */ + public Command c_setVoltage(double voltage) { return this.runOnce(() -> this.setVoltage(voltage)); } + + /// ERRORS + protected class UnsynchronizedMotorControllerRuntimeException extends RuntimeException { + private static final long serialVersionUID = 8688590919561059584L; + + public UnsynchronizedMotorControllerRuntimeException() { + super(getName() + "'s MotorControllers report different speeds"); + } + } + + @Deprecated + protected class StrangeCANMotorControllerModeRuntimeException extends RuntimeException { + private static final long serialVersionUID = -539917227288371271L; + + public StrangeCANMotorControllerModeRuntimeException() { + super("One of " + getName() + + "'s MotorControllers is a CANMotorController with a non-zero mode. This might mess up it's .get(), so Motor cannot verify safety."); + } + } +} From 36d0f7dbf87b209b4ebf752a8ac45308831eb132 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 6 Feb 2023 22:17:27 -0800 Subject: [PATCH 020/134] clarify MotorSubsystem docstring remove AbstractMotor, reminder to use 4904 custom MotorController interface --- subsystems/motor/MotorSubsystem.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/subsystems/motor/MotorSubsystem.java b/subsystems/motor/MotorSubsystem.java index bf582c69..3fe9b1c6 100644 --- a/subsystems/motor/MotorSubsystem.java +++ b/subsystems/motor/MotorSubsystem.java @@ -12,8 +12,9 @@ * Wraps MotorControllers to represent as a subsystem. Should be used as the * atomic, primary motor class for subsystem compositions. * - * Does not include inversion logic, instead, AbstractMotors/MotorControllers - * should be inverted if needed before constructing this class. + * Does not include inversion logic, instead, MotorControllers should be + * inverted before constructing this class. Please use 4904custom + * MotorControllers rather than wpilibj2 MotorControllers. * * Replaces Motor.java in pre-2023 standard, except without CTRE voltage comp by * default and without inversion logic. From 02f1fa07943260c2f276db40d63e510970acbebd Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 6 Feb 2023 22:18:21 -0800 Subject: [PATCH 021/134] remove Controller interface; use upcoming EZMotion --- commands/motor/MotorControl.java | 108 --------------- commands/motor/MotorControlAccelCap.java | 125 ------------------ commands/motor/MotorPositionControl.java | 98 -------------- custom/controllers/Controller.java | 19 --- custom/controllers/CustomCommandJoystick.java | 3 +- 5 files changed, 1 insertion(+), 352 deletions(-) delete mode 100644 commands/motor/MotorControl.java delete mode 100644 commands/motor/MotorControlAccelCap.java delete mode 100644 commands/motor/MotorPositionControl.java delete mode 100644 custom/controllers/Controller.java diff --git a/commands/motor/MotorControl.java b/commands/motor/MotorControl.java deleted file mode 100644 index 82c6678d..00000000 --- a/commands/motor/MotorControl.java +++ /dev/null @@ -1,108 +0,0 @@ -package org.usfirst.frc4904.standard.commands.motor; - -import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.custom.controllers.Controller; -import org.usfirst.frc4904.standard.subsystems.motor.Motor; -import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor; -import edu.wpi.first.wpilibj2.command.CommandBase; - -/** - * Controls a Motor directly from a Controller (e.g. Joystick or Xbox) - * - * - */ -public class MotorControl extends CommandBase { - protected final Motor motor; - protected final Controller controller; - protected final int axis; - protected final double scale; - - /** - * This Command directly controls a Motor based on an axis of the Controller. - * This can allow an Operator to easily control a single Motor from an axis of - * the Controller. - * - * @param name - * @param motor - * @param controller - * @param axis - * @param scale - */ - public MotorControl(String name, Motor motor, Controller controller, int axis, double scale) { - super(); - setName(name); - this.motor = motor; - this.controller = controller; - this.axis = axis; - this.scale = scale; - addRequirements(motor); - LogKitten.d("MotorControl created."); - } - - /** - * This Command directly controls a Motor based on an axis of the Controller. - * This can allow an Operator to easily control a single Motor from an axis of - * the Controller. - * - * @param motor - * @param controller - * @param axis - * @param scale - */ - public MotorControl(Motor motor, Controller controller, int axis, double scale) { - this("Motor Control", motor, controller, axis, scale); - } - - /** - * This Command directly controls a Motor based on an axis of the Controller. - * This can allow an Operator to easily control a single Motor from an axis of - * the Controller. - * - * @param name - * @param motor - * @param controller - * @param axis - */ - public MotorControl(String name, Motor motor, Controller controller, int axis) { - this(name, motor, controller, axis, 1.0); - } - - /** - * This Command directly controls a Motor based on an axis of the Controller. - * This can allow an Operator to easily control a single Motor from an axis of - * the Controller. - * - * @param motor - * @param controller - * @param axis - */ - public MotorControl(Motor motor, Controller controller, int axis) { - this(motor, controller, axis, 1.0); - } - - @Override - public void initialize() { - LogKitten.d("MotorControl initialized"); - if (motor instanceof PositionSensorMotor) { - ((PositionSensorMotor) motor).disableMotionController(); - } - } - - @Override - public void execute() { - LogKitten.d("MotorControl executing: " + controller.getAxis(axis)); - motor.set(controller.getAxis(axis) * scale); - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - if (interrupted) { - LogKitten.d("MotorControl interrupted"); - } - } -} \ No newline at end of file diff --git a/commands/motor/MotorControlAccelCap.java b/commands/motor/MotorControlAccelCap.java deleted file mode 100644 index c9a9ada3..00000000 --- a/commands/motor/MotorControlAccelCap.java +++ /dev/null @@ -1,125 +0,0 @@ -package org.usfirst.frc4904.standard.commands.motor; - -import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.custom.controllers.Controller; -import org.usfirst.frc4904.standard.subsystems.motor.Motor; -import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor; -import edu.wpi.first.wpilibj2.command.CommandBase; - -/** - * Controls a Motor directly from a Controller (e.g. Joystick or Xbox) Has an - * acceleration cap (but not deceleration cap) - * - */ -public class MotorControlAccelCap extends CommandBase { - protected final Motor motor; - protected final Controller controller; - protected final int axis; - protected final double scale; - protected final double accel_cap; - protected double last_speed; - protected long last_t; - - /** - * This Command directly controls a Motor based on an axis of the Controller. - * This can allow an Operator to easily control a single Motor from an axis of - * the Controller. Has an acceleration cap (but not deceleration cap) - * - * @param name - * @param motor - * @param controller - * @param axis - * @param scale - * @param accel_cap This is the maximum change in motor speed per second - */ - public MotorControlAccelCap(String name, Motor motor, Controller controller, int axis, double scale, - double accel_cap) { - super(); - setName(name); - addRequirements(motor); - this.motor = motor; - this.controller = controller; - this.axis = axis; - this.scale = scale; - this.accel_cap = accel_cap; - this.last_speed = 0.0; - last_t = System.currentTimeMillis(); - LogKitten.d("MotorControl created for " + motor.getName()); - } - - /** - * This Command directly controls a Motor based on an axis of the Controller. - * This can allow an Operator to easily control a single Motor from an axis of - * the Controller. Has an acceleration cap (but not deceleration cap) - * - * @param motor - * @param controller - * @param axis - * @param scale - * @param accel_cap This is the maximum change in motor speed per second - */ - public MotorControlAccelCap(Motor motor, Controller controller, int axis, double scale, double accel_cap) { - this("MotorControlAccelCap", motor, controller, axis, scale, accel_cap); - } - - /** - * This Command directly controls a Motor based on an axis of the Controller. - * This can allow an Operator to easily control a single Motor from an axis of - * the Controller. - * - * @param name - * @param motor - * @param controller - * @param axis - * @param accel_cap This is the maximum change in motor speed per second - */ - public MotorControlAccelCap(String name, Motor motor, Controller controller, int axis, double accel_cap) { - this(name, motor, controller, axis, 1.0, accel_cap); - } - - /** - * This Command directly controls a Motor based on an axis of the Controller. - * This can allow an Operator to easily control a single Motor from an axis of - * the Controller. - * - * @param motor - * @param controller - * @param axis - * @param accel_cap This is the maximum change in motor speed per second - */ - public MotorControlAccelCap(Motor motor, Controller controller, int axis, double accel_cap) { - this(motor, controller, axis, 1.0, accel_cap); - } - - @Override - public void initialize() { - LogKitten.d("MotorControl initialized"); - if (motor instanceof PositionSensorMotor) { - ((PositionSensorMotor) motor).disableMotionController(); - } - } - - @Override - public void execute() { - LogKitten.d("MotorControl executing: " + controller.getAxis(axis)); - double human_input = controller.getAxis(axis) * scale; - double new_speed; - double delta_t = (System.currentTimeMillis() - last_t) / 1000.0; - last_t = System.currentTimeMillis(); - new_speed = Math.min(last_speed + accel_cap * delta_t, human_input); - motor.set(new_speed); - last_speed = new_speed; - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - if (interrupted) { - LogKitten.d("MotorControl interrupted"); - } - } -} \ No newline at end of file diff --git a/commands/motor/MotorPositionControl.java b/commands/motor/MotorPositionControl.java deleted file mode 100644 index 03c9c845..00000000 --- a/commands/motor/MotorPositionControl.java +++ /dev/null @@ -1,98 +0,0 @@ -package org.usfirst.frc4904.standard.commands.motor; - -import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.Util; -import org.usfirst.frc4904.standard.custom.controllers.Controller; -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor; -import edu.wpi.first.wpilibj2.command.CommandBase; - -/** - * Controls a SensorMotor's position directly from a Controller (e.g. Joystick - * or Xbox) - * - */ -public class MotorPositionControl extends CommandBase { - protected final PositionSensorMotor motor; - protected final Controller controller; - protected final int axis; - protected final boolean invert; - protected final Util.Range motorPositionRange; - protected final CommandBase fallbackCommand; - - /** - * This Command directly controls a SensorMotor's position based on an axis of - * the Controller. This can allow an Operator to easily control the position of - * a single SensorMotor from an axis of the Controller. - * - * @param name - * @param motor - * @param controller - * @param axis - * @param invert - * @param fallbackCommand If the sensor fails for some reason, this command will - * be cancelled, then the fallbackCommand will start - */ - public MotorPositionControl(String name, PositionSensorMotor motor, Util.Range motorPositionRange, - Controller controller, int axis, boolean invert, CommandBase fallbackCommand) { - super(); - setName(name); - addRequirements(motor); - this.motor = motor; - this.motorPositionRange = motorPositionRange; - this.controller = controller; - this.axis = axis; - this.invert = invert; - this.fallbackCommand = fallbackCommand; - LogKitten.d("MotorControl created for " + motor.getName()); - } - - /** - * This Command directly controls a SensorMotor's position based on an axis of - * the Controller. This can allow an Operator to easily control the position of - * a single SensorMotor from an axis of the Controller. - * - * @param name - * @param motor - * @param controller - * @param axis - * @param invert - */ - - public MotorPositionControl(PositionSensorMotor motor, Util.Range motorPositionRange, Controller controller, - int axis, boolean invert) { - this("MotorPositionControl", motor, motorPositionRange, controller, axis, invert, null); - } - - @Override - public void initialize() { - LogKitten.d("MotorPositionControl initialized"); - } - - @Override - public void execute() { - double axisValue = invert ? -1.0 * controller.getAxis(axis) : controller.getAxis(axis); - double targetPosition = motorPositionRange.scaleValue(axisValue); - LogKitten.d("MotorPositionControl executing: " + targetPosition); - try { - motor.setPositionSafely(targetPosition); - } catch (InvalidSensorException e) { - cancel(); - if (fallbackCommand != null) { - fallbackCommand.schedule(); - } - } - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - if (interrupted) { - LogKitten.d("MotorPositionControl interrupted"); - } - } -} \ No newline at end of file diff --git a/custom/controllers/Controller.java b/custom/controllers/Controller.java deleted file mode 100644 index 12b45dc6..00000000 --- a/custom/controllers/Controller.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.usfirst.frc4904.standard.custom.controllers; - -/** - * A generic interface for a controller. - * - */ -public interface Controller { - public static final int X_AXIS = 0; - public static final int Y_AXIS = 1; - public static final int TWIST_AXIS = 2; - - /** - * Allows a generic controller to return an individual axis - * - * @param axis (corresponding to standard axis - * @return a double (-1 to 1) representing the position of the axis - */ - public double getAxis(int axis); -} diff --git a/custom/controllers/CustomCommandJoystick.java b/custom/controllers/CustomCommandJoystick.java index 41f874f0..dbf068ef 100644 --- a/custom/controllers/CustomCommandJoystick.java +++ b/custom/controllers/CustomCommandJoystick.java @@ -13,7 +13,7 @@ * * TODO: should probably extend or be replaced with https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/wpilibj2/command/button/CommandJoystick.html */ -public class CustomCommandJoystick extends Joystick implements Controller { +public class CustomCommandJoystick extends Joystick { public static final int X_AXIS = 0; public static final int Y_AXIS = 1; public static final int SLIDER_AXIS = 3; @@ -80,7 +80,6 @@ public boolean connected() { /** * Returns the value of the given axis. */ - @Override public double getAxis(int axis) { if (Math.abs(super.getRawAxis(axis)) < deadzone) { return 0.0; From ce3cbcb5d616473838ecfae24cfe78ea220729a2 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 6 Feb 2023 23:05:56 -0800 Subject: [PATCH 022/134] BrakeableMotorSubsystem/Controller; remove MotorBrake renamed BrakeableMotor to BrakeableMotorController for consistency with MotorController MotorBrake deleted, use Commands.runOnce(BrakeableMotorSubsystem.setBrakeOnNeutral) instead besides, you probably actually want BrakeableMotorSubsystem.c_brake() --- commands/motor/MotorBrake.java | 36 ----- custom/motioncontrollers/CANTalonFX.java | 5 +- custom/motioncontrollers/CANTalonSRX.java | 6 +- subsystems/motor/BrakeableMotor.java | 13 -- .../motor/BrakeableMotorController.java | 20 +++ subsystems/motor/BrakeableMotorSubsystem.java | 130 ++++++++++++++++++ 6 files changed, 154 insertions(+), 56 deletions(-) delete mode 100644 commands/motor/MotorBrake.java delete mode 100644 subsystems/motor/BrakeableMotor.java create mode 100644 subsystems/motor/BrakeableMotorController.java create mode 100644 subsystems/motor/BrakeableMotorSubsystem.java diff --git a/commands/motor/MotorBrake.java b/commands/motor/MotorBrake.java deleted file mode 100644 index 649cdd51..00000000 --- a/commands/motor/MotorBrake.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.usfirst.frc4904.standard.commands.motor; - -import org.usfirst.frc4904.standard.subsystems.motor.BrakeableMotor; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class MotorBrake extends CommandBase { - protected final BrakeableMotor[] motors; - - public MotorBrake(String name, BrakeableMotor... motors) { - super(); - setName(name); - this.motors = motors; - for (BrakeableMotor motor : motors) { - motor.setBrakeMode(); - if (motor instanceof SubsystemBase) { - addRequirements((SubsystemBase) motor); - } - } - } - - public MotorBrake(BrakeableMotor... motors) { - this("MotorBreak", motors); - } - - public void execute() { - for (BrakeableMotor motor : motors) { - motor.neutralOutput(); - } - } - - public boolean isFinished() { - return false; - } -} \ No newline at end of file diff --git a/custom/motioncontrollers/CANTalonFX.java b/custom/motioncontrollers/CANTalonFX.java index 7fe4921b..5bc0b132 100644 --- a/custom/motioncontrollers/CANTalonFX.java +++ b/custom/motioncontrollers/CANTalonFX.java @@ -3,10 +3,9 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import org.usfirst.frc4904.standard.subsystems.motor.BrakeableMotor; -import org.usfirst.frc4904.standard.subsystems.motor.MotorController; +import org.usfirst.frc4904.standard.subsystems.motor.BrakeableMotorController; -public class CANTalonFX extends WPI_TalonFX implements MotorController, BrakeableMotor { +public class CANTalonFX extends WPI_TalonFX implements BrakeableMotorController { protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; // TODO: implement setVoltage with native APIs diff --git a/custom/motioncontrollers/CANTalonSRX.java b/custom/motioncontrollers/CANTalonSRX.java index 1d07626b..1dcc2916 100644 --- a/custom/motioncontrollers/CANTalonSRX.java +++ b/custom/motioncontrollers/CANTalonSRX.java @@ -5,10 +5,8 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import org.usfirst.frc4904.standard.subsystems.motor.BrakeableMotor; -import org.usfirst.frc4904.standard.subsystems.motor.MotorController; - -public class CANTalonSRX extends WPI_TalonSRX implements MotorController, BrakeableMotor { +import org.usfirst.frc4904.standard.subsystems.motor.BrakeableMotorController; +public class CANTalonSRX extends WPI_TalonSRX implements BrakeableMotorController { protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; // TODO: implement setVoltage with native APIs diff --git a/subsystems/motor/BrakeableMotor.java b/subsystems/motor/BrakeableMotor.java deleted file mode 100644 index 87585dd8..00000000 --- a/subsystems/motor/BrakeableMotor.java +++ /dev/null @@ -1,13 +0,0 @@ - -// THIS FILE IS TESTED post wpilibj2 - -package org.usfirst.frc4904.standard.subsystems.motor; - -public interface BrakeableMotor { - - public abstract void setBrakeMode(); - - public abstract void setCoastMode(); - - public abstract void neutralOutput(); -} diff --git a/subsystems/motor/BrakeableMotorController.java b/subsystems/motor/BrakeableMotorController.java new file mode 100644 index 00000000..dcc69c5d --- /dev/null +++ b/subsystems/motor/BrakeableMotorController.java @@ -0,0 +1,20 @@ + +// THIS FILE IS TESTED post wpilibj2 + +package org.usfirst.frc4904.standard.subsystems.motor; + +public interface BrakeableMotorController extends MotorController { + // TODO: should we have these? brake() = setBrakeOnNeutral(); neutral() + // public abstract void setBrakeOnNeutral(); + // public abstract void setCoastOnNeutral(); + // public abstract void setDisableOnNeutral(); + // public abstract void brake(); + // public abstract void coast(); + // public abstract void neutral(); + + public abstract void setBrakeMode(); + + public abstract void setCoastMode(); + + public abstract void neutralOutput(); +} diff --git a/subsystems/motor/BrakeableMotorSubsystem.java b/subsystems/motor/BrakeableMotorSubsystem.java new file mode 100644 index 00000000..8d115278 --- /dev/null +++ b/subsystems/motor/BrakeableMotorSubsystem.java @@ -0,0 +1,130 @@ +package org.usfirst.frc4904.standard.subsystems.motor; + +import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; +import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; + +import edu.wpi.first.wpilibj2.command.Command; + +public class BrakeableMotorSubsystem extends MotorSubsystem { + // TODO: should this take general MotorControllers and just brake the ones that implement BrakeableMotorController + protected final BrakeableMotorController[] motors; + + /** + * A class that wraps around a variable number of BrakeableMotorController + * objects to give them Subsystem functionality. Can also modify their power + * with a SpeedModifier for things like scaling or brownout protection. + * Enables brake mode on each motor by default. + * + * @param name The name for the motor + * @param speedModifier A SpeedModifier changes the input to every motor based + * on some factor. The default is an IdentityModifier, + * which does not affect anything. + * @param motors The MotorControllers in this subsystem. Can be a single + * MotorController or multiple MotorControllers. + */ + public BrakeableMotorSubsystem(String name, SpeedModifier speedModifier, BrakeableMotorController... motors) { + super(name, speedModifier, motors); + this.motors = motors; + for (var motor : this.motors) motor.setBrakeMode(); + } + + /** + * A class that wraps around a variable number of BrakeableMotorController + * objects to give them Subsystem functionality. Can also modify their speed + * with a SpeedModifier for things like scaling or brownout protection. + * Enables brake mode on each motor by default. + * + * @param name The name for the motor + * @param motors The MotorControllers in this subsystem. Can be a single + * MotorController or multiple MotorControllers. + */ + public BrakeableMotorSubsystem(String name, BrakeableMotorController... motors) { + this(name, new IdentityModifier(), motors); + } + + /** + * A class that wraps around a variable number of BrakeableMotorController + * objects to give them Subsystem functionality. Can also modify their speed + * with a SpeedModifier for things like scaling or brownout protection. + * Enables brake mode on each motor by default. + * + * @param speedModifier A SpeedModifier changes the input to every motor based + * on some factor. The default is an IdentityModifier, + * which does not affect anything. + * @param motors The MotorControllers in this subsystem. Can be a single + * MotorController or multiple MotorControllers. + */ + public BrakeableMotorSubsystem(SpeedModifier speedModifier, BrakeableMotorController... motors) { + this("Motor", speedModifier, motors); + } + + /** + * A class that wraps around a variable number of BrakeableMotorController + * objects to give them Subsystem functionality. Can also modify their speed + * with a SpeedModifier for things like scaling or brownout protection. + * Enables brake mode on each motor by default. + * + * @param motors The MotorControllers in this subsystem. Can be a single + * MotorController or multiple MotorControllers. + */ + public BrakeableMotorSubsystem(BrakeableMotorController... motors) { + this("Motor", motors); + } + + /** + * Sets the neutral output mode to brake. Motor will continue to run, but + * future calls to neutralOutput() will cause motor to brake. + * + * Uses the underlying .neutralOutput() method. + */ + public void setBrakeModeOnNeutral() { for (var motor : motors) motor.setBrakeMode(); } + /** + * Sets the neutral output mode to coast. Motor will continue to run, but + * future calls to neutralOutput() will cause motor to coast. + * + * Uses the underlying .neutralOutput() method. + */ + public void setCoastModeOnNeutral() { for (var motor : motors) motor.setCoastMode(); } + + /** + * Sets the output mode to neutral, which is either break or coast (default + * brake). Use setBrakeModeOnNeutral or setCoastModeOnNeutral to set this + * mode. + * + * Uses the underlying .neutralOutput() method. + */ + public void neutralOutput() { for (var motor : motors) motor.neutralOutput(); } + + /** + * Enables brake mode on and brakes each motor. + */ + public void brake() { + setBrakeModeOnNeutral(); + neutralOutput(); + } + + /** + * Enables coast mode on and coasts each motor. + */ + public void coast() { + setCoastModeOnNeutral(); + neutralOutput(); + } + + /** + * Enables brake mode on and brakes each motor. Command version of brake(). + * + * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_brake()) + */ + public Command c_brake() { + return this.runOnce(() -> this.brake()); + } + /** + * Enables coast mode on and coasts each motor. Command version of coast(). + * + * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_coast()) + */ + public Command c_coast() { + return this.runOnce(() -> this.coast()); + } +} From d4bbca6b285774dbc78d8599b8a1e07276b06938 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 6 Feb 2023 23:07:12 -0800 Subject: [PATCH 023/134] MotorSubsystem.c_setVoltageHold; docstring warnings wpilib says that for setVoltage, you have to keep calling it => remove the commands that don't keep calling it --- subsystems/motor/MotorSubsystem.java | 28 +++++++++++++++++++--------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/subsystems/motor/MotorSubsystem.java b/subsystems/motor/MotorSubsystem.java index 3fe9b1c6..6f4f75e2 100644 --- a/subsystems/motor/MotorSubsystem.java +++ b/subsystems/motor/MotorSubsystem.java @@ -150,10 +150,17 @@ public void setPower(double power) { * * @param voltage The voltage to set. Future calls to get() will return the * ratio of this voltage to the current battery voltage. + * + * NOTE FROM BASE CLASS: This function *must* be called regularly in order + * for voltage compensation to work properly - unlike the ordinary set + * function, it is not "set it and forget it." */ public void setVoltage(double voltage) { LogKitten.v("Motor " + getName() + " @ " + voltage + "v"); prevPower = voltage / RobotController.getBatteryVoltage(); // TODO: use something with less latency than RobotController.getBatteryVoltage(); cite @zbuster05 + for (var motor : motors) { + motor.setVoltage(voltage); + } } @@ -163,14 +170,14 @@ public void setVoltage(double voltage) { /** * Disable motor using the underlying .disable(); command version of .disable(). * - * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_idle()) + * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_disable()) */ public Command c_disable() { return this.run(() -> this.disable()); } /** * Stop motor using the underlying .stopMotor(); command version of .stopMotor(). * - * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_idle()) + * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_stopMotor()) */ public Command c_stopMotor() { return this.run(() -> this.stopMotor()); } @@ -186,7 +193,7 @@ public void setVoltage(double voltage) { * * @param power The power to set, in the range [-1, 1]. * - * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_idle()) + * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_setPower()) */ public Command c_setPower(double power) { return this.runOnce(() -> this.setPower(power)); } @@ -195,20 +202,23 @@ public void setVoltage(double voltage) { * * @param power The power to set, in the range [-1, 1]. * - * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_idle()) + * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_setPowerHold()) */ public Command c_setPowerHold(double power) { return this.run(() -> this.setPower(power)); } /** - * Set motor voltage. Command version of setVoltagePower(). In case .get() - * is later called, estimates and remembers the power by dividing by battery - * voltage. + * Repeatedly set motor voltage. Command version of setVoltagePower(). + * Estimates and remembers the power by dividing by battery voltage in case + * .get() is later called. + * + * c_setVoltage (a runOnce version) is not provided as wpilib + * MotorController recommends always repeatedly setting voltage. * * @param voltage The voltage to set. * - * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_idle()) + * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_setVoltageHold()) */ - public Command c_setVoltage(double voltage) { return this.runOnce(() -> this.setVoltage(voltage)); } + public Command c_setVoltageHold(double voltage) { return this.run(() -> this.setVoltage(voltage)); } /// ERRORS protected class UnsynchronizedMotorControllerRuntimeException extends RuntimeException { From fc4d8d82f240a3ef7e1c45f8d33b0784661447f3 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 6 Feb 2023 23:22:18 -0800 Subject: [PATCH 024/134] clear warnings by removing unused imports and extends --- custom/MCChassisController.java | 1 - custom/PCMPort.java | 2 -- custom/motioncontrollers/MotionController.java | 1 - custom/sensors/CANTalonEncoder.java | 1 - humaninput/Driver.java | 3 +-- humaninput/Operator.java | 4 +--- subsystems/chassis/SensorDrive.java | 6 ------ subsystems/chassis/SplinesDrive.java | 7 ------- 8 files changed, 2 insertions(+), 23 deletions(-) diff --git a/custom/MCChassisController.java b/custom/MCChassisController.java index 1759f6f6..c3c75f98 100644 --- a/custom/MCChassisController.java +++ b/custom/MCChassisController.java @@ -5,7 +5,6 @@ import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; import org.usfirst.frc4904.standard.custom.sensors.IMU; import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.custom.CustomPIDSourceType; public class MCChassisController implements ChassisController { protected ChassisController controller; diff --git a/custom/PCMPort.java b/custom/PCMPort.java index e1f9b191..de17d03a 100644 --- a/custom/PCMPort.java +++ b/custom/PCMPort.java @@ -1,7 +1,5 @@ package org.usfirst.frc4904.standard.custom; -import org.usfirst.frc4904.standard.LogKitten; - import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PneumaticsModuleType; diff --git a/custom/motioncontrollers/MotionController.java b/custom/motioncontrollers/MotionController.java index 0cbbe8ff..4cb33a4f 100644 --- a/custom/motioncontrollers/MotionController.java +++ b/custom/motioncontrollers/MotionController.java @@ -2,7 +2,6 @@ import java.util.function.DoubleConsumer; -import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; import org.usfirst.frc4904.standard.custom.sensors.PIDSensor; diff --git a/custom/sensors/CANTalonEncoder.java b/custom/sensors/CANTalonEncoder.java index 3283a6a6..fed7f656 100644 --- a/custom/sensors/CANTalonEncoder.java +++ b/custom/sensors/CANTalonEncoder.java @@ -2,7 +2,6 @@ package org.usfirst.frc4904.standard.custom.sensors; -import org.usfirst.frc4904.robot.Robot; import org.usfirst.frc4904.standard.Util; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.VelocityMeasPeriod; diff --git a/humaninput/Driver.java b/humaninput/Driver.java index 8a0ea1e5..dd986bbe 100644 --- a/humaninput/Driver.java +++ b/humaninput/Driver.java @@ -1,14 +1,13 @@ package org.usfirst.frc4904.standard.humaninput; import org.usfirst.frc4904.standard.custom.ChassisController; -import org.usfirst.frc4904.standard.custom.Nameable; /** * Driver specific version of HumanInterface. Also designed to be passed around * to control the chassis. * */ -public abstract class Driver extends HumanInput implements Nameable, ChassisController { +public abstract class Driver extends HumanInput implements ChassisController { public Driver(String name) { super(name); } diff --git a/humaninput/Operator.java b/humaninput/Operator.java index 0c5cf381..fa26cc72 100644 --- a/humaninput/Operator.java +++ b/humaninput/Operator.java @@ -1,12 +1,10 @@ package org.usfirst.frc4904.standard.humaninput; -import org.usfirst.frc4904.standard.custom.Nameable; - /** * Operator specifc version of HumanInterface * */ -public abstract class Operator extends HumanInput implements Nameable { +public abstract class Operator extends HumanInput { public Operator(String name) { super(name); } diff --git a/subsystems/chassis/SensorDrive.java b/subsystems/chassis/SensorDrive.java index 69c89fd9..bc54ab04 100644 --- a/subsystems/chassis/SensorDrive.java +++ b/subsystems/chassis/SensorDrive.java @@ -7,21 +7,16 @@ package org.usfirst.frc4904.standard.subsystems.chassis; -import com.ctre.phoenix.sensors.CANCoder; -import org.usfirst.frc4904.robot.RobotMap; import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.commands.chassis.SimpleSplines; import org.usfirst.frc4904.standard.custom.CustomPIDSourceType; import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; import org.usfirst.frc4904.standard.custom.sensors.PIDSensor; -import org.usfirst.frc4904.standard.subsystems.chassis.TankDrive; import org.usfirst.frc4904.standard.subsystems.motor.Motor; import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder; import org.usfirst.frc4904.standard.custom.sensors.NavX; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -33,7 +28,6 @@ public class SensorDrive implements Subsystem, PIDSensor { // Based largely on private final CANTalonEncoder leftEncoder; private final CANTalonEncoder rightEncoder; private final NavX gyro; - private boolean refresh = true; private final DifferentialDriveOdometry odometry; private CustomPIDSourceType sensorType; diff --git a/subsystems/chassis/SplinesDrive.java b/subsystems/chassis/SplinesDrive.java index 9ea1cca3..35ff1fa8 100644 --- a/subsystems/chassis/SplinesDrive.java +++ b/subsystems/chassis/SplinesDrive.java @@ -7,20 +7,13 @@ 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.IMU; -import org.usfirst.frc4904.standard.subsystems.chassis.TankDrive; -import org.usfirst.frc4904.standard.subsystems.motor.Motor; import org.usfirst.frc4904.standard.custom.sensors.NavX; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Subsystem; public class SplinesDrive extends SensorDrive { private final SimpleSplines.AutoConstants autoConstants; From eadb0b1d006fc72fe911aedf1ceee2570aee8ec8 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 6 Feb 2023 23:23:04 -0800 Subject: [PATCH 025/134] replace limit switch with new Trigger; clear more warnings; deprecate Motor --- .../CustomCommandDigitalLimitSwitch.java | 20 +++++++++++++ custom/sensors/CustomDigitalLimitSwitch.java | 29 ------------------- subsystems/motor/Motor.java | 3 ++ subsystems/motor/PositionSensorMotor.java | 2 -- subsystems/motor/VelocitySensorMotor.java | 1 - 5 files changed, 23 insertions(+), 32 deletions(-) create mode 100644 custom/sensors/CustomCommandDigitalLimitSwitch.java delete mode 100644 custom/sensors/CustomDigitalLimitSwitch.java diff --git a/custom/sensors/CustomCommandDigitalLimitSwitch.java b/custom/sensors/CustomCommandDigitalLimitSwitch.java new file mode 100644 index 00000000..479aecd8 --- /dev/null +++ b/custom/sensors/CustomCommandDigitalLimitSwitch.java @@ -0,0 +1,20 @@ +package org.usfirst.frc4904.standard.custom.sensors; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +// TODO untested + +/** + * Digital limit switch that provides the same Trigger interface as HID + * controller buttons. Updated 2023. + */ +public class CustomCommandDigitalLimitSwitch extends Trigger { + public CustomCommandDigitalLimitSwitch(DigitalInput limitSwitch) { + super(() -> limitSwitch.get()); + } + + public CustomCommandDigitalLimitSwitch(int port) { + this(new DigitalInput(port)); + } +} diff --git a/custom/sensors/CustomDigitalLimitSwitch.java b/custom/sensors/CustomDigitalLimitSwitch.java deleted file mode 100644 index f3fffe3b..00000000 --- a/custom/sensors/CustomDigitalLimitSwitch.java +++ /dev/null @@ -1,29 +0,0 @@ -package org.usfirst.frc4904.standard.custom.sensors; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj2.command.button.Button; - -/** - * A limit switch that extends the generic button class. - * - */ -public class CustomDigitalLimitSwitch extends Button implements CustomButton { - protected final DigitalInput limitSwitch; - - public CustomDigitalLimitSwitch(DigitalInput limitSwitch) { - this.limitSwitch = limitSwitch; - } - - public CustomDigitalLimitSwitch(int port) { - this(new DigitalInput(port)); - } - - /** - * Returns true when the limit switch is pressed. This is based on the raw value - * of the limit switch, not only once when it is first pressed. - */ - @Override - public boolean get() { - return !limitSwitch.get(); - } -} diff --git a/subsystems/motor/Motor.java b/subsystems/motor/Motor.java index 62d186da..7dbdec93 100644 --- a/subsystems/motor/Motor.java +++ b/subsystems/motor/Motor.java @@ -11,10 +11,13 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; /** + * DEPRECATED: use MotorSubsystem + * * A class that wraps around a variable number of MotorController objects to * give them Subsystem functionality. Can also modify their speed with a * SpeedModifier for things like scaling or brownout protection. */ +@Deprecated public class Motor extends SubsystemBase implements MotorController { protected final MotorController[] motors; protected final SpeedModifier speedModifier; diff --git a/subsystems/motor/PositionSensorMotor.java b/subsystems/motor/PositionSensorMotor.java index bd3eeef3..46ec3ca7 100644 --- a/subsystems/motor/PositionSensorMotor.java +++ b/subsystems/motor/PositionSensorMotor.java @@ -1,8 +1,6 @@ package org.usfirst.frc4904.standard.subsystems.motor; -import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; import edu.wpi.first.wpilibj.motorcontrol.MotorController; diff --git a/subsystems/motor/VelocitySensorMotor.java b/subsystems/motor/VelocitySensorMotor.java index 226f720c..a1b4f482 100644 --- a/subsystems/motor/VelocitySensorMotor.java +++ b/subsystems/motor/VelocitySensorMotor.java @@ -1,6 +1,5 @@ package org.usfirst.frc4904.standard.subsystems.motor; -import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; From 7ab3647b2a9bb6dd978e0ec38fa64e69a63c7913 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Wed, 8 Feb 2023 12:39:16 -0800 Subject: [PATCH 026/134] convert chassis code from Motor to MotorSubsystem --- LogKitten.java | 3 +-- subsystems/chassis/Chassis.java | 11 ++++++----- subsystems/chassis/MecanumChassis.java | 8 ++++---- subsystems/chassis/SensorDrive.java | 4 ++-- subsystems/chassis/SwerveChassis.java | 22 +++++++++++----------- subsystems/chassis/TankDrive.java | 20 ++++++++++---------- subsystems/chassis/TankDriveShifting.java | 22 +++++++++++----------- subsystems/motor/SensorMotor.java | 1 + 8 files changed, 46 insertions(+), 45 deletions(-) diff --git a/LogKitten.java b/LogKitten.java index e0f91a7f..4140feeb 100644 --- a/LogKitten.java +++ b/LogKitten.java @@ -9,8 +9,7 @@ import java.nio.file.Files; import java.text.SimpleDateFormat; import java.util.Date; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.hal.HAL; // DONT REMOVE, see TODO below public class LogKitten { private static BufferedOutputStream fileOutput; diff --git a/subsystems/chassis/Chassis.java b/subsystems/chassis/Chassis.java index c00d0f74..0b55586c 100644 --- a/subsystems/chassis/Chassis.java +++ b/subsystems/chassis/Chassis.java @@ -1,6 +1,7 @@ package org.usfirst.frc4904.standard.subsystems.chassis; -import org.usfirst.frc4904.standard.subsystems.motor.Motor; +import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; + import edu.wpi.first.wpilibj2.command.SubsystemBase; /** @@ -11,7 +12,7 @@ public abstract class Chassis extends SubsystemBase { protected double[] motorSpeeds; - protected Motor[] motors; + protected MotorSubsystem[] motors; /** * @@ -19,7 +20,7 @@ public abstract class Chassis extends SubsystemBase { * @param motors all the motors that are part of this Chassis. Pass from front * to back, left to right */ - public Chassis(String name, Motor... motors) { + public Chassis(String name, MotorSubsystem... motors) { this.motors = motors; motorSpeeds = new double[motors.length]; setName(name); @@ -30,7 +31,7 @@ public Chassis(String name, Motor... motors) { * @param motors all the motors that are part of this Chassis. Pass from front * to back, left to right */ - public Chassis(Motor... motors) { + public Chassis(MotorSubsystem... motors) { this("Chassis", motors); } @@ -40,7 +41,7 @@ public Chassis(Motor... motors) { * * @return all motors in the order passed to the constructor */ - public Motor[] getMotors() { + public MotorSubsystem[] getMotors() { return motors; } diff --git a/subsystems/chassis/MecanumChassis.java b/subsystems/chassis/MecanumChassis.java index 1561b9cf..7794a7fa 100644 --- a/subsystems/chassis/MecanumChassis.java +++ b/subsystems/chassis/MecanumChassis.java @@ -1,6 +1,6 @@ package org.usfirst.frc4904.standard.subsystems.chassis; -import org.usfirst.frc4904.standard.subsystems.motor.Motor; +import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; /** * Mecanum Chassis. Has one wheel for each corner. Can move in all directions. @@ -16,8 +16,8 @@ public class MecanumChassis extends Chassis { * @param backLeftWheel * @param backRightWheel */ - public MecanumChassis(String name, Motor frontLeftWheel, Motor frontRightWheel, Motor backLeftWheel, - Motor backRightWheel) { + public MecanumChassis(String name, MotorSubsystem frontLeftWheel, MotorSubsystem frontRightWheel, MotorSubsystem backLeftWheel, + MotorSubsystem backRightWheel) { super(name, frontLeftWheel, frontRightWheel, backLeftWheel, backRightWheel); } @@ -29,7 +29,7 @@ public MecanumChassis(String name, Motor frontLeftWheel, Motor frontRightWheel, * @param backLeftWheel * @param backRightWheel */ - public MecanumChassis(Motor frontLeftWheel, Motor frontRightWheel, Motor backLeftWheel, Motor backRightWheel) { + public MecanumChassis(MotorSubsystem frontLeftWheel, MotorSubsystem frontRightWheel, MotorSubsystem backLeftWheel, MotorSubsystem backRightWheel) { this("Mecanum Chassis", frontLeftWheel, frontRightWheel, backLeftWheel, backRightWheel); } diff --git a/subsystems/chassis/SensorDrive.java b/subsystems/chassis/SensorDrive.java index bc54ab04..4c35e6d5 100644 --- a/subsystems/chassis/SensorDrive.java +++ b/subsystems/chassis/SensorDrive.java @@ -12,7 +12,7 @@ import org.usfirst.frc4904.standard.custom.CustomPIDSourceType; import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; import org.usfirst.frc4904.standard.custom.sensors.PIDSensor; -import org.usfirst.frc4904.standard.subsystems.motor.Motor; +import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder; import org.usfirst.frc4904.standard.custom.sensors.NavX; @@ -116,7 +116,7 @@ public void resetOdometry(Pose2d pose) { * @param rightVolts the commanded right output */ public void tankDriveVolts(double leftVolts, double rightVolts) { - Motor[] motors = driveBase.getMotors(); + MotorSubsystem[] motors = driveBase.getMotors(); if (motors.length == 2) { driveBase.getMotors()[0].setVoltage(leftVolts); driveBase.getMotors()[1].setVoltage(rightVolts); diff --git a/subsystems/chassis/SwerveChassis.java b/subsystems/chassis/SwerveChassis.java index feb56241..e369855e 100644 --- a/subsystems/chassis/SwerveChassis.java +++ b/subsystems/chassis/SwerveChassis.java @@ -1,12 +1,12 @@ package org.usfirst.frc4904.standard.subsystems.chassis; -import org.usfirst.frc4904.standard.subsystems.motor.Motor; +import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; public class SwerveChassis extends Chassis { - public final Motor frontLeftWheelSwerve; - public final Motor frontRightWheelSwerve; - public final Motor backLeftWheelSwerve; - public final Motor backRightWheelSwerve; + public final MotorSubsystem frontLeftWheelSwerve; + public final MotorSubsystem frontRightWheelSwerve; + public final MotorSubsystem backLeftWheelSwerve; + public final MotorSubsystem backRightWheelSwerve; /** * Constructs a swerve drive chassis @@ -21,9 +21,9 @@ public class SwerveChassis extends Chassis { * @param backLeftWheelSwerve * @param backRightWheelSwerve */ - public SwerveChassis(String name, Motor frontLeftWheel, Motor frontRightWheel, Motor backLeftWheel, - Motor backRightWheel, Motor frontLeftWheelSwerve, Motor frontRightWheelSwerve, Motor backLeftWheelSwerve, - Motor backRightWheelSwerve) { + public SwerveChassis(String name, MotorSubsystem frontLeftWheel, MotorSubsystem frontRightWheel, MotorSubsystem backLeftWheel, + MotorSubsystem backRightWheel, MotorSubsystem frontLeftWheelSwerve, MotorSubsystem frontRightWheelSwerve, MotorSubsystem backLeftWheelSwerve, + MotorSubsystem backRightWheelSwerve) { super(frontLeftWheel, frontRightWheel, backLeftWheel, backRightWheel); this.frontLeftWheelSwerve = frontLeftWheelSwerve; this.frontRightWheelSwerve = frontRightWheelSwerve; @@ -31,9 +31,9 @@ public SwerveChassis(String name, Motor frontLeftWheel, Motor frontRightWheel, M this.backRightWheelSwerve = backRightWheelSwerve; } - public SwerveChassis(Motor frontLeftWheel, Motor frontRightWheel, Motor backLeftWheel, Motor backRightWheel, - Motor frontLeftWheelSwerve, Motor frontRightWheelSwerve, Motor backLeftWheelSwerve, - Motor backRightWheelSwerve) { + public SwerveChassis(MotorSubsystem frontLeftWheel, MotorSubsystem frontRightWheel, MotorSubsystem backLeftWheel, MotorSubsystem backRightWheel, + MotorSubsystem frontLeftWheelSwerve, MotorSubsystem frontRightWheelSwerve, MotorSubsystem backLeftWheelSwerve, + MotorSubsystem backRightWheelSwerve) { this("SwerveChassis", frontLeftWheel, frontRightWheel, backLeftWheel, backRightWheel, frontLeftWheelSwerve, frontRightWheelSwerve, backLeftWheelSwerve, backRightWheelSwerve); } diff --git a/subsystems/chassis/TankDrive.java b/subsystems/chassis/TankDrive.java index b92d90ec..304aca7e 100644 --- a/subsystems/chassis/TankDrive.java +++ b/subsystems/chassis/TankDrive.java @@ -2,7 +2,7 @@ package org.usfirst.frc4904.standard.subsystems.chassis; -import org.usfirst.frc4904.standard.subsystems.motor.Motor; +import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; /** * Tank drive chassis. Has two sets of wheels, left and right. Can only turn @@ -22,8 +22,8 @@ public class TankDrive extends Chassis { * @param turnCorrection Amount by which to correct turning to make up for dead * CIM or unbalanced weight. In the range -1 to 1. */ - public TankDrive(String name, Double turnCorrection, Motor leftWheelA, Motor leftWheelB, Motor rightWheelA, - Motor rightWheelB) { + public TankDrive(String name, Double turnCorrection, MotorSubsystem leftWheelA, MotorSubsystem leftWheelB, MotorSubsystem rightWheelA, + MotorSubsystem rightWheelB) { super(name, leftWheelA, leftWheelB, rightWheelA, rightWheelB); this.turnCorrection = turnCorrection; } @@ -37,7 +37,7 @@ public TankDrive(String name, Double turnCorrection, Motor leftWheelA, Motor lef * @param turnCorrection Amount by which to correct turning to make up for dead * CIM or unbalanced weight. In the range -1 to 1. */ - public TankDrive(Double turnCorrection, Motor leftWheelA, Motor leftWheelB, Motor rightWheelA, Motor rightWheelB) { + public TankDrive(Double turnCorrection, MotorSubsystem leftWheelA, MotorSubsystem leftWheelB, MotorSubsystem rightWheelA, MotorSubsystem rightWheelB) { this("TankDrive", turnCorrection, leftWheelA, leftWheelB, rightWheelA, rightWheelB); } @@ -48,7 +48,7 @@ public TankDrive(Double turnCorrection, Motor leftWheelA, Motor leftWheelB, Moto * @param rightWheelA * @param rightWheelB */ - public TankDrive(String name, Double turnCorrection, Motor leftWheel, Motor rightWheel) { + public TankDrive(String name, Double turnCorrection, MotorSubsystem leftWheel, MotorSubsystem rightWheel) { super(name, leftWheel, rightWheel); this.turnCorrection = turnCorrection; } @@ -60,7 +60,7 @@ public TankDrive(String name, Double turnCorrection, Motor leftWheel, Motor righ * @param rightWheelA * @param rightWheelB */ - public TankDrive(Double turnCorrection, Motor leftWheel, Motor rightWheel) { + public TankDrive(Double turnCorrection, MotorSubsystem leftWheel, MotorSubsystem rightWheel) { this("TankDrive", turnCorrection, leftWheel, rightWheel); } @@ -72,7 +72,7 @@ public TankDrive(Double turnCorrection, Motor leftWheel, Motor rightWheel) { * @param rightWheelA * @param rightWheelB */ - public TankDrive(String name, Motor leftWheelA, Motor leftWheelB, Motor rightWheelA, Motor rightWheelB) { + public TankDrive(String name, MotorSubsystem leftWheelA, MotorSubsystem leftWheelB, MotorSubsystem rightWheelA, MotorSubsystem rightWheelB) { super(name, leftWheelA, leftWheelB, rightWheelA, rightWheelB); this.turnCorrection = 0.0; } @@ -84,7 +84,7 @@ public TankDrive(String name, Motor leftWheelA, Motor leftWheelB, Motor rightWhe * @param rightWheelA * @param rightWheelB */ - public TankDrive(Motor leftWheelA, Motor leftWheelB, Motor rightWheelA, Motor rightWheelB) { + public TankDrive(MotorSubsystem leftWheelA, MotorSubsystem leftWheelB, MotorSubsystem rightWheelA, MotorSubsystem rightWheelB) { this("TankDrive", leftWheelA, leftWheelB, rightWheelA, rightWheelB); } @@ -94,7 +94,7 @@ public TankDrive(Motor leftWheelA, Motor leftWheelB, Motor rightWheelA, Motor ri * @param leftWheel * @param rightWheel */ - public TankDrive(String name, Motor leftWheel, Motor rightWheel) { + public TankDrive(String name, MotorSubsystem leftWheel, MotorSubsystem rightWheel) { super(name, leftWheel, rightWheel); this.turnCorrection = 0.0; } @@ -104,7 +104,7 @@ public TankDrive(String name, Motor leftWheel, Motor rightWheel) { * @param leftWheel * @param rightWheel */ - public TankDrive(Motor leftWheel, Motor rightWheel) { + public TankDrive(MotorSubsystem leftWheel, MotorSubsystem rightWheel) { this("TankDrive", leftWheel, rightWheel); } diff --git a/subsystems/chassis/TankDriveShifting.java b/subsystems/chassis/TankDriveShifting.java index 5f449da6..59632cfc 100644 --- a/subsystems/chassis/TankDriveShifting.java +++ b/subsystems/chassis/TankDriveShifting.java @@ -1,6 +1,6 @@ package org.usfirst.frc4904.standard.subsystems.chassis; -import org.usfirst.frc4904.standard.subsystems.motor.Motor; +import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; /** * A tank drive chassis with the shifting system. This effectively adds a @@ -22,8 +22,8 @@ public class TankDriveShifting extends TankDrive implements ShiftingChassis { * @param shifter */ - public TankDriveShifting(String name, Double turnCorrection, Motor leftWheelA, Motor leftWheelB, Motor rightWheelA, - Motor rightWheelB, SolenoidShifters shifter) { + public TankDriveShifting(String name, Double turnCorrection, MotorSubsystem leftWheelA, MotorSubsystem leftWheelB, MotorSubsystem rightWheelA, + MotorSubsystem rightWheelB, SolenoidShifters shifter) { super(name, turnCorrection, leftWheelA, leftWheelB, rightWheelA, rightWheelB); this.shifter = shifter; } @@ -39,8 +39,8 @@ public TankDriveShifting(String name, Double turnCorrection, Motor leftWheelA, M * @param shifter */ - public TankDriveShifting(Double turnCorrection, Motor leftWheelA, Motor leftWheelB, Motor rightWheelA, - Motor rightWheelB, SolenoidShifters shifter) { + public TankDriveShifting(Double turnCorrection, MotorSubsystem leftWheelA, MotorSubsystem leftWheelB, MotorSubsystem rightWheelA, + MotorSubsystem rightWheelB, SolenoidShifters shifter) { this("Tank Drive Shifting", turnCorrection, leftWheelA, leftWheelB, rightWheelA, rightWheelB, shifter); } @@ -56,7 +56,7 @@ public TankDriveShifting(Double turnCorrection, Motor leftWheelA, Motor leftWhee * @param shifter */ - public TankDriveShifting(String name, Double turnCorrection, Motor leftWheel, Motor rightWheel, + public TankDriveShifting(String name, Double turnCorrection, MotorSubsystem leftWheel, MotorSubsystem rightWheel, SolenoidShifters shifter) { super(name, turnCorrection, leftWheel, rightWheel); this.shifter = shifter; @@ -73,7 +73,7 @@ public TankDriveShifting(String name, Double turnCorrection, Motor leftWheel, Mo * @param shifter */ - public TankDriveShifting(Double turnCorrection, Motor leftWheel, Motor rightWheel, SolenoidShifters shifter) { + public TankDriveShifting(Double turnCorrection, MotorSubsystem leftWheel, MotorSubsystem rightWheel, SolenoidShifters shifter) { this("Tank Drive Shifting", turnCorrection, leftWheel, rightWheel, shifter); } @@ -87,7 +87,7 @@ public TankDriveShifting(Double turnCorrection, Motor leftWheel, Motor rightWhee * @param rightWheelB * @param shifter */ - public TankDriveShifting(String name, Motor leftWheelA, Motor leftWheelB, Motor rightWheelA, Motor rightWheelB, + public TankDriveShifting(String name, MotorSubsystem leftWheelA, MotorSubsystem leftWheelB, MotorSubsystem rightWheelA, MotorSubsystem rightWheelB, SolenoidShifters shifter) { super(name, leftWheelA, leftWheelB, rightWheelA, rightWheelB); this.shifter = shifter; @@ -102,7 +102,7 @@ public TankDriveShifting(String name, Motor leftWheelA, Motor leftWheelB, Motor * @param rightWheelB * @param shifter */ - public TankDriveShifting(Motor leftWheelA, Motor leftWheelB, Motor rightWheelA, Motor rightWheelB, + public TankDriveShifting(MotorSubsystem leftWheelA, MotorSubsystem leftWheelB, MotorSubsystem rightWheelA, MotorSubsystem rightWheelB, SolenoidShifters shifter) { this("Tank Drive Shifting", leftWheelA, leftWheelB, rightWheelA, rightWheelB, shifter); } @@ -115,7 +115,7 @@ public TankDriveShifting(Motor leftWheelA, Motor leftWheelB, Motor rightWheelA, * @param rightWheel * @param shifter */ - public TankDriveShifting(String name, Motor leftWheel, Motor rightWheel, SolenoidShifters shifter) { + public TankDriveShifting(String name, MotorSubsystem leftWheel, MotorSubsystem rightWheel, SolenoidShifters shifter) { super(name, leftWheel, rightWheel); this.shifter = shifter; } @@ -127,7 +127,7 @@ public TankDriveShifting(String name, Motor leftWheel, Motor rightWheel, Solenoi * @param rightWheel * @param shifter */ - public TankDriveShifting(Motor leftWheel, Motor rightWheel, SolenoidShifters shifter) { + public TankDriveShifting(MotorSubsystem leftWheel, MotorSubsystem rightWheel, SolenoidShifters shifter) { this("Tank Drive Shifting", leftWheel, rightWheel, shifter); } diff --git a/subsystems/motor/SensorMotor.java b/subsystems/motor/SensorMotor.java index 6b164ef0..33d323f4 100644 --- a/subsystems/motor/SensorMotor.java +++ b/subsystems/motor/SensorMotor.java @@ -9,6 +9,7 @@ import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; import edu.wpi.first.wpilibj.motorcontrol.MotorController; +@Deprecated public abstract class SensorMotor extends Motor implements DoubleConsumer { protected final MotionController motionController; From 5d4cc7af5c04fe977ced06f7cc4b4f50c1a7c1f9 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sun, 12 Feb 2023 14:37:08 -0800 Subject: [PATCH 027/134] force passing neutralMode and inverted to CANTalonFX/SRX Co-authored-by: annvoor --- custom/motioncontrollers/CANTalonFX.java | 20 ++++++++++++++------ custom/motioncontrollers/CANTalonSRX.java | 10 ++++------ 2 files changed, 18 insertions(+), 12 deletions(-) diff --git a/custom/motioncontrollers/CANTalonFX.java b/custom/motioncontrollers/CANTalonFX.java index 5bc0b132..c86e8cfa 100644 --- a/custom/motioncontrollers/CANTalonFX.java +++ b/custom/motioncontrollers/CANTalonFX.java @@ -1,5 +1,6 @@ package org.usfirst.frc4904.standard.custom.motioncontrollers; +import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; @@ -7,16 +8,23 @@ public class CANTalonFX extends WPI_TalonFX implements BrakeableMotorController { protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; + protected static final InvertType DEFAULT_INVERT_TYPE = InvertType.FollowMaster; // TODO: implement setVoltage with native APIs - public CANTalonFX(int deviceNumber, NeutralMode mode) { + /** + * Represents a Falcon motor in code. You probably want NeutralMode.Brake, InvertType.FollowMaster + * TODO document better + * + * @param deviceNumber + * @param neutralMode + * @param inverted + */ + public CANTalonFX(int deviceNumber, NeutralMode neutralMode, InvertType inverted) { super(deviceNumber); - setNeutralMode(mode); - } - - public CANTalonFX(int deviceNumber) { - this(deviceNumber, DEFAULT_NEUTRAL_MODE); + configFactoryDefault(); // use default settings to prevent unexpected behavior, reccommended in examples + setNeutralMode(neutralMode); + setInverted(inverted); } public void setCoastMode() { diff --git a/custom/motioncontrollers/CANTalonSRX.java b/custom/motioncontrollers/CANTalonSRX.java index 1dcc2916..065ff288 100644 --- a/custom/motioncontrollers/CANTalonSRX.java +++ b/custom/motioncontrollers/CANTalonSRX.java @@ -2,6 +2,7 @@ package org.usfirst.frc4904.standard.custom.motioncontrollers; +import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; @@ -11,13 +12,10 @@ public class CANTalonSRX extends WPI_TalonSRX implements BrakeableMotorControlle // TODO: implement setVoltage with native APIs - public CANTalonSRX(int deviceNumber, NeutralMode mode) { + public CANTalonSRX(int deviceNumber, NeutralMode neutralMode, InvertType inverted) { super(deviceNumber); - setNeutralMode(mode); - } - - public CANTalonSRX(int deviceNumber) { - this(deviceNumber, DEFAULT_NEUTRAL_MODE); + setNeutralMode(neutralMode); + setInverted(inverted); } public void setCoastMode() { From 55d0dfa4ab640e67208c012a6bd39c7716b7a3f2 Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Tue, 14 Feb 2023 19:05:38 -0600 Subject: [PATCH 028/134] Deleted old PID workflow and added --- custom/CustomPIDSourceType.java | 2 - .../motioncontrollers/BangBangController.java | 133 ------ .../CustomPIDController.java | 390 ------------------ .../CANTalonFX.java | 0 .../CANTalonSRX.java | 0 custom/sensors/PIDSensor.java | 38 -- 6 files changed, 563 deletions(-) delete mode 100644 custom/motioncontrollers/BangBangController.java delete mode 100644 custom/motioncontrollers/CustomPIDController.java rename custom/{motioncontrollers => motorcontrollers}/CANTalonFX.java (100%) rename custom/{motioncontrollers => motorcontrollers}/CANTalonSRX.java (100%) delete mode 100644 custom/sensors/PIDSensor.java diff --git a/custom/CustomPIDSourceType.java b/custom/CustomPIDSourceType.java index b45f9e2e..75020e4e 100644 --- a/custom/CustomPIDSourceType.java +++ b/custom/CustomPIDSourceType.java @@ -1,5 +1,3 @@ -// THIS FILE IS TESTED post wpilibj2 - package org.usfirst.frc4904.standard.custom; public enum CustomPIDSourceType { diff --git a/custom/motioncontrollers/BangBangController.java b/custom/motioncontrollers/BangBangController.java deleted file mode 100644 index ed316b39..00000000 --- a/custom/motioncontrollers/BangBangController.java +++ /dev/null @@ -1,133 +0,0 @@ -package org.usfirst.frc4904.standard.custom.motioncontrollers; - -import java.util.function.DoubleConsumer; - -import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.custom.sensors.PIDSensor; - -/** - * A bang bang controller. The bang bang controller increases the value of the - * output if it is below the setpoint or decreases the value of the output if it - * is above the setpoint. - * - */ -public class BangBangController extends MotionController { - protected double error; - protected double A; - protected double F; - protected double threshold; - - public BangBangController(PIDSensor sensor, DoubleConsumer output, double A, double F, double threshold) { - super(sensor, output); - this.A = A; - this.F = F; - this.threshold = threshold; - reset(); - } - /** - * BangBang controller A bang bang controller. The bang bang controller - * increases the value of the output if it is below the setpoint or decreases - * the value of the output if it is above the setpoint. - * - * @param sensor Sensor - * @param A Adjustment term The is the amount the setpoint is increased - * or decreased by. - * @param F Feedforward term The scalar on the input. - * @param threshold The threshold for the bangbang to start doing something. - */ - public BangBangController(PIDSensor sensor, double A, double F, double threshold) { - super(sensor); - this.A = A; - this.F = F; - this.threshold = threshold; - reset(); - } - - /** - * BangBang controller A bang bang controller. The bang bang controller - * increases the value of the output if it is below the setpoint or decreases - * the value of the output if it is above the setpoint. - * - * @param source Sensor - * @param A Adjustment term The is the amount the setpoint is increased - * or decreased by. - * @param F Feedforward term The scalar on the input. - * @param threshold The threshold for the bangbang to start doing something. - */ - - public BangBangController(PIDSensor sensor, double A, double F) { - this(sensor, A, F, Double.MIN_VALUE); - } - - /** - * Sets the stored error value to zero (0) - */ - @Override - public void resetErrorToZero() { - error = 0; - } - - /** - * Get the current output of the bang bang controller. This should be used to - * set the output. - * - * @return The current output of the bang bang controller. - * @throws InvalidSensorException - */ - @Override - public double getSafely() throws InvalidSensorException { - if (!isEnabled()) { - return F * setpoint; - } - double input = 0.0; - input = sensor.pidGetSafely(); - error = setpoint - input; - LogKitten.v(input + " " + setpoint + " " + error); - if (continuous) { - double range = inputMax - inputMin; - // If the error is more than half of the range, it is faster to increase the - // error and loop around the boundary - if (Math.abs(error) > range / 2) { - if (error > 0) { - error -= range; - } else { - error += range; - } - } - } - if (error < 0.0 && Math.abs(error) > threshold) { - LogKitten.v("+A " + error); - return A + F * setpoint; - } else if (error > 0.0 && Math.abs(error) > threshold) { - LogKitten.v("-A" + error); - return -1.0 * A + F * setpoint; - } - return F * setpoint; - } - - /** - * Get the current output of the bang bang controller. This should be used to - * set the output. - * - * @return The current output of the bang bang controller. - * @warning does not indicate sensor error - */ - @Override - public double get() { - try { - return getSafely(); - } catch (Exception e) { - LogKitten.ex(e); - return 0; - } - } - - /** - * @return The most recent error. - */ - @Override - public double getError() { - return error; - } -} \ No newline at end of file diff --git a/custom/motioncontrollers/CustomPIDController.java b/custom/motioncontrollers/CustomPIDController.java deleted file mode 100644 index 04520095..00000000 --- a/custom/motioncontrollers/CustomPIDController.java +++ /dev/null @@ -1,390 +0,0 @@ -// THIS FILE IS TESTED post wpilibj2 - -package org.usfirst.frc4904.standard.custom.motioncontrollers; - -import java.util.function.DoubleConsumer; - -import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.custom.sensors.NativeDerivativeSensor; -import org.usfirst.frc4904.standard.custom.sensors.PIDSensor; - -import org.usfirst.frc4904.standard.custom.CustomPIDSourceType; -import org.usfirst.frc4904.standard.custom.motioncontrollers.CustomPIDController; -import edu.wpi.first.hal.util.BoundaryException; - -/** - * An extremely basic PID controller. It does not differentiate between rate and - * distance. - * - */ -public class CustomPIDController extends MotionController { - protected double P; - protected double I; - protected double IPrime = 0.0; - protected double D; - protected double F; - protected double integralThreshold = 0.0; - protected double totalError; - protected double lastError; - protected double accumulatedOutput; - protected long lastTime; - protected double lastErrorDerivative; - protected double derivativeTolerance = Double.MIN_VALUE; - protected double minimumNominalOutput = 0.0; - - /** - * An extremely basic PID controller. It does not differentiate between rate and - * distance. - * - * @param P Initial P constant - * @param I Initial I constant - * @param D Initial D constant - * @param F Initial F (feed forward) constant - * @param sensor The sensor linked to the output - * @param output A motor that accepts PID controller inputs - */ - public CustomPIDController(double P, double I, double D, double F, PIDSensor sensor, DoubleConsumer output) { - super(sensor, output); - this.P = P; - this.I = I; - this.D = D; - this.F = F; - } - - /** - * An extremely basic PID controller. It does not differentiate between rate and - * distance. - * - * @param P Initial P constant - * @param I Initial I constant - * @param D Initial D constant - * @param F Initial F (feed forward) constant - * @param sensor The sensor linked to the output - */ - public CustomPIDController(double P, double I, double D, double F, PIDSensor sensor) { - super(sensor); - this.P = P; - this.I = I; - this.D = D; - this.F = F; - } - - /** - * An extremely basic PID controller. It does not differentiate between rate and - * distance. - * - * @param P Initial P constant - * @param I Initial I constant - * @param D Initial D constant - * @param F Initial F (feed forward) constant - * @param sensor The sensor linked to the output - */ - public CustomPIDController(double P, double I, double D, double F, PIDSensor sensor, double integralThreshold) { - this(P, I, D, F, sensor); - this.setIThreshold(integralThreshold); - } - - /** - * An extremely basic PID controller. It does not differentiate between rate and - * distance. - * - * @param P Initial P constant - * @param I Initial I constant - * @param D Initial D constant - * @param sensor The sensor linked to the output - */ - public CustomPIDController(double P, double I, double D, PIDSensor sensor) { - super(sensor); - this.P = P; - this.I = I; - this.D = D; - } - - /** - * An extremely basic PID controller. It does not differentiate between rate and - * distance. - * - * @param sensor The sensor linked to the output - */ - public CustomPIDController(PIDSensor sensor) { - this(0, 0, 0, sensor); - } - - /** - * @return The current P value - */ - public double getP() { - return P; - } - - /** - * @return The current I value - */ - public double getI() { - return I; - } - - /** - * @return The current D value - */ - public double getD() { - return D; - } - - /** - * @return The current F (feed forward) value - */ - public double getF() { - return F; - } - - /** - * @return The current I' (ouput integral) value - */ - public double getIPrime() { - return IPrime; - } - - /** - * @param IPrime Integral of the PID output - */ - public void setIPrime(double IPrime) { - this.IPrime = IPrime; - } - - /** - * - * @return The current minimumNominalOutput (minimum nominal output) value - */ - public double getMinimumNominalOutput() { - return minimumNominalOutput; - } - - /** - * Sets the parameters of the PID loop - * - * @param P Proportional - * @param I Integral - * @param D Derivative - * - * If you do not know what these mean, please refer to this link: - * https://en.wikipedia.org/wiki/PID_controller - */ - public void setPID(double P, double I, double D) { - this.P = P; - this.I = I; - this.D = D; - } - - /** - * Sets the parameters of the PID loop - * - * @param P Proportional - * @param I Integral - * @param D Derivative - * @param F Feed forward (scalar on input added to output) - * - * If you do not know what these mean, please refer to this link: - * https://en.wikipedia.org/wiki/PID_controller - */ - public void setPIDF(double P, double I, double D, double F) { - this.P = P; - this.I = I; - this.D = D; - this.F = F; - } - - /** - * Set the maximum derivative value at which the controller can be considered - * "on-target," given that the source suggests that the setpoint has been - * reached. Set this value to a higher value if overshoot due to high velocity - * is a problem. - * - * @param derivativeTolerance the maximum derivative value for onTarget() to - * return true - */ - public void setDerivativeTolerance(double derivativeTolerance) { - this.derivativeTolerance = derivativeTolerance; - } - - /** - * Get the absolute derivative value stop condition. - * - * @see #setDerivativeTolerance(double) - * @return the maximum derivative value for onTarget() to return true - */ - public double getDerivativeTolerance() { - return derivativeTolerance; - } - - /** - * - * @param minimumNominalOutput Minimum Nominal Output result will be set to - * ±this value if the absolute value of the result - * is less than this value. This is useful if the - * motor can only run well above a value. - */ - public void setMinimumNominalOutput(double minimumNominalOutput) { - this.minimumNominalOutput = minimumNominalOutput; - } - - /** - * Sets the threshold below which the I term becomes active. When the I term is - * active, the error sum increases. When the I term is not active, the error sum - * is set to zero. - * - * @param integralThreshold - */ - public void setIThreshold(double integralThreshold) { - if (integralThreshold < 0) { - throw new BoundaryException("I threshold negative"); - } - this.integralThreshold = integralThreshold; - } - - /** - * Gets the threshold below which the I term becomes active. When the I term is - * active, the error sum increases. When the I term is not active, the error sum - * is set to zero. - * - * @return - */ - public double getIThreshold() { - return integralThreshold; - } - - /** - * Resets the PID controller error to zero. - */ - @Override - protected void resetErrorToZero() { - totalError = 0; - lastError = 0; - } - - @Override - public double getError() { - return lastError; - } - - /** - * Feedforward constant. If a position controller, the F constant is multiplied - * by the sign of the setpoint. If a velocity controller, F is multiplied by - * setpoint. - * - * @return the feedforward value for the given setpoint. - */ - // private double feedForward() { - // if (sensor.getCustomPIDSourceType() == CustomPIDSourceType.kDisplacement) { - // return F * Math.signum(lastError); - // } else { - // return F * setpoint; - // } - // } - - /** - * Get the current output of the PID loop. This should be used to set the output - * (like a Motor). - * - * @return The current output of the PID loop. - * @throws InvalidSensorException when a sensor fails - */ - @Override - public double getSafely() throws InvalidSensorException { - // If PID is not enabled, use feedforward only - if (!isEnabled()) { - // return feedForward(); - } - double input = 0.0; - input = sensor.pidGet(); - double error = setpoint - input; - // Account for continuous input ranges - if (continuous) { - double range = inputMax - inputMin; - // If the error is more than half of the range, it is faster to increase the - // error and loop around the boundary - if (Math.abs(error) > range / 2) { - if (error > 0) { - error -= range; - } else { - error += range; - } - } - } - long latestTime = System.currentTimeMillis(); - long timeDiff = latestTime - lastTime; - lastTime = latestTime; - // If we just reset, then the lastTime could be way before the latestTime and so - // timeDiff would be huge. - // This would lead to a very big I (and a big D, briefly). - // Also, D could be unpredictable because lastError could be wildly different - // than error (since they're - // separated by more than a tick in time). - // Hence, if we just reset, just pretend we're still disabled and record the - // lastTime and lastError for next tick. - if (didJustReset()) { - lastError = error; - // return feedForward(); - } - // Check if the sensor supports native derivative calculations and that we're - // doing displacement PID - // (if we're doing rate PID, then getRate() would be the PID input rather then - // the input's derivative) - double errorDerivative; - if (sensor instanceof NativeDerivativeSensor - && sensor.getCustomPIDSourceType() == CustomPIDSourceType.kDisplacement) { - errorDerivative = ((NativeDerivativeSensor) sensor).getRateSafely(); - } else { - // Calculate the approximation of the derivative. - errorDerivative = (error - lastError) / timeDiff; - } - if (integralThreshold == 0 || Math.abs(error) < integralThreshold) { - // Calculate the approximation of the error's integral - totalError += error * timeDiff; - } else {// if (error/Math.abs(error) == -totalError/Math.abs(totalError)){ - totalError = 0.0; - } - // Calculate the result using the PIDF formula - double PIDresult = P * error + I * totalError + D * errorDerivative + F * ((sensor.getCustomPIDSourceType() == CustomPIDSourceType.kDisplacement) ? Math.signum(error) : setpoint); - double output = PIDresult + IPrime * accumulatedOutput; - accumulatedOutput += PIDresult * timeDiff; - // Save the error for calculating future derivatives - lastError = error; - lastErrorDerivative = errorDerivative; - if (capOutput) { - // Limit the result to be within the output range [outputMin, outputMax] - output = Math.max(Math.min(output, outputMax), outputMin); - } - if (Math.abs(output) < minimumNominalOutput) { - output = Math.signum(output) * minimumNominalOutput; - } - return output; - } - - /** - * Get the current output of the PID loop. This should be used to set the output - * (like a Motor). - * - * @return The current output of the PID loop. - * @warning does not indicate sensor error - */ - @Override - public double get() { - try { - return getSafely(); - } catch (Exception e) { - LogKitten.ex(e); - return 0; - } - } - - public boolean derivativeOnTarget() { - return derivativeTolerance == 0 || Math.abs(lastErrorDerivative) < derivativeTolerance; - } - - @Override - public boolean onTarget() { - return super.onTarget() && derivativeOnTarget(); - } -} diff --git a/custom/motioncontrollers/CANTalonFX.java b/custom/motorcontrollers/CANTalonFX.java similarity index 100% rename from custom/motioncontrollers/CANTalonFX.java rename to custom/motorcontrollers/CANTalonFX.java diff --git a/custom/motioncontrollers/CANTalonSRX.java b/custom/motorcontrollers/CANTalonSRX.java similarity index 100% rename from custom/motioncontrollers/CANTalonSRX.java rename to custom/motorcontrollers/CANTalonSRX.java diff --git a/custom/sensors/PIDSensor.java b/custom/sensors/PIDSensor.java deleted file mode 100644 index e5bc3800..00000000 --- a/custom/sensors/PIDSensor.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.usfirst.frc4904.standard.custom.sensors; - -import org.usfirst.frc4904.standard.custom.CustomPIDSourceType; - -public interface PIDSensor { - /** - * Set which parameter of the device you are using as a process control - * variable. - * - * @param pidSource An enum to select the parameter. - */ - public void setCustomPIDSourceType(CustomPIDSourceType pidSource); - - /** - * Get which parameter of the device you are using as a process control - * variable. - * - * @return the currently selected PID source parameter - */ - public CustomPIDSourceType getCustomPIDSourceType(); - - /** - * Get the result to use in PIDController $ - * - * @return the result to use in PIDController - * @warning does not report sensor errors, will just return 0 - */ - public double pidGet(); - - /** - * Get the result to use in PIDController $ - * - * @return the result to use in PIDController - * @throws InvalidSensorException when sensor data should not be used for PID - * due to potential inaccuracy - */ - public double pidGetSafely() throws InvalidSensorException; -} \ No newline at end of file From 382a5c731f86c713f92f4c31a3eb0d66b8f3e6a6 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Tue, 14 Feb 2023 18:28:28 -0800 Subject: [PATCH 029/134] delete chassis --- commands/chassis/ChassisConstant.java | 84 -------- commands/chassis/ChassisIdle.java | 37 ---- commands/chassis/ChassisMinimumDistance.java | 128 ----------- commands/chassis/ChassisMove.java | 157 -------------- commands/chassis/ChassisMoveDistance.java | 153 -------------- commands/chassis/ChassisShift.java | 73 ------- commands/chassis/ChassisTurn.java | 156 -------------- commands/chassis/ChassisTurnAbsolute.java | 80 ------- commands/chassis/SimpleSplines.java | 164 --------------- subsystems/chassis/Chassis.java | 81 ------- subsystems/chassis/MecanumChassis.java | 111 ---------- subsystems/chassis/SensorDrive.java | 147 ------------- subsystems/chassis/ShiftingChassis.java | 12 -- subsystems/chassis/SolenoidShifters.java | 210 ------------------- subsystems/chassis/SplinesDrive.java | 38 ---- subsystems/chassis/SwerveChassis.java | 50 ----- subsystems/chassis/TankDrive.java | 130 ------------ subsystems/chassis/TankDriveShifting.java | 141 ------------- 18 files changed, 1952 deletions(-) delete mode 100644 commands/chassis/ChassisConstant.java delete mode 100644 commands/chassis/ChassisIdle.java delete mode 100644 commands/chassis/ChassisMinimumDistance.java delete mode 100644 commands/chassis/ChassisMove.java delete mode 100644 commands/chassis/ChassisMoveDistance.java delete mode 100644 commands/chassis/ChassisShift.java delete mode 100644 commands/chassis/ChassisTurn.java delete mode 100644 commands/chassis/ChassisTurnAbsolute.java delete mode 100644 commands/chassis/SimpleSplines.java delete mode 100644 subsystems/chassis/Chassis.java delete mode 100644 subsystems/chassis/MecanumChassis.java delete mode 100644 subsystems/chassis/SensorDrive.java delete mode 100644 subsystems/chassis/ShiftingChassis.java delete mode 100644 subsystems/chassis/SolenoidShifters.java delete mode 100644 subsystems/chassis/SplinesDrive.java delete mode 100644 subsystems/chassis/SwerveChassis.java delete mode 100644 subsystems/chassis/TankDrive.java delete mode 100644 subsystems/chassis/TankDriveShifting.java diff --git a/commands/chassis/ChassisConstant.java b/commands/chassis/ChassisConstant.java deleted file mode 100644 index 257bdbfa..00000000 --- a/commands/chassis/ChassisConstant.java +++ /dev/null @@ -1,84 +0,0 @@ -package org.usfirst.frc4904.standard.commands.chassis; - -import org.usfirst.frc4904.standard.custom.ChassisController; -import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; - -public class ChassisConstant extends CommandBase implements ChassisController { - protected final ParallelRaceGroup move; - protected final double x; - protected final double y; - protected double turn; - protected double timeout; - - /** - * - * @param name - * @param chassis - * @param x - * @param y - * @param turn - * @param timeout - */ - public ChassisConstant(String name, Chassis chassis, double x, double y, double turn, double timeout) { - move = new ChassisMove(chassis, this).withTimeout(timeout); - this.timeout = timeout; - this.x = x; - this.y = y; - this.turn = turn; - setName(name); - addRequirements(chassis); - } - - /** - * - * @param chassis - * @param x - * @param y - * @param turn - * @param timeout - */ - public ChassisConstant(Chassis chassis, double x, double y, double turn, double timeout) { - this("Chassis Constant", chassis, x, y, turn, timeout); - } - - @Override - public double getX() { - return x; - } - - @Override - public double getY() { - return y; - } - - @Override - public double getTurnSpeed() { - return turn; - } - - @Override - public void initialize() { - move.schedule(); - } - - /** - * - * The command has timed out if the time since scheduled is greater than the - * timeout - * - * @return finished - * - */ - @Override - public boolean isFinished() { - return move.isFinished() || CommandScheduler.getInstance().timeSinceScheduled(this) >= this.timeout; - } - - @Override - public void end(boolean interrupted) { - move.cancel(); - } -} diff --git a/commands/chassis/ChassisIdle.java b/commands/chassis/ChassisIdle.java deleted file mode 100644 index 023afcbf..00000000 --- a/commands/chassis/ChassisIdle.java +++ /dev/null @@ -1,37 +0,0 @@ -package org.usfirst.frc4904.standard.commands.chassis; - -import org.usfirst.frc4904.standard.commands.motor.MotorIdle; -import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; -import org.usfirst.frc4904.standard.subsystems.motor.Motor; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.Command; - -/** - * - * This command causes the Chassis to idle by spawning a MotorIdle for every - * Motor in the Chassis. - * - */ -public class ChassisIdle extends ParallelCommandGroup { - /** - * - * @param name - * @param chassis The robot Chassis to idle. - */ - public ChassisIdle(String name, Chassis chassis) { - for (Motor motor : chassis.getMotors()) { - addCommands((Command) new MotorIdle(motor)); - } - - setName(name); - addRequirements(chassis); - } - - /** - * - * @param chassis The robot Chassis to idle. - */ - public ChassisIdle(Chassis chassis) { - this("ChassisIdle", chassis); - } -} diff --git a/commands/chassis/ChassisMinimumDistance.java b/commands/chassis/ChassisMinimumDistance.java deleted file mode 100644 index 5813921d..00000000 --- a/commands/chassis/ChassisMinimumDistance.java +++ /dev/null @@ -1,128 +0,0 @@ -package org.usfirst.frc4904.standard.commands.chassis; - -import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.custom.sensors.CustomEncoder; -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; - -public class ChassisMinimumDistance extends ChassisConstant { - protected CustomEncoder[] encoders; - protected final ChassisConstant fallbackCommand; - protected double distance; - protected double[] initialDistances; - - /** - * Constructor. This command moves the chassis forward a known distance via a - * set of encoders. The distance is calculated as the average of the provided - * encoders. Control is purely bang (as opposed to bang-bang/pid). The chassis - * will move at the speed until it passes the point, then will stop. - * - * @param name - * @param chassis - * @param distance distance to move in encoder ticks - * @param speed the speed to move at - * @param fallbackCommand If the sensor fails for some reason, this command will - * be cancelled, then the fallbackCommand will start - * @param encoders - */ - public ChassisMinimumDistance(String name, Chassis chassis, double distance, double speed, - ChassisConstant fallbackCommand, CustomEncoder... encoders) { - super(name, chassis, 0.0, speed, 0.0, Double.MAX_VALUE); - this.encoders = encoders; - this.distance = distance; - this.fallbackCommand = fallbackCommand; - initialDistances = new double[encoders.length]; - } - - /** - * Constructor. This command moves the chassis forward a known distance via a - * set of encoders. The distance is calculated as the average of the provided - * encoders. Control is purely bang (as opposed to bang-bang/pid). The chassis - * will move at the speed until it passes the point, then will stop. - * - * @param chassis - * @param distance distance to move in encoder ticks - * @param speed the speed to move at - * @param fallbackCommand If the sensor fails for some reason, this command will - * be cancelled, then the fallbackCommand will start - * @param encoders - */ - public ChassisMinimumDistance(Chassis chassis, double distance, double speed, ChassisConstant fallbackCommand, - CustomEncoder... encoders) { - this("Chassis Minimum Distance", chassis, distance, speed, fallbackCommand, encoders); - } - - /** - * Constructor. This command moves the chassis forward a known distance via a - * set of encoders. The distance is calculated as the average of the provided - * encoders. Control is purely bang (as opposed to bang-bang/pid). The chassis - * will move at the speed until it passes the point, then will stop. - * - * @param name - * @param chassis - * @param distance distance to move in encoder ticks - * @param speed the speed to move at - * @param encoders - */ - public ChassisMinimumDistance(String name, Chassis chassis, double distance, double speed, - CustomEncoder... encoders) { - this(name, chassis, distance, speed, null, encoders); - } - - /** - * Constructor. This command moves the chassis forward a known distance via a - * set of encoders. The distance is calculated as the average of the provided - * encoders. Control is purely bang (as opposed to bang-bang/pid). The chassis - * will move at the speed until it passes the point, then will stop. - * - * @param chassis - * @param distance distance to move in encoder ticks - * @param speed the speed to move at - * @param encoders - */ - public ChassisMinimumDistance(Chassis chassis, double distance, double speed, CustomEncoder... encoders) { - this(chassis, distance, speed, null, encoders); - } - - @Override - public void initialize() { - super.initialize(); - for (int i = 0; i < encoders.length; i++) { - try { - initialDistances[i] = encoders[i].getDistanceSafely(); - } catch (InvalidSensorException e) { - cancel(); - return; - } - } - } - - @Override - public boolean isFinished() { - double distanceSum = 0; - for (int i = 0; i < encoders.length; i++) { - try { - distanceSum += encoders[i].getDistanceSafely() - initialDistances[i]; - LogKitten.d("Encoder " + i + " reads " + encoders[i].getDistanceSafely() + " out of " + distance); - } catch (InvalidSensorException e) { - cancel(); - if (fallbackCommand != null) { - fallbackCommand.schedule(); - } - return true; - } - } - - return distanceSum / (double) encoders.length >= distance; - } - - @Override - public void end(boolean interrupted) { - super.end(interrupted); - if (interrupted) { - LogKitten.w("Interrupted! " + getName() + " is in undefined location."); - } else { - LogKitten.v("Finished traveling " + distance + " units (as set by setDistancePerPulse)"); - } - } -} \ No newline at end of file diff --git a/commands/chassis/ChassisMove.java b/commands/chassis/ChassisMove.java deleted file mode 100644 index f1f3b208..00000000 --- a/commands/chassis/ChassisMove.java +++ /dev/null @@ -1,157 +0,0 @@ -// THIS FILE IS TESTED post wpilibj2 - -package org.usfirst.frc4904.standard.commands.chassis; - -import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.commands.motor.MotorSet; -import org.usfirst.frc4904.standard.custom.ChassisController; -import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; -import org.usfirst.frc4904.standard.subsystems.motor.Motor; -import org.usfirst.frc4904.standard.subsystems.motor.VelocitySensorMotor; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; - -/** - * - * This command moves the chassis. It move based on the driver class. Note that - * it supports all types of chassis. The chassis is used to calculate the motor - * movement. The command works by creating a movement command for each motor. - * This is the best way to handle this because it allows each motor to be a full - * subsystem. - */ -public class ChassisMove extends ParallelCommandGroup { - protected final MotorSet[] motorSpins; - protected double[] motorSpeeds; - protected final Motor[] motors; - protected final boolean usePID; - protected final Chassis chassis; - protected final ChassisController controller; - - /** - * - * @param name - * @param chassis The robot's Chassis. - * @param controller A ChassisController to control the Chassis, such as a - * Driver or autonomous routine. - * @param usePID Whether to enable PID using any SensorMotors that Chassis - * has. - */ - public ChassisMove(String name, Chassis chassis, ChassisController controller, boolean usePID) { - super(); - this.chassis = chassis; - this.controller = controller; - this.usePID = usePID; - addRequirements(chassis); - setName(name); - motors = chassis.getMotors(); - motorSpins = new MotorSet[motors.length]; - for (int i = 0; i < motors.length; i++) { - motorSpins[i] = new MotorSet(motors[i].getName(), motors[i]); - } - addCommands(motorSpins); - } - - /** - * - * @param name - * @param chassis The robot's chassis. - * @param controller A ChassisController to control the Chassis, such as a - * Driver or autonomous routine. - */ - public ChassisMove(String name, Chassis chassis, ChassisController controller) { - this(name, chassis, controller, false); - } - - /** - * - * @param chassis The robot's Chassis. - * @param controller A ChassisController to control the Chassis, such as a - * Driver or autonomous routine. - * @param usePID Whether to enable PID using any SensorMotors that Chassis - * has. - */ - public ChassisMove(Chassis chassis, ChassisController controller, boolean usePID) { - this("Chassis Move", chassis, controller, usePID); - } - - /** - * - * - * @param chassis The robot's chassis. - * @param controller A ChassisController to control the Chassis, such as a - * Driver or autonomous routine. - * - */ - public ChassisMove(Chassis chassis, ChassisController controller) { - this("Chassis Move", chassis, controller); - } - - /** - * - * VelocitySensorMotors will attempt to very precisely achieve the speed set by - * this command when PID is enabled PositionSensorMotors will either attempt to - * maintain their previous position, or worse, will try to move to somewhere - * between -1.0 and 1.0, which is probably not the correct position regardless - * of the scaling. - * - */ - @Override - public void initialize() { - super.initialize(); - for (Motor motor : motors) { - if (motor instanceof VelocitySensorMotor) { - if (usePID) { - ((VelocitySensorMotor) motor).enableMotionController(); - } else { - ((VelocitySensorMotor) motor).disableMotionController(); - } - } - } - LogKitten.v("ChassisMove initialized"); - } - - @Override - public void execute() { - super.execute(); - chassis.moveCartesian(controller.getX(), controller.getY(), controller.getTurnSpeed()); - motorSpeeds = chassis.getMotorSpeeds(); - StringBuilder motorSpeedsString = new StringBuilder(); - motorSpeedsString.append("Motor speeds:"); - for (int i = 0; i < motorSpins.length; i++) { - LogKitten.d(Double.toString(motorSpeeds[i])); - motorSpins[i].set(motorSpeeds[i]); - motorSpeedsString.append(' '); - motorSpeedsString.append(motorSpeeds[i]); - } - LogKitten.d("ChassisMove executing"); - LogKitten.d(motorSpeedsString.toString()); - } - - @Override - public boolean isFinished() { - return false; - } - - /** - * It's important to stop motor spins before the ChassisMove command stops. - * Otherwise, the spins might briefly (for one tick) use the previously set - * values if the ChassisMove command is reused. - */ - protected void stopMotorSpins() { - for (int i = 0; i < motors.length; i++) { - motorSpins[i].set(0); - } - } - - @Override - public void end(boolean interrupted) { - super.end(interrupted); - stopMotorSpins(); - - if (interrupted) { - LogKitten.w("ChassisMove interrupted"); - } else { - LogKitten.v("ChassisMove ended"); - } - - } -} diff --git a/commands/chassis/ChassisMoveDistance.java b/commands/chassis/ChassisMoveDistance.java deleted file mode 100644 index c09f1832..00000000 --- a/commands/chassis/ChassisMoveDistance.java +++ /dev/null @@ -1,153 +0,0 @@ -package org.usfirst.frc4904.standard.commands.chassis; - -import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.custom.ChassisController; -import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class ChassisMoveDistance extends CommandBase implements ChassisController { - protected final ChassisMove chassisMove; - protected final MotionController motionController; - protected final Command fallbackCommand; - protected final double distance; - protected boolean runOnce; - - /** - * Constructor. This command moves the chassis forward a known distance via a - * set of encoders. The distance is calculated as the average of the provided - * encoders. The speed is decided by the provided motionController. - * - * @param name - * @param chassis - * @param distance distance to move in encoder ticks - * @param motionController - * @param fallbackCommand If the sensor fails for some reason, this command - * will be cancelled, then the fallbackCommand will - * start - * @param encoders - */ - public ChassisMoveDistance(String name, Chassis chassis, double distance, MotionController motionController, - Command fallbackCommand) { - chassisMove = new ChassisMove(chassis, this, false); - this.motionController = motionController; - this.distance = distance; - this.fallbackCommand = fallbackCommand; - runOnce = false; - setName(name); - addRequirements(chassis); - } - - /** - * - * Constructor. This command moves the chassis forward a known distance via a - * set of encoders. The distance is calculated as the average of the provided - * encoders. The speed is decided by the provided motionController. - * - * @param chassis - * @param distance distance to move in encoder ticks - * @param motionController - * @param encoders - * @param name - */ - public ChassisMoveDistance(String name, Chassis chassis, double distance, MotionController motionController) { - this(name, chassis, distance, motionController, null); - } - - /** - * - * Constructor. This command moves the chassis forward a known distance via a - * set of encoders. The distance is calculated as the average of the provided - * encoders. The speed is decided by the provided motionController. - * - * @param chassis - * @param distance distance to move in encoder ticks - * @param motionController - * @param encoders - */ - public ChassisMoveDistance(Chassis chassis, double distance, MotionController motionController) { - this("ChassisMoveDistance", chassis, distance, motionController); - } - - /** - * Constructor. This command moves the chassis forward a known distance via a - * set of encoders. The distance is calculated as the average of the provided - * encoders. The speed is decided by the provided motionController. - * - * @param chassis - * @param distance distance to move in encoder ticks - * @param motionController - * @param fallbackCommand If the sensor fails for some reason, this command - * will be cancelled, then the fallbackCommand will - * start - * @param encoders - */ - public ChassisMoveDistance(Chassis chassis, double distance, MotionController motionController, - Command fallbackCommand) { - this("ChassisMoveDistance", chassis, distance, motionController, fallbackCommand); - } - - @Override - public void initialize() { - chassisMove.schedule(); - try { - motionController.resetSafely(); - } catch (InvalidSensorException e) { - LogKitten.w("Cancelling ChassisMoveDistance with InvalidSensorException"); - chassisMove.cancel(); - cancel(); - if (fallbackCommand != null) { - fallbackCommand.schedule(); - } - return; - } - motionController.setSetpoint(motionController.getSensorValue() + distance); - motionController.enable(); - } - - @Override - public double getX() { - return 0; - } - - @Override - public double getY() { - double speed; - try { - speed = motionController.getSafely(); - } catch (InvalidSensorException e) { - LogKitten.w("Cancelling ChassisMoveDistance with InvalidSensorException"); - chassisMove.cancel(); - cancel(); - if (fallbackCommand != null) { - fallbackCommand.schedule(); - } - speed = 0; - } - LogKitten.d("MotionProfileSpeed: " + speed); - return speed; - } - - @Override - public double getTurnSpeed() { - return 0; - } - - @Override - public void end(boolean interrupted) { - chassisMove.cancel(); - motionController.disable(); - motionController.reset(); - runOnce = false; - } - - @Override - public boolean isFinished() { - if (chassisMove.isScheduled() && !runOnce) { - runOnce = true; - } - return (motionController.onTarget() || !chassisMove.isScheduled()) && runOnce; - } -} diff --git a/commands/chassis/ChassisShift.java b/commands/chassis/ChassisShift.java deleted file mode 100644 index db4385a8..00000000 --- a/commands/chassis/ChassisShift.java +++ /dev/null @@ -1,73 +0,0 @@ -package org.usfirst.frc4904.standard.commands.chassis; - -import org.usfirst.frc4904.standard.subsystems.chassis.SolenoidShifters; -import edu.wpi.first.wpilibj2.command.CommandBase; - -/** - * This command shifts a set of solenoids. - * - */ -public class ChassisShift extends CommandBase { - protected final SolenoidShifters solenoids; - protected final SolenoidShifters.SolenoidState state; - - /** - * Shifts the solenoids to the provided state - * - * @param name - * @param solenoids - * @param state - */ - public ChassisShift(String name, SolenoidShifters solenoids, SolenoidShifters.SolenoidState state) { - super(); - this.state = state; - this.solenoids = solenoids; - setName(name); - addRequirements(solenoids); - } - - /** - * Shifts the solenoids to the provided state - * - * @param solenoids - * @param state - */ - public ChassisShift(SolenoidShifters solenoids, SolenoidShifters.SolenoidState state) { - this("ChassisShift", solenoids, state); - } - - /** - * Toggles the solenoids - * - * @param name - * @param solenoids - */ - public ChassisShift(String name, SolenoidShifters solenoids) { - this(name, solenoids, null); - } - - /** - * Toggles the solenoids - * - * @param solenoids - */ - public ChassisShift(SolenoidShifters solenoids) { - this("ChassisShift", solenoids); - } - - @Override - public void initialize() { - if (state == null) { - // null state means toggle - solenoids.set(); // TODO: needs to be implemented - } else { - // not null state means shift to it directly - solenoids.set(state); - } - } - - @Override - public boolean isFinished() { - return false; // Encoders stay in whatever state until shifted elsewhere. - } -} diff --git a/commands/chassis/ChassisTurn.java b/commands/chassis/ChassisTurn.java deleted file mode 100644 index 1c77590d..00000000 --- a/commands/chassis/ChassisTurn.java +++ /dev/null @@ -1,156 +0,0 @@ -package org.usfirst.frc4904.standard.commands.chassis; - -import org.usfirst.frc4904.standard.custom.ChassisController; -import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; -import org.usfirst.frc4904.standard.custom.sensors.IMU; -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class ChassisTurn extends CommandBase implements ChassisController { - protected final ChassisMove move; - protected double initialAngle; - protected final double finalAngle; - protected final MotionController motionController; - protected final CommandBase fallbackCommand; - protected final IMU imu; - protected boolean runOnce; - - /** - * - * Constructor This command rotates the chassis to a position relative to the - * current angle of the robot - * - * @param name - * @param chassis - * @param finalAngle - * @param imu - * @param fallbackCommand If the sensor fails for some reason, this command - * will be cancelled, then the fallbackCommand will - * start - * @param motionController - */ - public ChassisTurn(String name, final Chassis chassis, final double finalAngle, final IMU imu, - final CommandBase fallbackCommand, MotionController motionController) { - move = new ChassisMove(chassis, this); - this.finalAngle = -((finalAngle + 360) % 360 - 180); - this.imu = imu; - this.motionController = motionController; - this.fallbackCommand = fallbackCommand; - setName(name); - } - - /** - * - * Constructor This command rotates the chassis to a position relative to the - * current angle of the robot - * - * @param name - * @param chassis - * @param finalAngle - * @param imu - * @param motionController - */ - public ChassisTurn(String name, Chassis chassis, double finalAngle, IMU imu, MotionController motionController) { - this(name, chassis, finalAngle, imu, null, motionController); - } - - /** - * - * Constructor This command rotates the chassis to a position relative to the - * current angle of the robot - * - * @param chassis - * @param finalAngle - * @param imu - * @param motionController - */ - public ChassisTurn(Chassis chassis, double finalAngle, IMU imu, MotionController motionController) { - this("Chassis Turn", chassis, finalAngle, imu, null, motionController); - } - - /** - * - * Constructor This command rotates the chassis to a position relative to the - * current angle of the robot - * - * @param chassis - * @param finalAngle - * @param imu - * @param fallbackCommand If the sensor fails for some reason, this command - * will be cancelled, then the fallbackCommand will - * start - * @param motionController - */ - public ChassisTurn(Chassis chassis, double finalAngle, IMU imu, CommandBase fallbackCommand, - MotionController motionController) { - this("Chassis Turn", chassis, finalAngle, imu, fallbackCommand, motionController); - } - - @Override - public double getX() { - return 0.0; - } - - @Override - public double getY() { - return 0.0; - } - - @Override - public double getTurnSpeed() { - try { - return motionController.getSafely(); - } catch (final InvalidSensorException e) { - move.cancel(); - cancel(); - if (fallbackCommand != null) { - fallbackCommand.schedule(); - } - return 0; - } - } - - @Override - public void initialize() { - move.schedule(); - initialAngle = imu.getYaw(); - try { - motionController.resetSafely(); - } catch (final InvalidSensorException e) { - move.cancel(); - cancel(); - if (fallbackCommand != null) { - fallbackCommand.schedule(); - } - return; - } - } - - @Override - public void execute() { - motionController.setSetpoint(((finalAngle + initialAngle) + 360) % 360 - 180); - if (!motionController.isEnabled()) { - motionController.enable(); - } - } - - @Override - public boolean isFinished() { - if (move.isScheduled() && !runOnce) { - runOnce = true; - } - return (motionController.onTarget() || !move.isScheduled()) && runOnce; - } - - @Override - public void end(boolean interrupted) { - motionController.disable(); - move.cancel(); - - if (fallbackCommand != null && fallbackCommand.isScheduled()) { - fallbackCommand.cancel(); - } - runOnce = false; - } -} \ No newline at end of file diff --git a/commands/chassis/ChassisTurnAbsolute.java b/commands/chassis/ChassisTurnAbsolute.java deleted file mode 100644 index 6431be57..00000000 --- a/commands/chassis/ChassisTurnAbsolute.java +++ /dev/null @@ -1,80 +0,0 @@ -package org.usfirst.frc4904.standard.commands.chassis; - -import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; -import org.usfirst.frc4904.standard.custom.sensors.IMU; -import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class ChassisTurnAbsolute extends ChassisTurn { - /** - * Constructor This command rotates the chassis to a position relative to the - * starting point of the robot (e.g. the position where the imu was last reset). - * - * @param chassis - * @param finalAngle - * @param imu - * @param motionController - */ - public ChassisTurnAbsolute(Chassis chassis, double finalAngle, IMU imu, MotionController motionController) { - super("Chassis Turn Absolute", chassis, finalAngle, imu, motionController); - } - - /** - * Constructor This command rotates the chassis to a position relative to the - * starting point of the robot (e.g. the position where the imu was last reset). - * - * @param chassis - * @param finalAngle - * @param imu - * @param fallbackCommand If the sensor fails for some reason, this command - * will be cancelled, then the fallbackCommand will - * start - * @param motionController - */ - public ChassisTurnAbsolute(Chassis chassis, double finalAngle, IMU imu, CommandBase fallbackCommand, - MotionController motionController) { - super("Chassis Turn Absolute", chassis, finalAngle, imu, fallbackCommand, motionController); - } - - /** - * Constructor This command rotates the chassis to a position relative to the - * starting point of the robot (e.g. the position where the imu was last reset). - * - * @param name - * @param chassis - * @param finalAngle - * @param imu - * @param motionController - */ - public ChassisTurnAbsolute(String name, Chassis chassis, double finalAngle, IMU imu, - MotionController motionController) { - super(name, chassis, finalAngle, imu, motionController); - } - - /** - * Constructor This command rotates the chassis to a position relative to the - * starting point of the robot (e.g. the position where the imu was last reset). - * - * @param name - * @param chassis - * @param finalAngle - * @param imu - * @param fallbackCommand If the sensor fails for some reason, this command - * will be cancelled, then the fallbackCommand will - * start - * @param motionController - */ - public ChassisTurnAbsolute(String name, Chassis chassis, double finalAngle, IMU imu, CommandBase fallbackCommand, - MotionController motionController) { - super(name, chassis, finalAngle, imu, fallbackCommand, motionController); - } - - @Override - public void initialize() { - // ChassisTurnDegrees measures an initial angle and compensates for it - // to make its turns relative; - super.initialize(); - // Not anymore - super.initialAngle = 0.0; - } -} diff --git a/commands/chassis/SimpleSplines.java b/commands/chassis/SimpleSplines.java deleted file mode 100644 index 40b59b5f..00000000 --- a/commands/chassis/SimpleSplines.java +++ /dev/null @@ -1,164 +0,0 @@ -// Based on software from FIRST and WPI - -package org.usfirst.frc4904.standard.commands.chassis; - -import java.util.List; - -import org.usfirst.frc4904.robot.RobotMap; -import org.usfirst.frc4904.standard.subsystems.chassis.SplinesDrive; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.RamseteController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.trajectory.Trajectory.State; -import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -public class SimpleSplines extends SequentialCommandGroup { - public SimpleSplines(SplinesDrive robotDrive, Pose2d init_pos, List 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 - ), - robotDrive.getDriveConstants().kDriveKinematics, - maxVoltage - ) - ) - ), - robotDrive::getPose, - new RamseteController(robotDrive.getAutoConstants().kRamseteB, robotDrive.getAutoConstants().kRamseteZeta), - new SimpleMotorFeedforward(robotDrive.getDriveConstants().ksVolts, - robotDrive.getDriveConstants().kvVoltSecondsPerMeter, - robotDrive.getDriveConstants().kaVoltSecondsSquaredPerMeter), - robotDrive.getDriveConstants().kDriveKinematics, - robotDrive::getWheelSpeeds, - new PIDController(robotDrive.getDriveConstants().kPDriveVel, 0, 0), - new PIDController(robotDrive.getDriveConstants().kPDriveVel, 0, 0), - // RamseteCommand passes volts to the callback - robotDrive::tankDriveVolts, - robotDrive - ), - nextCommand - ); - robotDrive.resetOdometry(init_pos); - - Trajectory trajectory = 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 - ) - ) - ); - // Log the trajectory https://docs.wpilib.org/en/stable/docs/software/dashboards/glass/field2d-widget.html - Field2d m_field = new Field2d(); - SmartDashboard.putData(m_field); - m_field.getObject("traj").setTrajectory(trajectory); - - // // also log them individually for comparison - // trajectory.getStates().forEach(mmm -> { - // SmartDashboard.putNumber("Intended Trajectory elapsed_time", mmm.timeSeconds); - // SmartDashboard.putNumber("Intended Trajectory velocity", mmm.velocityMetersPerSecond); - // SmartDashboard.putNumber("Intended Trajectory poseX", mmm.poseMeters.getX()); - // SmartDashboard.putNumber("Intended Trajectory poseY", mmm.poseMeters.getY()); - // }); - - // actually, resample the trajectory to 20ms intervals to match the timed logging - for (double elapsed=0; elapsed inter_points, Pose2d final_pos, double maxVoltage){ - this(robotDrive, init_pos, inter_points, final_pos, maxVoltage, new InstantCommand(() -> robotDrive.tankDriveVolts(0, 0))); - } - - public SimpleSplines(SplinesDrive robotDrive, Pose2d init_pos, List inter_points, Pose2d final_pos){ - this(robotDrive, init_pos, inter_points, final_pos, 10); - } - - public SimpleSplines(SplinesDrive robotDrive, Pose2d init_pos, Pose2d final_pos){ - this(robotDrive, init_pos, List.of(), final_pos); - } - - - /** - * Class to store autonomous constants used for Ramsete Pathing. - */ - public static class AutoConstants { - public double kMaxSpeedMetersPerSecond; - public double kMaxAccelerationMetersPerSecondSquared; - public double kRamseteB; - public double kRamseteZeta; - - public AutoConstants(double kMaxSpeedMetersPerSecond, double kMaxAccelerationMetersPerSecondSquared, double kRamseteB, double kRamseteZeta) { - this.kMaxSpeedMetersPerSecond = kMaxSpeedMetersPerSecond; - this.kMaxAccelerationMetersPerSecondSquared = kMaxAccelerationMetersPerSecondSquared; - this.kRamseteB = kRamseteB; - this.kRamseteZeta = kRamseteZeta; - } - } - - /** - * Class to store drive constants used for Ramsete pathing. - */ - public static class DriveConstants { - public double ksVolts; - public double kvVoltSecondsPerMeter; - public double kaVoltSecondsSquaredPerMeter; - public double kTrackwidthMeters; - public DifferentialDriveKinematics kDriveKinematics; - public double kPDriveVel; - - public DriveConstants(double ksVolts, double kvVoltSecondsPerMeter, double kaVoltSecondsSquaredPerMeter, double kTrackwidthMeters, double kPDriveVel){ - this.ksVolts = ksVolts; - this.kvVoltSecondsPerMeter = kvVoltSecondsPerMeter; - this.kaVoltSecondsSquaredPerMeter = kaVoltSecondsSquaredPerMeter; - this.kTrackwidthMeters = kTrackwidthMeters; - this.kDriveKinematics = new DifferentialDriveKinematics(kTrackwidthMeters); - this.kPDriveVel = kPDriveVel; - } - } -} diff --git a/subsystems/chassis/Chassis.java b/subsystems/chassis/Chassis.java deleted file mode 100644 index 0b55586c..00000000 --- a/subsystems/chassis/Chassis.java +++ /dev/null @@ -1,81 +0,0 @@ -package org.usfirst.frc4904.standard.subsystems.chassis; - -import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -/** - * Generic Chassis class. In the 4904 standard, the Chassis is treated as a - * container for motors and as a calculator for motor speeds. setDefaultCommand - * should be called from RobotMap. The default command is a ChassisIdle. - */ - -public abstract class Chassis extends SubsystemBase { - protected double[] motorSpeeds; - protected MotorSubsystem[] motors; - - /** - * - * @param name - * @param motors all the motors that are part of this Chassis. Pass from front - * to back, left to right - */ - public Chassis(String name, MotorSubsystem... motors) { - this.motors = motors; - motorSpeeds = new double[motors.length]; - setName(name); - } - - /** - * - * @param motors all the motors that are part of this Chassis. Pass from front - * to back, left to right - */ - public Chassis(MotorSubsystem... motors) { - this("Chassis", motors); - } - - /** - * Returns an array of motors in the order that they were passed to the - * constructor - * - * @return all motors in the order passed to the constructor - */ - public MotorSubsystem[] getMotors() { - return motors; - } - - /** - * Returns an array of the correct motor speeds calculated with the values - * inputted using the move functions. - * - * @return current motor speeds - */ - public double[] getMotorSpeeds() { - return motorSpeeds; - } - - /** - * Sets the movement to be calculated by the Chassis using polar coordinates. - * - * @param speed The magnitude of the speed. In the range -1 to 1. - * @param angle The direction of the speed in angles clockwise from straight - * ahead. In the range 0 to 2Pi. - * @param turnSpeed The speed at which the robot will revolve around itself - * during the maneuver. In the range -1 to 1. - */ - public abstract void movePolar(double speed, double angle, double turnSpeed); - - /** - * Sets the movement to be calculated by the Chassis using cartesian - * coordinates. - * - * @param xSpeed The speed in the X direction (side to side, strafe). In the - * range -1 to 1. - * @param ySpeed The speed in the Y direction (forward and back). In the - * range -1 to 1. - * @param turnSpeed The speed at which the robot will revolve around itself - * during the maneuver. In the range -1 to 1. - */ - public abstract void moveCartesian(double xSpeed, double ySpeed, double turnSpeed); -} diff --git a/subsystems/chassis/MecanumChassis.java b/subsystems/chassis/MecanumChassis.java deleted file mode 100644 index 7794a7fa..00000000 --- a/subsystems/chassis/MecanumChassis.java +++ /dev/null @@ -1,111 +0,0 @@ -package org.usfirst.frc4904.standard.subsystems.chassis; - -import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; - -/** - * Mecanum Chassis. Has one wheel for each corner. Can move in all directions. - * - */ -public class MecanumChassis extends Chassis { - /** - * Constructs a mecanum chassis - * - * @param name - * @param frontLeftWheel - * @param frontRightWheel - * @param backLeftWheel - * @param backRightWheel - */ - public MecanumChassis(String name, MotorSubsystem frontLeftWheel, MotorSubsystem frontRightWheel, MotorSubsystem backLeftWheel, - MotorSubsystem backRightWheel) { - super(name, frontLeftWheel, frontRightWheel, backLeftWheel, backRightWheel); - } - - /** - * Constructs a mecanum chassis - * - * @param frontLeftWheel - * @param frontRightWheel - * @param backLeftWheel - * @param backRightWheel - */ - public MecanumChassis(MotorSubsystem frontLeftWheel, MotorSubsystem frontRightWheel, MotorSubsystem backLeftWheel, MotorSubsystem backRightWheel) { - this("Mecanum Chassis", frontLeftWheel, frontRightWheel, backLeftWheel, backRightWheel); - } - - /** - * Sets the movement to be calculated by the MecanumChassis using 2d polar - * coordinates. - * - * @param speed The magnitude of the speed. In the range -1 to 1. - * @param angle The direction of the speed in angles clockwise from straight - * ahead. In the range 0 to 2Pi. - * @param turnSpeed The speed at which the robot will revolve around itself - * during the maneuver. In the range -1 to 1. - */ - @Override - public void movePolar(double speed, double angle, double turnSpeed) { - motorSpeeds = MecanumChassis.calculateWheels(speed, angle, turnSpeed); - } - - /** - * Sets the movement to be calculated by the MecanumChassis using 2d cartesian - * coordinates. - * - * @param xSpeed The speed in the X direction (side to side, strafe). In the - * range -1 to 1. - * @param ySpeed The speed in the Y direction (forward and back). In the - * range -1 to 1. - * @param turnSpeed The speed at which the robot will revolve around itself - * during the maneuver. In the range -1 to 1. - */ - @Override - public void moveCartesian(double xSpeed, double ySpeed, double turnSpeed) { - double[] polar = MecanumChassis.cartesianToPolar(xSpeed, ySpeed); - movePolar(polar[0], polar[1], turnSpeed); - } - - /** - * Calculates the speeds for each motor given polar coordinates. - * - * @param speed The magnitude of the speed. In the range -1 to 1. - * @param angle The direction of the speed in angles clockwise from straight - * ahead. In the range 0 to 2Pi. - * @param turnSpeed The speed at which the robot will revolve around itself - * during the maneuver. In the range -1 to 1. - * - * @return An array {frontLeftSpeed, frontRightSpeed, backLeftSpeed, - * backRightSpeed} - */ - public static double[] calculateWheels(double speed, double angle, double turnSpeed) { - angle -= Math.PI / 4.0; // Shift axes to work with mecanum - angle = angle % (Math.PI * 2); // make sure angle makes sense - double frontLeft = speed * Math.sin(angle) + turnSpeed; - double frontRight = -1 * speed * Math.cos(angle) + turnSpeed; - double backLeft = speed * Math.cos(angle) + turnSpeed; - double backRight = -1 * speed * Math.sin(angle) + turnSpeed; - double scaleFactor = Math.max(Math.max(Math.max(Math.abs(frontLeft), Math.abs(frontRight)), Math.abs(backLeft)), - Math.abs(backRight)); - if (scaleFactor < 1) { - scaleFactor = 1; - } - frontLeft /= scaleFactor; - frontRight /= scaleFactor; - backLeft /= scaleFactor; - backRight /= scaleFactor; - return new double[] { frontLeft, frontRight, backLeft, backRight }; - } - - /** - * Converts an x and y coordinate into an array of speed and angle. - * - * @param x The x coordinate - * @param y The y coordinate - * @return An array {speed, angle} - */ - public static double[] cartesianToPolar(double x, double y) { - double speed = Math.sqrt(x * x + y * y); - double angle = Math.atan2(y, x); - return new double[] { speed, angle }; - } -} diff --git a/subsystems/chassis/SensorDrive.java b/subsystems/chassis/SensorDrive.java deleted file mode 100644 index 4c35e6d5..00000000 --- a/subsystems/chassis/SensorDrive.java +++ /dev/null @@ -1,147 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.usfirst.frc4904.standard.subsystems.chassis; - - -import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.custom.CustomPIDSourceType; -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.custom.sensors.PIDSensor; -import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; -import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder; -import org.usfirst.frc4904.standard.custom.sensors.NavX; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; -import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Subsystem; - -public class SensorDrive implements Subsystem, PIDSensor { // Based largely on - // https://github.com/wpilibsuite/allwpilib/blob/master/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java - private final TankDrive driveBase; - private final CANTalonEncoder leftEncoder; - private final CANTalonEncoder rightEncoder; - private final NavX gyro; - private final DifferentialDriveOdometry odometry; - private CustomPIDSourceType sensorType; - - /** - * Creates a new DriveSubsystem. - */ - public SensorDrive(TankDrive driveBase, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, NavX gyro, - CustomPIDSourceType sensorType, Pose2d initialPose) { - this.driveBase = driveBase; - this.leftEncoder = leftEncoder; - this.rightEncoder = rightEncoder; - this.gyro = gyro; - setCustomPIDSourceType(sensorType); - - resetEncoders(); - odometry = new DifferentialDriveOdometry(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), initialPose); - CommandScheduler.getInstance().registerSubsystem(this); - } - - public SensorDrive(TankDrive driveBase, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, NavX gyro, Pose2d initialPose) { - this(driveBase, leftEncoder, rightEncoder, gyro, CustomPIDSourceType.kDisplacement, initialPose); - } - - @Override - public void periodic() { - odometry.update(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance()); - } - - @Override - public void setCustomPIDSourceType(CustomPIDSourceType sensorType) { - this.sensorType = sensorType; - } - - @Override - public CustomPIDSourceType getCustomPIDSourceType() { - return sensorType; - } - - @Override - public double pidGetSafely() throws InvalidSensorException { - return getPose().getRotation().getDegrees(); - } - - @Override - public double pidGet() { - try { - return pidGetSafely(); - } catch (Exception e) { - LogKitten.ex(e); - return 0; - } - } - - /** - * Returns the currently-estimated pose of the robot. - * - * @return The pose. - */ - public Pose2d getPose() { - return odometry.getPoseMeters(); - } - - /** - * Returns the current wheel speeds of the robot. - * - * @return The current wheel speeds. - */ - public DifferentialDriveWheelSpeeds getWheelSpeeds() { - return new DifferentialDriveWheelSpeeds(leftEncoder.getRate(), rightEncoder.getRate()); - } - - /** - * Resets the odometry to the specified pose. - * - * @param pose The pose to which to set the odometry. - */ - public void resetOdometry(Pose2d pose) { - resetEncoders(); - odometry.resetPosition(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), pose); - } - - /** - * Controls the left and right sides of the drive directly with voltages. - * - * @param leftVolts the commanded left output - * @param rightVolts the commanded right output - */ - public void tankDriveVolts(double leftVolts, double rightVolts) { - MotorSubsystem[] motors = driveBase.getMotors(); - if (motors.length == 2) { - driveBase.getMotors()[0].setVoltage(leftVolts); - driveBase.getMotors()[1].setVoltage(rightVolts); - } else { - driveBase.getMotors()[0].setVoltage(leftVolts); - driveBase.getMotors()[1].setVoltage(leftVolts); - driveBase.getMotors()[2].setVoltage(rightVolts); - driveBase.getMotors()[3].setVoltage(rightVolts); - } - } - - /** - * Resets the drive encoders to currently read a position of 0. - */ - public void resetEncoders() { - leftEncoder.reset(); - rightEncoder.reset(); - } - - /** - * Returns the heading of the robot. - * - * @return the robot's heading in degrees, from 180 to 180 - */ - public double getHeading() { - return gyro.getYaw() * -1; - } -} diff --git a/subsystems/chassis/ShiftingChassis.java b/subsystems/chassis/ShiftingChassis.java deleted file mode 100644 index 980a03fc..00000000 --- a/subsystems/chassis/ShiftingChassis.java +++ /dev/null @@ -1,12 +0,0 @@ -package org.usfirst.frc4904.standard.subsystems.chassis; - -/** - * An interface that can be attached to a chassis to allow it to return a pair - * of shifters to shift the chassis into a high gear and low gear configuration. - */ -public interface ShiftingChassis { - /** - * @return an array of Solenoids (should return from left to right) - */ - public SolenoidShifters getShifter(); -} diff --git a/subsystems/chassis/SolenoidShifters.java b/subsystems/chassis/SolenoidShifters.java deleted file mode 100644 index ca479446..00000000 --- a/subsystems/chassis/SolenoidShifters.java +++ /dev/null @@ -1,210 +0,0 @@ -package org.usfirst.frc4904.standard.subsystems.chassis; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -/** - * A class that wraps multiple DoubleSolenoid objects with subsystem - * functionality. Allows for easy inversion and setting of default state of - * solenoids - */ -public class SolenoidShifters extends SubsystemBase { // TODO: make solenoidshifters extend solenoidsubsystem - protected DoubleSolenoid[] solenoids; - protected SolenoidState state; - protected SolenoidState defaultState; - protected boolean isInverted; - - /** - * A class that wraps multiple DoubleSolenoid objects with subsystem - * functionality. Allows for easy inversion and setting of default state of - * solenoids - * - * @param name Name of subsystem - * @param isInverted True if the solenoids should be inverted - * @param defaultState Set the default state of the SolenoidSystem - * @param solenoids Double solenoids of the system - */ - public SolenoidShifters(String name, boolean isInverted, SolenoidState defaultState, DoubleSolenoid... solenoids) { - setName(name); - this.solenoids = solenoids; - this.isInverted = isInverted; - this.defaultState = defaultState; - this.state = defaultState; - } - - /** - * A class that wraps multiple DoubleSolenoid objects with subsystem - * functionality. Allows for easy inversion and setting of default state of - * solenoidss - * - * @param name Name of subsystem - * @param isInverted True if the solenoids should be inverted - * @param solenoids Double solenoids of the system - */ - public SolenoidShifters(String name, boolean isInverted, DoubleSolenoid... solenoids) { - this(name, isInverted, SolenoidState.RETRACT, solenoids); - } - - /** - * A class that wraps multiple DoubleSolenoid objects with subsystem - * functionality. Allows for easy inversion and setting of default state of - * solenoids - * - * @param name Name of subsystem - * @param defaultState Set the default state of the SolenoidSystem - * @param solenoids Double solenoids of the system - */ - public SolenoidShifters(String name, SolenoidState defaultState, DoubleSolenoid... solenoids) { - this(name, false, defaultState, solenoids); - } - - /** - * A class that wraps multiple DoubleSolenoid objects with subsystem - * functionality. Allows for easy inversion and setting of default state of - * solenoids - * - * @param name Name of subsystem - * @param solenoids Double solenoids of the system - */ - public SolenoidShifters(String name, DoubleSolenoid... solenoids) { - this(name, false, solenoids); - } - - /** - * A class that wraps multiple DoubleSolenoid objects with subsystem - * functionality. Allows for easy inversion and setting of default state of - * solenoids - * - * @param defaultState Set the default state of the SolenoidSystem - * @param solenoids Double solenoids of the system - */ - public SolenoidShifters(SolenoidState defaultState, DoubleSolenoid... solenoids) { - this("SolenoidShifters", defaultState, solenoids); - } - - /** - * A class that wraps multiple DoubleSolenoid objects with subsystem - * functionality. Allows for easy inversion and setting of default state of - * solenoids - * - * @param isInverted True if the solenoids should be inverted - * @param solenoids Double solenoids of the system - */ - public SolenoidShifters(boolean isInverted, DoubleSolenoid... solenoids) { - this("SolenoidShifters", isInverted, solenoids); - } - - /** - * A class that wraps multiple DoubleSolenoid objects with subsystem - * functionality. Allows for easy inversion and setting of default state of - * solenoids - * - * @param solenoids Double solenoids of the system - */ - public SolenoidShifters(DoubleSolenoid... solenoids) { - this("SolenoidShifters", solenoids); - } - - /** - * DoubleSolenoid.Value simplified to three simple states - */ - public enum SolenoidState { - EXTEND(DoubleSolenoid.Value.kForward), RETRACT(DoubleSolenoid.Value.kReverse); - - public final DoubleSolenoid.Value value; - - private SolenoidState(DoubleSolenoid.Value value) { - this.value = value; - } - } - - /** - * @param state Returns the current state of the system - */ - public SolenoidState getState() { - return state; - } - - /** - * Inverts the state given - * - * @param state SolenoidState to be inverted - * @return SolenoidState Inverted state - * - */ - public SolenoidState invertState(SolenoidState state) { - switch (state) { - case EXTEND: - return SolenoidState.RETRACT; - case RETRACT: - return SolenoidState.EXTEND; - } - return state; - } - - /** - * Sets the state of the system Only sets if current state is not equal to state - * to be set - * - * @param state State to set system - */ - public void set(SolenoidState state) { - if (isInverted) { - state = invertState(state); - } - this.state = state; - if (this.state != state) { - for (DoubleSolenoid solenoid : solenoids) { - solenoid.set(state.value); - } - } - } - - /** - * Sets the state of the system Only sets if current state is not equal to state - * to be set - * - * This implementation takes no parameters, as it just reverses the current - * state TODO: this may need to be changed - * - * @throws Exception if the solenoid state currently is off - */ - public void set() { // TODO: consider OFF case - if (state == SolenoidState.RETRACT) { - set(SolenoidState.EXTEND); - } else if (state == SolenoidState.EXTEND) { - set(SolenoidState.RETRACT); - } - } - - /** - * Sets the state of the system regardless of current state - * - * @param state State to set - */ - public void setOverride(SolenoidState state) { - if (isInverted) { - state = invertState(state); - } - this.state = state; - for (DoubleSolenoid solenoid : solenoids) { - solenoid.set(state.value); - } - } - - /** - * @return solenoids DoubleSolenoid objects of the system - */ - public DoubleSolenoid[] getSolenoids() { - return solenoids; - } - - /** - * Returns whether the solenoid is extended. - * - * @return extended - */ - public boolean isExtended() { - return solenoids[0].get() == SolenoidState.EXTEND.value; - } -} diff --git a/subsystems/chassis/SplinesDrive.java b/subsystems/chassis/SplinesDrive.java deleted file mode 100644 index 35ff1fa8..00000000 --- a/subsystems/chassis/SplinesDrive.java +++ /dev/null @@ -1,38 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package org.usfirst.frc4904.standard.subsystems.chassis; - - -import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder; - -import org.usfirst.frc4904.standard.commands.chassis.SimpleSplines; -import org.usfirst.frc4904.standard.custom.sensors.NavX; - -import edu.wpi.first.math.geometry.Pose2d; - -public class SplinesDrive extends SensorDrive { - private final SimpleSplines.AutoConstants autoConstants; - private final SimpleSplines.DriveConstants driveConstants; - - /** - * Creates a new DriveSubsystem. - */ - public SplinesDrive(TankDrive driveBase, SimpleSplines.AutoConstants autoConstants, SimpleSplines.DriveConstants driveConstants, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, NavX gyro, Pose2d initialPose) { - super(driveBase,leftEncoder, rightEncoder, gyro, initialPose); - this.autoConstants = autoConstants; - this.driveConstants = driveConstants; - } - - public SimpleSplines.AutoConstants getAutoConstants(){ - return autoConstants; - } - - public SimpleSplines.DriveConstants getDriveConstants(){ - return driveConstants; - } -} diff --git a/subsystems/chassis/SwerveChassis.java b/subsystems/chassis/SwerveChassis.java deleted file mode 100644 index e369855e..00000000 --- a/subsystems/chassis/SwerveChassis.java +++ /dev/null @@ -1,50 +0,0 @@ -package org.usfirst.frc4904.standard.subsystems.chassis; - -import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; - -public class SwerveChassis extends Chassis { - public final MotorSubsystem frontLeftWheelSwerve; - public final MotorSubsystem frontRightWheelSwerve; - public final MotorSubsystem backLeftWheelSwerve; - public final MotorSubsystem backRightWheelSwerve; - - /** - * Constructs a swerve drive chassis - * - * @param name - * @param frontLeftWheel - * @param frontRightWheel - * @param backLeftWheel - * @param backRightWheel - * @param frontLeftWheelSwerve - * @param frontRightWheelSwerve - * @param backLeftWheelSwerve - * @param backRightWheelSwerve - */ - public SwerveChassis(String name, MotorSubsystem frontLeftWheel, MotorSubsystem frontRightWheel, MotorSubsystem backLeftWheel, - MotorSubsystem backRightWheel, MotorSubsystem frontLeftWheelSwerve, MotorSubsystem frontRightWheelSwerve, MotorSubsystem backLeftWheelSwerve, - MotorSubsystem backRightWheelSwerve) { - super(frontLeftWheel, frontRightWheel, backLeftWheel, backRightWheel); - this.frontLeftWheelSwerve = frontLeftWheelSwerve; - this.frontRightWheelSwerve = frontRightWheelSwerve; - this.backLeftWheelSwerve = backLeftWheelSwerve; - this.backRightWheelSwerve = backRightWheelSwerve; - } - - public SwerveChassis(MotorSubsystem frontLeftWheel, MotorSubsystem frontRightWheel, MotorSubsystem backLeftWheel, MotorSubsystem backRightWheel, - MotorSubsystem frontLeftWheelSwerve, MotorSubsystem frontRightWheelSwerve, MotorSubsystem backLeftWheelSwerve, - MotorSubsystem backRightWheelSwerve) { - this("SwerveChassis", frontLeftWheel, frontRightWheel, backLeftWheel, backRightWheel, frontLeftWheelSwerve, - frontRightWheelSwerve, backLeftWheelSwerve, backRightWheelSwerve); - } - - @Override - public void movePolar(double xSpeed, double ySpeed, double turnSpeed) { - // TODO Implement - } - - @Override - public void moveCartesian(double speed, double angle, double turnSpeed) { - // TODO Implement - } -} diff --git a/subsystems/chassis/TankDrive.java b/subsystems/chassis/TankDrive.java deleted file mode 100644 index 304aca7e..00000000 --- a/subsystems/chassis/TankDrive.java +++ /dev/null @@ -1,130 +0,0 @@ -// THIS FILE IS TESTED post wpilibj2 - -package org.usfirst.frc4904.standard.subsystems.chassis; - -import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; - -/** - * Tank drive chassis. Has two sets of wheels, left and right. Can only turn - * left or right. - */ -public class TankDrive extends Chassis { - public double turnCorrection; // 0 by default but can be manually overwritten. Positive values induce a turn - // to the right - - /** - * - * @param name - * @param leftWheelA - * @param leftWheelB - * @param rightWheelA - * @param rightWheelB - * @param turnCorrection Amount by which to correct turning to make up for dead - * CIM or unbalanced weight. In the range -1 to 1. - */ - public TankDrive(String name, Double turnCorrection, MotorSubsystem leftWheelA, MotorSubsystem leftWheelB, MotorSubsystem rightWheelA, - MotorSubsystem rightWheelB) { - super(name, leftWheelA, leftWheelB, rightWheelA, rightWheelB); - this.turnCorrection = turnCorrection; - } - - /** - * - * @param leftWheelA - * @param leftWheelB - * @param rightWheelA - * @param rightWheelB - * @param turnCorrection Amount by which to correct turning to make up for dead - * CIM or unbalanced weight. In the range -1 to 1. - */ - public TankDrive(Double turnCorrection, MotorSubsystem leftWheelA, MotorSubsystem leftWheelB, MotorSubsystem rightWheelA, MotorSubsystem rightWheelB) { - this("TankDrive", turnCorrection, leftWheelA, leftWheelB, rightWheelA, rightWheelB); - } - - /** - * @param name - * @param leftWheelA - * @param leftWheelB - * @param rightWheelA - * @param rightWheelB - */ - public TankDrive(String name, Double turnCorrection, MotorSubsystem leftWheel, MotorSubsystem rightWheel) { - super(name, leftWheel, rightWheel); - this.turnCorrection = turnCorrection; - } - - /** - * - * @param leftWheelA - * @param leftWheelB - * @param rightWheelA - * @param rightWheelB - */ - public TankDrive(Double turnCorrection, MotorSubsystem leftWheel, MotorSubsystem rightWheel) { - this("TankDrive", turnCorrection, leftWheel, rightWheel); - } - - /** - * - * @param name - * @param leftWheelA - * @param leftWheelB - * @param rightWheelA - * @param rightWheelB - */ - public TankDrive(String name, MotorSubsystem leftWheelA, MotorSubsystem leftWheelB, MotorSubsystem rightWheelA, MotorSubsystem rightWheelB) { - super(name, leftWheelA, leftWheelB, rightWheelA, rightWheelB); - this.turnCorrection = 0.0; - } - - /** - * - * @param leftWheelA - * @param leftWheelB - * @param rightWheelA - * @param rightWheelB - */ - public TankDrive(MotorSubsystem leftWheelA, MotorSubsystem leftWheelB, MotorSubsystem rightWheelA, MotorSubsystem rightWheelB) { - this("TankDrive", leftWheelA, leftWheelB, rightWheelA, rightWheelB); - } - - /** - * - * @param name - * @param leftWheel - * @param rightWheel - */ - public TankDrive(String name, MotorSubsystem leftWheel, MotorSubsystem rightWheel) { - super(name, leftWheel, rightWheel); - this.turnCorrection = 0.0; - } - - /** - * - * @param leftWheel - * @param rightWheel - */ - public TankDrive(MotorSubsystem leftWheel, MotorSubsystem rightWheel) { - this("TankDrive", leftWheel, rightWheel); - } - - @Override - public void moveCartesian(double xSpeed, double ySpeed, double turnSpeed) { - movePolar(ySpeed, 0.0, turnSpeed); - } - - @Override - public void movePolar(double speed, double angle, double turnSpeed) { - turnSpeed += turnCorrection * speed; // turns to deal with constant turning error due to unbalanced weight or - // dead CIM - double normalize = Math.max(Math.max(Math.abs(speed + turnSpeed), Math.abs(speed - turnSpeed)), 1); - double leftSpeed = (speed + turnSpeed) / normalize; - double rightSpeed = (speed - turnSpeed) / normalize; - if (motors.length == 2) { - motorSpeeds = new double[] { leftSpeed, rightSpeed }; - } else { - motorSpeeds = new double[] { leftSpeed, leftSpeed, rightSpeed, rightSpeed }; - } - } - -} diff --git a/subsystems/chassis/TankDriveShifting.java b/subsystems/chassis/TankDriveShifting.java deleted file mode 100644 index 59632cfc..00000000 --- a/subsystems/chassis/TankDriveShifting.java +++ /dev/null @@ -1,141 +0,0 @@ -package org.usfirst.frc4904.standard.subsystems.chassis; - -import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; - -/** - * A tank drive chassis with the shifting system. This effectively adds a - * solenoid shifter to the chassis as a contained object. - * - */ -public class TankDriveShifting extends TankDrive implements ShiftingChassis { - protected final SolenoidShifters shifter; - - /** - * A tank drive with shifting solenoids (only two solenoids supported) - * - * @param name - * @param turnCorrection - * @param leftWheelA - * @param leftWheelB - * @param rightWheelA - * @param rightWheelB - * @param shifter - */ - - public TankDriveShifting(String name, Double turnCorrection, MotorSubsystem leftWheelA, MotorSubsystem leftWheelB, MotorSubsystem rightWheelA, - MotorSubsystem rightWheelB, SolenoidShifters shifter) { - super(name, turnCorrection, leftWheelA, leftWheelB, rightWheelA, rightWheelB); - this.shifter = shifter; - } - - /** - * A tank drive with shifting solenoids (only two solenoids supported) - * - * @param turnCorrection - * @param leftWheelA - * @param leftWheelB - * @param rightWheelA - * @param rightWheelB - * @param shifter - */ - - public TankDriveShifting(Double turnCorrection, MotorSubsystem leftWheelA, MotorSubsystem leftWheelB, MotorSubsystem rightWheelA, - MotorSubsystem rightWheelB, SolenoidShifters shifter) { - this("Tank Drive Shifting", turnCorrection, leftWheelA, leftWheelB, rightWheelA, rightWheelB, shifter); - } - - /** - * A tank drive with shifting solenoids (only two solenoids supported) - * - * @param name - * @param turnCorrection - * @param leftWheelA - * @param leftWheelB - * @param rightWheelA - * @param rightWheelB - * @param shifter - */ - - public TankDriveShifting(String name, Double turnCorrection, MotorSubsystem leftWheel, MotorSubsystem rightWheel, - SolenoidShifters shifter) { - super(name, turnCorrection, leftWheel, rightWheel); - this.shifter = shifter; - } - - /** - * A tank drive with shifting solenoids (only two solenoids supported) - * - * @param turnCorrection - * @param leftWheelA - * @param leftWheelB - * @param rightWheelA - * @param rightWheelB - * @param shifter - */ - - public TankDriveShifting(Double turnCorrection, MotorSubsystem leftWheel, MotorSubsystem rightWheel, SolenoidShifters shifter) { - this("Tank Drive Shifting", turnCorrection, leftWheel, rightWheel, shifter); - } - - /** - * A tank drive with shifting solenoids (only two solenoids supported) - * - * @param name - * @param leftWheelA - * @param leftWheelB - * @param rightWheelA - * @param rightWheelB - * @param shifter - */ - public TankDriveShifting(String name, MotorSubsystem leftWheelA, MotorSubsystem leftWheelB, MotorSubsystem rightWheelA, MotorSubsystem rightWheelB, - SolenoidShifters shifter) { - super(name, leftWheelA, leftWheelB, rightWheelA, rightWheelB); - this.shifter = shifter; - } - - /** - * A tank drive with shifting solenoids (only two solenoids supported) - * - * @param leftWheelA - * @param leftWheelB - * @param rightWheelA - * @param rightWheelB - * @param shifter - */ - public TankDriveShifting(MotorSubsystem leftWheelA, MotorSubsystem leftWheelB, MotorSubsystem rightWheelA, MotorSubsystem rightWheelB, - SolenoidShifters shifter) { - this("Tank Drive Shifting", leftWheelA, leftWheelB, rightWheelA, rightWheelB, shifter); - } - - /** - * A tank drive with shifting solenoids (only two solenoids supported) - * - * @param name - * @param leftWheel - * @param rightWheel - * @param shifter - */ - public TankDriveShifting(String name, MotorSubsystem leftWheel, MotorSubsystem rightWheel, SolenoidShifters shifter) { - super(name, leftWheel, rightWheel); - this.shifter = shifter; - } - - /** - * A tank drive with shifting solenoids (only two solenoids supported) - * - * @param leftWheel - * @param rightWheel - * @param shifter - */ - public TankDriveShifting(MotorSubsystem leftWheel, MotorSubsystem rightWheel, SolenoidShifters shifter) { - this("Tank Drive Shifting", leftWheel, rightWheel, shifter); - } - - /** - * Returns an array of solenoids in the order left, right - */ - @Override - public SolenoidShifters getShifter() { - return shifter; - } -} From 9c93b1eaf75be078384c73dfb22c4ca5cc07f21f Mon Sep 17 00:00:00 2001 From: Exr0n Date: Tue, 14 Feb 2023 18:31:08 -0800 Subject: [PATCH 030/134] solidify setBrakeOnNeutral naming; brakable motor subsys default cmd --- subsystems/motor/BrakeableMotorController.java | 14 ++------------ subsystems/motor/BrakeableMotorSubsystem.java | 17 +++++++---------- 2 files changed, 9 insertions(+), 22 deletions(-) diff --git a/subsystems/motor/BrakeableMotorController.java b/subsystems/motor/BrakeableMotorController.java index dcc69c5d..e6cf552b 100644 --- a/subsystems/motor/BrakeableMotorController.java +++ b/subsystems/motor/BrakeableMotorController.java @@ -4,17 +4,7 @@ package org.usfirst.frc4904.standard.subsystems.motor; public interface BrakeableMotorController extends MotorController { - // TODO: should we have these? brake() = setBrakeOnNeutral(); neutral() - // public abstract void setBrakeOnNeutral(); - // public abstract void setCoastOnNeutral(); - // public abstract void setDisableOnNeutral(); - // public abstract void brake(); - // public abstract void coast(); - // public abstract void neutral(); - - public abstract void setBrakeMode(); - - public abstract void setCoastMode(); - + public abstract BrakeableMotorController setBrakeOnNeutral(); + public abstract BrakeableMotorController setCoastOnNeutral(); public abstract void neutralOutput(); } diff --git a/subsystems/motor/BrakeableMotorSubsystem.java b/subsystems/motor/BrakeableMotorSubsystem.java index 8d115278..dd7be35f 100644 --- a/subsystems/motor/BrakeableMotorSubsystem.java +++ b/subsystems/motor/BrakeableMotorSubsystem.java @@ -4,6 +4,7 @@ import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; public class BrakeableMotorSubsystem extends MotorSubsystem { // TODO: should this take general MotorControllers and just brake the ones that implement BrakeableMotorController @@ -25,8 +26,8 @@ public class BrakeableMotorSubsystem extends MotorSubsystem { public BrakeableMotorSubsystem(String name, SpeedModifier speedModifier, BrakeableMotorController... motors) { super(name, speedModifier, motors); this.motors = motors; - for (var motor : this.motors) motor.setBrakeMode(); - } + setDefaultCommand(Commands.runOnce(() -> this.neutralOutput())); + } /** * A class that wraps around a variable number of BrakeableMotorController @@ -74,17 +75,13 @@ public BrakeableMotorSubsystem(BrakeableMotorController... motors) { /** * Sets the neutral output mode to brake. Motor will continue to run, but * future calls to neutralOutput() will cause motor to brake. - * - * Uses the underlying .neutralOutput() method. */ - public void setBrakeModeOnNeutral() { for (var motor : motors) motor.setBrakeMode(); } + public void setBrakeOnNeutral() { for (var motor : motors) motor.setBrakeOnNeutral(); } /** * Sets the neutral output mode to coast. Motor will continue to run, but * future calls to neutralOutput() will cause motor to coast. - * - * Uses the underlying .neutralOutput() method. */ - public void setCoastModeOnNeutral() { for (var motor : motors) motor.setCoastMode(); } + public void setCoastOnNeutral() { for (var motor : motors) motor.setCoastOnNeutral(); } /** * Sets the output mode to neutral, which is either break or coast (default @@ -99,7 +96,7 @@ public BrakeableMotorSubsystem(BrakeableMotorController... motors) { * Enables brake mode on and brakes each motor. */ public void brake() { - setBrakeModeOnNeutral(); + setBrakeOnNeutral(); neutralOutput(); } @@ -107,7 +104,7 @@ public void brake() { * Enables coast mode on and coasts each motor. */ public void coast() { - setCoastModeOnNeutral(); + setCoastOnNeutral(); neutralOutput(); } From 7c0e3bdce338b0d20f898fdaa974518001ff1048 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Tue, 14 Feb 2023 18:35:01 -0800 Subject: [PATCH 031/134] delete motorController .get; TalonMotorController --- custom/motioncontrollers/CANTalonFX.java | 86 ++++++++++++++++---- custom/motioncontrollers/CANTalonSRX.java | 91 +++++++++++++++++++--- subsystems/motor/MotorSubsystem.java | 9 ++- subsystems/motor/TalonMotorController.java | 24 ++++++ subsystems/motor/TalonMotorSubsystem.java | 74 ++++++++++++++++++ 5 files changed, 254 insertions(+), 30 deletions(-) create mode 100644 subsystems/motor/TalonMotorController.java create mode 100644 subsystems/motor/TalonMotorSubsystem.java diff --git a/custom/motioncontrollers/CANTalonFX.java b/custom/motioncontrollers/CANTalonFX.java index c86e8cfa..38dbc621 100644 --- a/custom/motioncontrollers/CANTalonFX.java +++ b/custom/motioncontrollers/CANTalonFX.java @@ -4,34 +4,88 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import org.usfirst.frc4904.standard.subsystems.motor.BrakeableMotorController; +import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorController; -public class CANTalonFX extends WPI_TalonFX implements BrakeableMotorController { - protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; - protected static final InvertType DEFAULT_INVERT_TYPE = InvertType.FollowMaster; - - // TODO: implement setVoltage with native APIs +public class CANTalonFX extends WPI_TalonFX implements TalonMotorController { + protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; + protected static final InvertType DEFAULT_INVERT_TYPE = InvertType.FollowMaster; + protected static final double DEFAULT_NEUTRAL_DEADBAND = 0.001; // 0.1%, the minimum possible value /** - * Represents a Falcon motor in code. You probably want NeutralMode.Brake, InvertType.FollowMaster - * TODO document better - * - * @param deviceNumber - * @param neutralMode - * @param inverted + * Represents a Falcon motor in code. You probably want NeutralMode.Brake, + * InvertType.FollowMaster. + * + * @param deviceNumber Usually the CAN ID of the device, + * declared in RobotMap + * @param neutralMode Whether the motor should brake or coast + * when the the output is near zero, or + * .disable() or .stopMotor() are called. + * @param inverted InvertMode of the motor. If this will be + * part of a motor group, consider using + * FolloMaster or OpposeMaster, so that you + * can invert the entire motor group by + * inverting the lead motor. Use None or + * InvertMotorOutput for the lead motor. + * @param neutralDeadbandPercent Percent output range around zero to + * enable neutralOutput (brake/coast mode) + * instead. For more info, see + * https://v5.docs.ctr-electronics.com/en/latest/ch13_MC.html#neutral-deadband */ - public CANTalonFX(int deviceNumber, NeutralMode neutralMode, InvertType inverted) { + public CANTalonFX(int deviceNumber, NeutralMode neutralMode, InvertType inverted, double neutralDeadbandPercent) { super(deviceNumber); configFactoryDefault(); // use default settings to prevent unexpected behavior, reccommended in examples + configNeutralDeadband(neutralDeadbandPercent); setNeutralMode(neutralMode); setInverted(inverted); } + /** + * Represents a Falcon motor in code. You probably want NeutralMode.Brake, + * InvertType.FollowMaster. + * + * @param deviceNumber Usually the CAN ID of the device, + * declared in RobotMap + * @param neutralMode Whether the motor should brake or coast + * when the the output is near zero, or + * .disable() or .stopMotor() are called. + * @param inverted InvertMode of the motor. If this will be + * part of a motor group, consider using + * FolloMaster or OpposeMaster, so that you + * can invert the entire motor group by + * inverting the lead motor. Use None or + * InvertMotorOutput for the lead motor. + */ + public CANTalonFX(int deviceNumber, NeutralMode neutralMode, InvertType inverted) { + this(deviceNumber, neutralMode, inverted, DEFAULT_NEUTRAL_DEADBAND); + } - public void setCoastMode() { + /** + * Setting to enable brake mode on neutral (when .neutralOutput(), + * .disable(), or .stopMotor() is called, or when output percent is within + * neutralDeadbandPercent of zero). + * + * This does not brake the motor. Use .neutralOutput() instead, after + * setBrakeOnNeutral. + */ + public TalonMotorController setBrakeOnNeutral() { + setNeutralMode(NeutralMode.Brake); + return this; + } + /** + * Setting to enable coast mode on neutral (when .neutralOutput(), + * .disable(), or .stopMotor() is called, or when output percent is within + * neutralDeadbandPercent of zero). + * + * This does not coast the motor. Use .neutralOutput() instead, after + * setCoastOnNeutral. + */ + public TalonMotorController setCoastOnNeutral() { setNeutralMode(NeutralMode.Coast); + return this; } - public void setBrakeMode() { - setNeutralMode(NeutralMode.Brake); + @Override + public TalonMotorController follow(TalonMotorController leader) { + follow(leader); + return this; } } \ No newline at end of file diff --git a/custom/motioncontrollers/CANTalonSRX.java b/custom/motioncontrollers/CANTalonSRX.java index 065ff288..6992bffb 100644 --- a/custom/motioncontrollers/CANTalonSRX.java +++ b/custom/motioncontrollers/CANTalonSRX.java @@ -6,23 +6,94 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import org.usfirst.frc4904.standard.subsystems.motor.BrakeableMotorController; -public class CANTalonSRX extends WPI_TalonSRX implements BrakeableMotorController { - protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; +import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorController; +public class CANTalonSRX extends WPI_TalonSRX implements TalonMotorController { + protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; + protected static final InvertType DEFAULT_INVERT_TYPE = InvertType.FollowMaster; + protected static final double DEFAULT_NEUTRAL_DEADBAND = 0.001; // 0.1%, the minimum possible value - // TODO: implement setVoltage with native APIs - - public CANTalonSRX(int deviceNumber, NeutralMode neutralMode, InvertType inverted) { + /** + * Represents a TalonSRX controller (eg. attached to a 775) in code. You + * probably want NeutralMode.Brake, InvertType.FollowMaster. + * + * @param deviceNumber Usually the CAN ID of the device, + * declared in RobotMap + * @param neutralMode Whether the motor should brake or coast + * when the the output is near zero, or + * .disable() or .stopMotor() are called. + * @param inverted InvertMode of the motor. If this will be + * part of a motor group, consider using + * FolloMaster or OpposeMaster, so that you + * can invert the entire motor group by + * inverting the lead motor. Use None or + * InvertMotorOutput for the lead motor. + * @param neutralDeadbandPercent Percent output range around zero to + * enable neutralOutput (brake/coast mode) + * instead. For more info, see + * https://v5.docs.ctr-electronics.com/en/latest/ch13_MC.html#neutral-deadband + */ + public CANTalonSRX(int deviceNumber, NeutralMode neutralMode, InvertType inverted, double neutralDeadbandPercent) { super(deviceNumber); + configFactoryDefault(); // use default settings to prevent unexpected behavior, reccommended in examples + configNeutralDeadband(neutralDeadbandPercent); setNeutralMode(neutralMode); setInverted(inverted); } + /** + * Represents a TalonSRX controller (eg. attached to a 775) in code. You + * probably want NeutralMode.Brake, InvertType.FollowMaster. + * + * @param deviceNumber Usually the CAN ID of the device, + * declared in RobotMap + * @param neutralMode Whether the motor should brake or coast + * when the the output is near zero, or + * .disable() or .stopMotor() are called. + * @param inverted InvertMode of the motor. If this will be + * part of a motor group, consider using + * FolloMaster or OpposeMaster, so that you + * can invert the entire motor group by + * inverting the lead motor. Use None or + * InvertMotorOutput for the lead motor. + */ + public CANTalonSRX(int deviceNumber, NeutralMode neutralMode, InvertType inverted) { + this(deviceNumber, neutralMode, inverted, DEFAULT_NEUTRAL_DEADBAND); + } - public void setCoastMode() { + /** + * Setting to enable brake mode on neutral (when .neutralOutput(), + * .disable(), or .stopMotor() is called, or when output percent is within + * neutralDeadbandPercent of zero). + * + * This does not brake the motor. Use .neutralOutput() instead, after + * setBrakeOnNeutral. + */ + public TalonMotorController setBrakeOnNeutral() { + setNeutralMode(NeutralMode.Brake); + return this; + } + /** + * Setting to enable coast mode on neutral (when .neutralOutput(), + * .disable(), or .stopMotor() is called, or when output percent is within + * neutralDeadbandPercent of zero). + * + * This does not coast the motor. Use .neutralOutput() instead, after + * setCoastOnNeutral. + */ + public TalonMotorController setCoastOnNeutral() { setNeutralMode(NeutralMode.Coast); + return this; } - - public void setBrakeMode() { - setNeutralMode(NeutralMode.Brake); + + /** + * Set this motorController to follow the percent output of another talon + * motor controller. + * + * NOTE: maybe we need an AUX_OUTPUT version of this method if we ever + * decide to use the Talons' Auxilary PID + */ + @Override + public TalonMotorController follow(TalonMotorController leader) { + follow(leader); + return this; } } diff --git a/subsystems/motor/MotorSubsystem.java b/subsystems/motor/MotorSubsystem.java index 6f4f75e2..863def1c 100644 --- a/subsystems/motor/MotorSubsystem.java +++ b/subsystems/motor/MotorSubsystem.java @@ -118,9 +118,10 @@ public void stopMotor() { * * @return The most recently set power between -1.0 and 1.0. */ - public double get() { - return prevPower; - } + // TODO: do we even need .get()? if not, also remove prevPower and the edge casing in setVoltage? + // public double get() { + // return prevPower; + // } /** * Set the motor power. Passes through SpeedModifier. @@ -220,7 +221,7 @@ public void setVoltage(double voltage) { */ public Command c_setVoltageHold(double voltage) { return this.run(() -> this.setVoltage(voltage)); } - /// ERRORS + /// ERRORS (copied from 2022 standard Motor.java, not clear what the use is) protected class UnsynchronizedMotorControllerRuntimeException extends RuntimeException { private static final long serialVersionUID = 8688590919561059584L; diff --git a/subsystems/motor/TalonMotorController.java b/subsystems/motor/TalonMotorController.java new file mode 100644 index 00000000..83453c43 --- /dev/null +++ b/subsystems/motor/TalonMotorController.java @@ -0,0 +1,24 @@ +package org.usfirst.frc4904.standard.subsystems.motor; + +/** + * A base class for CANTalonFX and CANTalonSRX that extends 4904 MotorController + * but separates Talons from eg. SparkMaxes. + * + * May be converted to a general interface for motor controllers in the future, + * given that SparkMaxes can also do brake mode, follow mode, etc. + */ +public interface TalonMotorController extends BrakeableMotorController { + //TODO: add all the things + + // TODO: implement setVoltage with native APIs? or just use voltageComp? + + /** + * Follow `leader`'s percent output. + * + * TODO: also add an auxoutput version if SparkMax supports it + * TODO: use deviceNumber to make this spark/talon agnostic? + * + * @param leader the motor to follow + */ + public TalonMotorController follow(TalonMotorController leader); // return self to allow builder pattern +} diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java new file mode 100644 index 00000000..636f8347 --- /dev/null +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -0,0 +1,74 @@ +package org.usfirst.frc4904.standard.subsystems.motor; + +import java.util.stream.Stream; + +import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; + +public class TalonMotorSubsystem extends BrakeableMotorSubsystem { + public final TalonMotorController leadMotor; + public final TalonMotorController[] followMotors; + + /** + * Motor Subsystem for a group of Talon motor controllers (Falcons, 775s). + * Uses Talon-specific APIs like follow mode and motionProfile control mode to + * offload work from the RoboRIO. + * + * You probably want to set inverted mode on the TalonMotorController using + * FollowLeader or OpposeLeader + * @param name + * @param speedModifier + * @param leadMotor + * @param followMotors + */ + public TalonMotorSubsystem(String name, SpeedModifier speedModifier, TalonMotorController leadMotor, TalonMotorController... followMotors) { + super(name, speedModifier, Stream.concat(Stream.of(leadMotor), Stream.of(followMotors)).toArray(TalonMotorController[]::new)); // java has no spread operator, so you have to concat. best way i could find is to do it in a stream + + this.leadMotor = leadMotor; + this.followMotors = followMotors; + + setFollowMode(); + } + /** + * Make all follow motors follow the lead motor. + */ + private void setFollowMode() { + for (var motor : this.followMotors) { + motor.follow(leadMotor); + } + } + + // don't override disable() or stop() because we *should* indeed use the base implementation of disabling/stopping each motor controller individually. Otherwise the following motors will try to follow a disabled motor, which may cause unexpected behavior (although realistically, it likely just gets set to zero and neutrallized by the deadband). + + @Override + public void set(double power) { + setFollowMode(); // make sure all motors are following the lead as we expect. Possible OPTIMIZATION: store we set the output type to something else on the follow motors (eg. Neutral), and only re-set follow mode if we did. + leadMotor.set(power); + } + + // @Override + // public double get() { + // return prevPower; + // // if we actually need this, we'll need to store prevPower and add an edge case in setVoltage and update it in set() + // // or maybe just get the leadMotorController.get()? + // } + + @Override + /** + * NOTE: CTRE BaseMotorController setVoltage just uses set() under the hood. + * Follow motors will use the same percent output as calculated by the + * setVoltage of the lead motor. This is fine, and equivalent to the base + * implementation (calling setVoltage on each motor) as that will just run the + * same voltage->power calculation for each motor. + * + * TODO: shouldn't we use voltagecomp instead + */ + public void setVoltage(double voltage) { + setFollowMode(); + leadMotor.setVoltage(voltage); + } + + // no need to override setPower because the base class just uses set + + // TODO: does setting brake mode on leader set it on follower? if so, then override setBrakeModeOnNeutral, setCoastModeOnNeutral, and neutralOutput +} + From 62b414b76837a693363141efce894ac52ce10bc3 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Tue, 14 Feb 2023 19:32:31 -0800 Subject: [PATCH 032/134] cannot builder pattern on TalonMC.follow bc name collision --- .../motor}/CANTalonFX.java | 10 +--------- .../motor}/CANTalonSRX.java | 17 +---------------- subsystems/motor/TalonMotorController.java | 4 +++- subsystems/motor/TalonMotorSubsystem.java | 18 +++++++++++++++++- 4 files changed, 22 insertions(+), 27 deletions(-) rename {custom/motorcontrollers => subsystems/motor}/CANTalonFX.java (93%) rename {custom/motorcontrollers => subsystems/motor}/CANTalonSRX.java (89%) diff --git a/custom/motorcontrollers/CANTalonFX.java b/subsystems/motor/CANTalonFX.java similarity index 93% rename from custom/motorcontrollers/CANTalonFX.java rename to subsystems/motor/CANTalonFX.java index 38dbc621..ade10466 100644 --- a/custom/motorcontrollers/CANTalonFX.java +++ b/subsystems/motor/CANTalonFX.java @@ -1,11 +1,9 @@ -package org.usfirst.frc4904.standard.custom.motioncontrollers; +package org.usfirst.frc4904.standard.subsystems.motor; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorController; - public class CANTalonFX extends WPI_TalonFX implements TalonMotorController { protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; protected static final InvertType DEFAULT_INVERT_TYPE = InvertType.FollowMaster; @@ -82,10 +80,4 @@ public TalonMotorController setCoastOnNeutral() { setNeutralMode(NeutralMode.Coast); return this; } - - @Override - public TalonMotorController follow(TalonMotorController leader) { - follow(leader); - return this; - } } \ No newline at end of file diff --git a/custom/motorcontrollers/CANTalonSRX.java b/subsystems/motor/CANTalonSRX.java similarity index 89% rename from custom/motorcontrollers/CANTalonSRX.java rename to subsystems/motor/CANTalonSRX.java index 6992bffb..1b2764ce 100644 --- a/custom/motorcontrollers/CANTalonSRX.java +++ b/subsystems/motor/CANTalonSRX.java @@ -1,12 +1,10 @@ // THIS FILE IS TESTED post wpilibj2 -package org.usfirst.frc4904.standard.custom.motioncontrollers; +package org.usfirst.frc4904.standard.subsystems.motor; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; - -import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorController; public class CANTalonSRX extends WPI_TalonSRX implements TalonMotorController { protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; protected static final InvertType DEFAULT_INVERT_TYPE = InvertType.FollowMaster; @@ -83,17 +81,4 @@ public TalonMotorController setCoastOnNeutral() { setNeutralMode(NeutralMode.Coast); return this; } - - /** - * Set this motorController to follow the percent output of another talon - * motor controller. - * - * NOTE: maybe we need an AUX_OUTPUT version of this method if we ever - * decide to use the Talons' Auxilary PID - */ - @Override - public TalonMotorController follow(TalonMotorController leader) { - follow(leader); - return this; - } } diff --git a/subsystems/motor/TalonMotorController.java b/subsystems/motor/TalonMotorController.java index 83453c43..8454db40 100644 --- a/subsystems/motor/TalonMotorController.java +++ b/subsystems/motor/TalonMotorController.java @@ -1,5 +1,7 @@ package org.usfirst.frc4904.standard.subsystems.motor; +import com.ctre.phoenix.motorcontrol.IMotorController; + /** * A base class for CANTalonFX and CANTalonSRX that extends 4904 MotorController * but separates Talons from eg. SparkMaxes. @@ -20,5 +22,5 @@ public interface TalonMotorController extends BrakeableMotorController { * * @param leader the motor to follow */ - public TalonMotorController follow(TalonMotorController leader); // return self to allow builder pattern + public void follow(IMotorController leader); // are we able to return self to allow builder pattern? prob have to change the method name, else polymorphism breaks bc name collision with the one in base that takes IMotorController } diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index 636f8347..0af0fe53 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -2,6 +2,7 @@ import java.util.stream.Stream; +import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; public class TalonMotorSubsystem extends BrakeableMotorSubsystem { @@ -21,13 +22,28 @@ public class TalonMotorSubsystem extends BrakeableMotorSubsystem { * @param followMotors */ public TalonMotorSubsystem(String name, SpeedModifier speedModifier, TalonMotorController leadMotor, TalonMotorController... followMotors) { - super(name, speedModifier, Stream.concat(Stream.of(leadMotor), Stream.of(followMotors)).toArray(TalonMotorController[]::new)); // java has no spread operator, so you have to concat. best way i could find is to do it in a stream + super(name, speedModifier, Stream.concat(Stream.of(leadMotor), Stream.of(followMotors)).toArray(TalonMotorController[]::new)); // java has no spread operator, so you have to concat. best way i could find is to do it in a stream. please make this not bad if you know how this.leadMotor = leadMotor; this.followMotors = followMotors; setFollowMode(); } + /** + * Motor Subsystem for a group of Talon motor controllers (Falcons, 775s). + * Uses Talon-specific APIs like follow mode and motionProfile control mode to + * offload work from the RoboRIO. + * + * You probably want to set inverted mode on the TalonMotorController using + * FollowLeader or OpposeLeader + * @param name + * @param leadMotor + * @param followMotors + */ + public TalonMotorSubsystem(String name, TalonMotorController leadMotor, TalonMotorController... followMotors) { + this(name, new IdentityModifier(), leadMotor, followMotors); + } + /** * Make all follow motors follow the lead motor. */ From 989e228aba5aeeb9ee36c498cc429f8bfb4a2c73 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Tue, 14 Feb 2023 19:35:31 -0800 Subject: [PATCH 033/134] brakeableMotorSubsys use this.runOnce to auto-require --- subsystems/motor/BrakeableMotorSubsystem.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/subsystems/motor/BrakeableMotorSubsystem.java b/subsystems/motor/BrakeableMotorSubsystem.java index dd7be35f..f7a52f04 100644 --- a/subsystems/motor/BrakeableMotorSubsystem.java +++ b/subsystems/motor/BrakeableMotorSubsystem.java @@ -4,7 +4,6 @@ import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; public class BrakeableMotorSubsystem extends MotorSubsystem { // TODO: should this take general MotorControllers and just brake the ones that implement BrakeableMotorController @@ -26,7 +25,7 @@ public class BrakeableMotorSubsystem extends MotorSubsystem { public BrakeableMotorSubsystem(String name, SpeedModifier speedModifier, BrakeableMotorController... motors) { super(name, speedModifier, motors); this.motors = motors; - setDefaultCommand(Commands.runOnce(() -> this.neutralOutput())); + setDefaultCommand(this.runOnce(() -> this.neutralOutput())); } /** From a9cb241675740595f4145e82760915ec390300b2 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Tue, 14 Feb 2023 20:21:37 -0800 Subject: [PATCH 034/134] TalonMotorController extend IMotorControllerEnhanced; test followed brake mode need to extend so that we can pass TalonMotorController to .follow() which expects an IMotorController --- subsystems/motor/TalonMotorController.java | 3 ++- subsystems/motor/TalonMotorSubsystem.java | 5 +++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/subsystems/motor/TalonMotorController.java b/subsystems/motor/TalonMotorController.java index 8454db40..da119425 100644 --- a/subsystems/motor/TalonMotorController.java +++ b/subsystems/motor/TalonMotorController.java @@ -1,6 +1,7 @@ package org.usfirst.frc4904.standard.subsystems.motor; import com.ctre.phoenix.motorcontrol.IMotorController; +import com.ctre.phoenix.motorcontrol.IMotorControllerEnhanced; /** * A base class for CANTalonFX and CANTalonSRX that extends 4904 MotorController @@ -9,7 +10,7 @@ * May be converted to a general interface for motor controllers in the future, * given that SparkMaxes can also do brake mode, follow mode, etc. */ -public interface TalonMotorController extends BrakeableMotorController { +public interface TalonMotorController extends BrakeableMotorController, IMotorControllerEnhanced { //TODO: add all the things // TODO: implement setVoltage with native APIs? or just use voltageComp? diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index 0af0fe53..6b6c9761 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -9,6 +9,8 @@ public class TalonMotorSubsystem extends BrakeableMotorSubsystem { public final TalonMotorController leadMotor; public final TalonMotorController[] followMotors; + // TODO: limit switches + /** * Motor Subsystem for a group of Talon motor controllers (Falcons, 775s). * Uses Talon-specific APIs like follow mode and motionProfile control mode to @@ -84,7 +86,6 @@ public void setVoltage(double voltage) { } // no need to override setPower because the base class just uses set - - // TODO: does setting brake mode on leader set it on follower? if so, then override setBrakeModeOnNeutral, setCoastModeOnNeutral, and neutralOutput + // don't override setBrakeOnNeutral, setCoastOnNeutral, neutralOutput because we indeed want to set it individually on each motor. Otherwise, the followers might try to follow a disabled/neutral motor which might cause unexpected behavior. } From 29564146e3e3da5577f4a32ffe73aee19bbb1616 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 16 Feb 2023 17:07:51 -0800 Subject: [PATCH 035/134] make MotorSubsys and BrakeMotorSubsys generic so that extensions can use super.motors with it's own class --- subsystems/motor/BrakeableMotorSubsystem.java | 13 +++--- subsystems/motor/MotorController.java | 1 - subsystems/motor/MotorSubsystem.java | 42 +++++++++---------- 3 files changed, 27 insertions(+), 29 deletions(-) diff --git a/subsystems/motor/BrakeableMotorSubsystem.java b/subsystems/motor/BrakeableMotorSubsystem.java index f7a52f04..eb2237d0 100644 --- a/subsystems/motor/BrakeableMotorSubsystem.java +++ b/subsystems/motor/BrakeableMotorSubsystem.java @@ -5,9 +5,9 @@ import edu.wpi.first.wpilibj2.command.Command; -public class BrakeableMotorSubsystem extends MotorSubsystem { + +public class BrakeableMotorSubsystem extends MotorSubsystem { // generic to allow inherited class (eg. TalonMotorSubsystem) to directly use TalonMotorController APIs on super.motors (not possible if this.motors here was BrakeableMotorController) // TODO: should this take general MotorControllers and just brake the ones that implement BrakeableMotorController - protected final BrakeableMotorController[] motors; /** * A class that wraps around a variable number of BrakeableMotorController @@ -22,9 +22,8 @@ public class BrakeableMotorSubsystem extends MotorSubsystem { * @param motors The MotorControllers in this subsystem. Can be a single * MotorController or multiple MotorControllers. */ - public BrakeableMotorSubsystem(String name, SpeedModifier speedModifier, BrakeableMotorController... motors) { + public BrakeableMotorSubsystem(String name, SpeedModifier speedModifier, BMC... motors) { super(name, speedModifier, motors); - this.motors = motors; setDefaultCommand(this.runOnce(() -> this.neutralOutput())); } @@ -38,7 +37,7 @@ public BrakeableMotorSubsystem(String name, SpeedModifier speedModifier, Brakeab * @param motors The MotorControllers in this subsystem. Can be a single * MotorController or multiple MotorControllers. */ - public BrakeableMotorSubsystem(String name, BrakeableMotorController... motors) { + public BrakeableMotorSubsystem(String name, BMC... motors) { this(name, new IdentityModifier(), motors); } @@ -54,7 +53,7 @@ public BrakeableMotorSubsystem(String name, BrakeableMotorController... motors) * @param motors The MotorControllers in this subsystem. Can be a single * MotorController or multiple MotorControllers. */ - public BrakeableMotorSubsystem(SpeedModifier speedModifier, BrakeableMotorController... motors) { + public BrakeableMotorSubsystem(SpeedModifier speedModifier, BMC... motors) { this("Motor", speedModifier, motors); } @@ -67,7 +66,7 @@ public BrakeableMotorSubsystem(SpeedModifier speedModifier, BrakeableMotorContro * @param motors The MotorControllers in this subsystem. Can be a single * MotorController or multiple MotorControllers. */ - public BrakeableMotorSubsystem(BrakeableMotorController... motors) { + public BrakeableMotorSubsystem(BMC... motors) { this("Motor", motors); } diff --git a/subsystems/motor/MotorController.java b/subsystems/motor/MotorController.java index c20707b9..505d89ce 100644 --- a/subsystems/motor/MotorController.java +++ b/subsystems/motor/MotorController.java @@ -8,7 +8,6 @@ * - to force custom implementation of setVoltage (WPILib's uses RobotController.getBatteryVoltage() which is slow, cite @zbuster05) */ public interface MotorController extends edu.wpi.first.wpilibj.motorcontrol.MotorController { - /** * Implementations should implement this using a better voltage reading than * RobotController.getBatteryVoltage(), preferably a native interface. diff --git a/subsystems/motor/MotorSubsystem.java b/subsystems/motor/MotorSubsystem.java index 863def1c..c3af4742 100644 --- a/subsystems/motor/MotorSubsystem.java +++ b/subsystems/motor/MotorSubsystem.java @@ -19,8 +19,8 @@ * Replaces Motor.java in pre-2023 standard, except without CTRE voltage comp by * default and without inversion logic. */ -public class MotorSubsystem extends SubsystemBase { - protected final MotorController[] motors; +public class MotorSubsystem extends SubsystemBase { + protected final MC[] motors; protected final SpeedModifier speedModifier; // NOTE: maybe change to be called PowerModifier protected final String name; protected double prevPower; @@ -37,7 +37,7 @@ public class MotorSubsystem extends SubsystemBase { * @param motors The MotorControllers in this subsystem. Can be a single * MotorController or multiple MotorControllers. */ - public MotorSubsystem(String name, SpeedModifier speedModifier, MotorController... motors) { + public MotorSubsystem(String name, SpeedModifier speedModifier, MC... motors) { super(); setName(name); this.name = name; @@ -59,7 +59,7 @@ public MotorSubsystem(String name, SpeedModifier speedModifier, MotorController. * @param motors The MotorControllers in this subsystem. Can be a single * MotorController or multiple MotorControllers. */ - public MotorSubsystem(String name, MotorController... motors) { + public MotorSubsystem(String name, MC... motors) { this(name, new IdentityModifier(), motors); } @@ -74,7 +74,7 @@ public MotorSubsystem(String name, MotorController... motors) { * @param motors The MotorControllers in this subsystem. Can be a single * MotorController or multiple MotorControllers. */ - public MotorSubsystem(SpeedModifier speedModifier, MotorController... motors) { + public MotorSubsystem(SpeedModifier speedModifier, MC... motors) { this("Motor", speedModifier, motors); } @@ -86,7 +86,7 @@ public MotorSubsystem(SpeedModifier speedModifier, MotorController... motors) { * @param motors The MotorControllers in this subsystem. Can be a single * MotorController or multiple MotorControllers. */ - public MotorSubsystem(MotorController... motors) { + public MotorSubsystem(MC... motors) { this("Motor", motors); } @@ -221,22 +221,22 @@ public void setVoltage(double voltage) { */ public Command c_setVoltageHold(double voltage) { return this.run(() -> this.setVoltage(voltage)); } - /// ERRORS (copied from 2022 standard Motor.java, not clear what the use is) - protected class UnsynchronizedMotorControllerRuntimeException extends RuntimeException { - private static final long serialVersionUID = 8688590919561059584L; + // /// ERRORS (copied from 2022 standard Motor.java, not clear what the use is) + // protected class UnsynchronizedMotorControllerRuntimeException extends RuntimeException { + // private static final long serialVersionUID = 8688590919561059584L; - public UnsynchronizedMotorControllerRuntimeException() { - super(getName() + "'s MotorControllers report different speeds"); - } - } + // public UnsynchronizedMotorControllerRuntimeException() { + // super(getName() + "'s MotorControllers report different speeds"); + // } + // } - @Deprecated - protected class StrangeCANMotorControllerModeRuntimeException extends RuntimeException { - private static final long serialVersionUID = -539917227288371271L; + // @Deprecated + // protected class StrangeCANMotorControllerModeRuntimeException extends RuntimeException { + // private static final long serialVersionUID = -539917227288371271L; - public StrangeCANMotorControllerModeRuntimeException() { - super("One of " + getName() - + "'s MotorControllers is a CANMotorController with a non-zero mode. This might mess up it's .get(), so Motor cannot verify safety."); - } - } + // public StrangeCANMotorControllerModeRuntimeException() { + // super("One of " + getName() + // + "'s MotorControllers is a CANMotorController with a non-zero mode. This might mess up it's .get(), so Motor cannot verify safety."); + // } + // } } From bbcd23b26aad5c2876d082f7e6833ecd24fabe64 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 16 Feb 2023 17:09:09 -0800 Subject: [PATCH 036/134] move neutralMode, neutralDeadband to motorsubsys; voltagecomp and limitswitches --- subsystems/motor/CANTalonFX.java | 30 +------ subsystems/motor/CANTalonSRX.java | 9 -- subsystems/motor/TalonMotorSubsystem.java | 105 ++++++++++++++++++++-- 3 files changed, 99 insertions(+), 45 deletions(-) diff --git a/subsystems/motor/CANTalonFX.java b/subsystems/motor/CANTalonFX.java index ade10466..49eadf2f 100644 --- a/subsystems/motor/CANTalonFX.java +++ b/subsystems/motor/CANTalonFX.java @@ -15,46 +15,18 @@ public class CANTalonFX extends WPI_TalonFX implements TalonMotorController { * * @param deviceNumber Usually the CAN ID of the device, * declared in RobotMap - * @param neutralMode Whether the motor should brake or coast - * when the the output is near zero, or - * .disable() or .stopMotor() are called. * @param inverted InvertMode of the motor. If this will be * part of a motor group, consider using * FolloMaster or OpposeMaster, so that you * can invert the entire motor group by * inverting the lead motor. Use None or * InvertMotorOutput for the lead motor. - * @param neutralDeadbandPercent Percent output range around zero to - * enable neutralOutput (brake/coast mode) - * instead. For more info, see - * https://v5.docs.ctr-electronics.com/en/latest/ch13_MC.html#neutral-deadband */ - public CANTalonFX(int deviceNumber, NeutralMode neutralMode, InvertType inverted, double neutralDeadbandPercent) { + public CANTalonFX(int deviceNumber, InvertType inverted) { super(deviceNumber); configFactoryDefault(); // use default settings to prevent unexpected behavior, reccommended in examples - configNeutralDeadband(neutralDeadbandPercent); - setNeutralMode(neutralMode); setInverted(inverted); } - /** - * Represents a Falcon motor in code. You probably want NeutralMode.Brake, - * InvertType.FollowMaster. - * - * @param deviceNumber Usually the CAN ID of the device, - * declared in RobotMap - * @param neutralMode Whether the motor should brake or coast - * when the the output is near zero, or - * .disable() or .stopMotor() are called. - * @param inverted InvertMode of the motor. If this will be - * part of a motor group, consider using - * FolloMaster or OpposeMaster, so that you - * can invert the entire motor group by - * inverting the lead motor. Use None or - * InvertMotorOutput for the lead motor. - */ - public CANTalonFX(int deviceNumber, NeutralMode neutralMode, InvertType inverted) { - this(deviceNumber, neutralMode, inverted, DEFAULT_NEUTRAL_DEADBAND); - } /** * Setting to enable brake mode on neutral (when .neutralOutput(), diff --git a/subsystems/motor/CANTalonSRX.java b/subsystems/motor/CANTalonSRX.java index 1b2764ce..2e8876fa 100644 --- a/subsystems/motor/CANTalonSRX.java +++ b/subsystems/motor/CANTalonSRX.java @@ -16,25 +16,16 @@ public class CANTalonSRX extends WPI_TalonSRX implements TalonMotorController { * * @param deviceNumber Usually the CAN ID of the device, * declared in RobotMap - * @param neutralMode Whether the motor should brake or coast - * when the the output is near zero, or - * .disable() or .stopMotor() are called. * @param inverted InvertMode of the motor. If this will be * part of a motor group, consider using * FolloMaster or OpposeMaster, so that you * can invert the entire motor group by * inverting the lead motor. Use None or * InvertMotorOutput for the lead motor. - * @param neutralDeadbandPercent Percent output range around zero to - * enable neutralOutput (brake/coast mode) - * instead. For more info, see - * https://v5.docs.ctr-electronics.com/en/latest/ch13_MC.html#neutral-deadband */ public CANTalonSRX(int deviceNumber, NeutralMode neutralMode, InvertType inverted, double neutralDeadbandPercent) { super(deviceNumber); configFactoryDefault(); // use default settings to prevent unexpected behavior, reccommended in examples - configNeutralDeadband(neutralDeadbandPercent); - setNeutralMode(neutralMode); setInverted(inverted); } /** diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index 6b6c9761..a8c59558 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -5,11 +5,20 @@ import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; -public class TalonMotorSubsystem extends BrakeableMotorSubsystem { +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; + +public class TalonMotorSubsystem extends BrakeableMotorSubsystem { + private final int configTimeoutMs = 50; // milliseconds until the Talon gives up trying to configure public final TalonMotorController leadMotor; public final TalonMotorController[] followMotors; + // TODO: voltage ramping (closed and open) + // TODO: limit switches + // TODO: add voltage/slew limit to drivetrain motors because we don't want the pid to actively try to stop the motor (negative power) when the driver just lets go of the controls. /** * Motor Subsystem for a group of Talon motor controllers (Falcons, 775s). @@ -18,17 +27,74 @@ public class TalonMotorSubsystem extends BrakeableMotorSubsystem { * * You probably want to set inverted mode on the TalonMotorController using * FollowLeader or OpposeLeader + * * @param name - * @param speedModifier + * @param speedModifier + * @param neutralMode Whether the motor should brake or coast + * when the the output is near zero, or + * .disable() or .stopMotor() are called. + * @param neutralDeadbandPercent Percent output range around zero to enable + * neutralOutput (brake/coast mode) instead. + * For more info, see + * https://v5.docs.ctr-electronics.com/en/latest/ch13_MC.html#neutral-deadband + * @param respectLeadMotorLimitSwitches Whether to enable the forward and + * reverse limit switches wired to the + * lead motor. This is the easiest way to + * use forward and reverse normally open + * limit switches (or just forward and + * just reverse, as not plugging a limit + * switch in is equivalent to a normally + * open switch being unpressed). NOTE + * This configuration is relatively + * limited, consider configuring limit + * switches manually on the + * TalonMotorController for advanced + * configuration (eg. normally closed + * switches). + * @param voltageCompensation 0 to disable, 10 is a good default. Set + * the voltage corresponding to power=1.0 + * This way, setting a power will lead to + * consistent output even when other + * components are running. Basically nerf all + * motors so that they have a consistent + * output. when the battery is low. * @param leadMotor * @param followMotors */ - public TalonMotorSubsystem(String name, SpeedModifier speedModifier, TalonMotorController leadMotor, TalonMotorController... followMotors) { + public TalonMotorSubsystem(String name, SpeedModifier speedModifier, NeutralMode neutralMode, double neutralDeadbandPercent, + Boolean respectLeadMotorLimitSwitches, double voltageCompensation, + TalonMotorController leadMotor, TalonMotorController... followMotors) { super(name, speedModifier, Stream.concat(Stream.of(leadMotor), Stream.of(followMotors)).toArray(TalonMotorController[]::new)); // java has no spread operator, so you have to concat. best way i could find is to do it in a stream. please make this not bad if you know how this.leadMotor = leadMotor; this.followMotors = followMotors; + // limit switch configuration + if (respectLeadMotorLimitSwitches) { + // when extending to SparkMAX: you have to sparkmax.getForward/ReverseLimitSwitch.enable() or something. may need custom polling/plugin logic. https://codedocs.revrobotics.com/java/com/revrobotics/cansparkmax#getReverseLimitSwitch(com.revrobotics.SparkMaxLimitSwitch.Type) + + /* notes on limit switches + * - FeedbackConnector is a given falcon's own feedback connector (the little white 4-pin plug next to the talonfx bump) + * - the LimitSwitchSource enum has both Talon and TalonSRX, but they have the same value internally (as of 2023) so they should be indistinguishable? + */ + leadMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen, configTimeoutMs); + leadMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen, configTimeoutMs); + + for (var motor : followMotors) { + motor.configForwardLimitSwitchSource(RemoteLimitSwitchSource.RemoteTalon, LimitSwitchNormal.NormallyOpen, leadMotor.getDeviceID(), configTimeoutMs); + motor.configReverseLimitSwitchSource(RemoteLimitSwitchSource.RemoteTalon, LimitSwitchNormal.NormallyOpen, leadMotor.getDeviceID(), configTimeoutMs); + } + } + + // other configuration (neutral mode, neutral deadband, voltagecomp) + for (var motor : motors) { + motor.setNeutralMode(neutralMode); + motor.configNeutralDeadband(neutralDeadbandPercent, configTimeoutMs); + if (voltageCompensation > 0) { + motor.configVoltageCompSaturation(voltageCompensation, configTimeoutMs); + motor.enableVoltageCompensation(true); + } + } setFollowMode(); } /** @@ -38,12 +104,36 @@ public TalonMotorSubsystem(String name, SpeedModifier speedModifier, TalonMotorC * * You probably want to set inverted mode on the TalonMotorController using * FollowLeader or OpposeLeader + * + * Consider using the advanced constructor for limit switches and voltage + * compensation, and using the .cfg...() methods for voltage ramping and + * nominal output. + * + * When using SparkMaxes: + * - can limit switches be enabled/disabled? can you have a sparkmax respect a talon limit switch? https://codedocs.revrobotics.com/java/com/revrobotics/cansparkmax + * * @param name + * @param neutralMode Whether the motor should brake or coast + * when the the output is near zero, or + * .disable() or .stopMotor() are called. + * @param neutralDeadbandPercent Percent output range around zero to enable + * neutralOutput (brake/coast mode) instead. + * For more info, see + * https://v5.docs.ctr-electronics.com/en/latest/ch13_MC.html#neutral-deadband + * @param voltageCompensation 0 to disable, 10 is a good default. Set + * the voltage corresponding to power=1.0 + * This way, setting a power will lead to + * consistent output even when other + * components are running. Basically nerf all + * motors so that they have a consistent + * output. when the battery is low. * @param leadMotor * @param followMotors */ - public TalonMotorSubsystem(String name, TalonMotorController leadMotor, TalonMotorController... followMotors) { - this(name, new IdentityModifier(), leadMotor, followMotors); + public TalonMotorSubsystem(String name, NeutralMode neutralMode, double neutralDeadbandPercent, + double voltageCompensation, + TalonMotorController leadMotor, TalonMotorController... followMotors) { + this(name, new IdentityModifier(), neutralMode, neutralDeadbandPercent, false, voltageCompensation, leadMotor, followMotors); } /** @@ -72,13 +162,14 @@ public void set(double power) { @Override /** + * if using a Ramsete controller, make sure to disable voltage compensation, + * because voltages have an actual physical meaning from sysid. + * * NOTE: CTRE BaseMotorController setVoltage just uses set() under the hood. * Follow motors will use the same percent output as calculated by the * setVoltage of the lead motor. This is fine, and equivalent to the base * implementation (calling setVoltage on each motor) as that will just run the * same voltage->power calculation for each motor. - * - * TODO: shouldn't we use voltagecomp instead */ public void setVoltage(double voltage) { setFollowMode(); From a5dbf5fd1250a1597f173f1879f3e621d683ef89 Mon Sep 17 00:00:00 2001 From: henryharms Date: Thu, 16 Feb 2023 17:42:56 -0800 Subject: [PATCH 037/134] added custom spark max class (incomplete) --- .../motioncontrollers/CustomCANSparkMax.java | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 custom/motioncontrollers/CustomCANSparkMax.java diff --git a/custom/motioncontrollers/CustomCANSparkMax.java b/custom/motioncontrollers/CustomCANSparkMax.java new file mode 100644 index 00000000..8cf2dcf1 --- /dev/null +++ b/custom/motioncontrollers/CustomCANSparkMax.java @@ -0,0 +1,32 @@ +package org.usfirst.frc4904.standard.custom.motioncontrollers; + +import com.ctre.phoenix.motorcontrol.InvertType; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import org.usfirst.frc4904.standard.subsystems.motor.BrakeableMotorController; + +public class CustomCANSparkMax extends CANSparkMax implements BrakeableMotorController { + protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; + protected static final InvertType DEFAULT_INVERT_TYPE = InvertType.FollowMaster; + + public CustomCANSparkMax(int deviceNumber, MotorType motorType, IdleMode neutralMode, boolean inverted) { + super(deviceNumber, motorType); + setIdleMode(neutralMode); + setInverted(inverted); + } + + public void setBrakeMode() { + setIdleMode(IdleMode.kBrake); + + } + + public void setCoastMode() { + setIdleMode(IdleMode.kCoast); + } + + @Override + public void neutralOutput() { + stopMotor(); + } +} From eaa3d48a2d649404317331edfd47a7937c4f6eb7 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 16 Feb 2023 18:19:25 -0800 Subject: [PATCH 038/134] remove old neutralMode and deadbandPercent constructor on cantalonsrx; comment typos --- subsystems/motor/CANTalonFX.java | 8 ++++---- subsystems/motor/CANTalonSRX.java | 23 ++--------------------- 2 files changed, 6 insertions(+), 25 deletions(-) diff --git a/subsystems/motor/CANTalonFX.java b/subsystems/motor/CANTalonFX.java index 49eadf2f..814fc140 100644 --- a/subsystems/motor/CANTalonFX.java +++ b/subsystems/motor/CANTalonFX.java @@ -13,12 +13,12 @@ public class CANTalonFX extends WPI_TalonFX implements TalonMotorController { * Represents a Falcon motor in code. You probably want NeutralMode.Brake, * InvertType.FollowMaster. * - * @param deviceNumber Usually the CAN ID of the device, - * declared in RobotMap + * @param deviceNumber Usually the CAN ID of the device, + * declared in RobotMap * @param inverted InvertMode of the motor. If this will be * part of a motor group, consider using - * FolloMaster or OpposeMaster, so that you - * can invert the entire motor group by + * FollowMaster or OpposeMaster, so that + * you can invert the entire motor group by * inverting the lead motor. Use None or * InvertMotorOutput for the lead motor. */ diff --git a/subsystems/motor/CANTalonSRX.java b/subsystems/motor/CANTalonSRX.java index 2e8876fa..a4883403 100644 --- a/subsystems/motor/CANTalonSRX.java +++ b/subsystems/motor/CANTalonSRX.java @@ -18,35 +18,16 @@ public class CANTalonSRX extends WPI_TalonSRX implements TalonMotorController { * declared in RobotMap * @param inverted InvertMode of the motor. If this will be * part of a motor group, consider using - * FolloMaster or OpposeMaster, so that you + * FollowMaster or OpposeMaster, so that you * can invert the entire motor group by * inverting the lead motor. Use None or * InvertMotorOutput for the lead motor. */ - public CANTalonSRX(int deviceNumber, NeutralMode neutralMode, InvertType inverted, double neutralDeadbandPercent) { + public CANTalonSRX(int deviceNumber, InvertType inverted) { super(deviceNumber); configFactoryDefault(); // use default settings to prevent unexpected behavior, reccommended in examples setInverted(inverted); } - /** - * Represents a TalonSRX controller (eg. attached to a 775) in code. You - * probably want NeutralMode.Brake, InvertType.FollowMaster. - * - * @param deviceNumber Usually the CAN ID of the device, - * declared in RobotMap - * @param neutralMode Whether the motor should brake or coast - * when the the output is near zero, or - * .disable() or .stopMotor() are called. - * @param inverted InvertMode of the motor. If this will be - * part of a motor group, consider using - * FolloMaster or OpposeMaster, so that you - * can invert the entire motor group by - * inverting the lead motor. Use None or - * InvertMotorOutput for the lead motor. - */ - public CANTalonSRX(int deviceNumber, NeutralMode neutralMode, InvertType inverted) { - this(deviceNumber, neutralMode, inverted, DEFAULT_NEUTRAL_DEADBAND); - } /** * Setting to enable brake mode on neutral (when .neutralOutput(), From 894b752be0629a086f236eb3cb3b6d38c2217e40 Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Thu, 16 Feb 2023 21:19:48 -0600 Subject: [PATCH 039/134] eeeeeeeeeeeeeeeee (that took far too long) --- commands/chassis/ChassisConstant.java | 148 ++++----- commands/chassis/ChassisIdle.java | 66 ++-- commands/chassis/ChassisMinimumDistance.java | 236 +++++++-------- commands/chassis/ChassisMove.java | 286 +++++++++--------- commands/chassis/ChassisMoveDistance.java | 282 ++++++++--------- commands/chassis/ChassisTurn.java | 284 ++++++++--------- commands/chassis/ChassisTurnAbsolute.java | 148 ++++----- custom/CustomPIDSourceType.java | 5 - custom/MCChassisController.java | 1 - custom/controllers/CustomCommandJoystick.java | 2 - .../motioncontrollers/MotionController.java | 5 +- custom/motorcontrollers/CANTalonFX.java | 2 +- custom/motorcontrollers/CANTalonSRX.java | 2 +- custom/sensors/CANEncoder.java | 57 +--- custom/sensors/CANTalonEncoder.java | 56 +--- custom/sensors/CustomCANCoder.java | 58 +--- custom/sensors/CustomDigitalEncoder.java | 41 +-- custom/sensors/CustomEncoder.java | 9 +- custom/sensors/EncoderPair.java | 95 +----- custom/sensors/IMU.java | 4 - custom/sensors/NativeDerivativeSensor.java | 1 + custom/sensors/NavX.java | 39 +-- custom/sensors/PDP.java | 2 - subsystems/chassis/SensorDrive.java | 41 +-- subsystems/chassis/TankDrive.java | 2 - subsystems/motor/Motor.java | 2 - subsystems/motor/MotorController.java | 2 - 27 files changed, 752 insertions(+), 1124 deletions(-) delete mode 100644 custom/CustomPIDSourceType.java diff --git a/commands/chassis/ChassisConstant.java b/commands/chassis/ChassisConstant.java index 257bdbfa..bb8ddb00 100644 --- a/commands/chassis/ChassisConstant.java +++ b/commands/chassis/ChassisConstant.java @@ -1,84 +1,84 @@ -package org.usfirst.frc4904.standard.commands.chassis; +// package org.usfirst.frc4904.standard.commands.chassis; -import org.usfirst.frc4904.standard.custom.ChassisController; -import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +// import org.usfirst.frc4904.standard.custom.ChassisController; +// import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; +// import edu.wpi.first.wpilibj2.command.CommandScheduler; +// import edu.wpi.first.wpilibj2.command.CommandBase; +// import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -public class ChassisConstant extends CommandBase implements ChassisController { - protected final ParallelRaceGroup move; - protected final double x; - protected final double y; - protected double turn; - protected double timeout; +// public class ChassisConstant extends CommandBase implements ChassisController { +// protected final ParallelRaceGroup move; +// protected final double x; +// protected final double y; +// protected double turn; +// protected double timeout; - /** - * - * @param name - * @param chassis - * @param x - * @param y - * @param turn - * @param timeout - */ - public ChassisConstant(String name, Chassis chassis, double x, double y, double turn, double timeout) { - move = new ChassisMove(chassis, this).withTimeout(timeout); - this.timeout = timeout; - this.x = x; - this.y = y; - this.turn = turn; - setName(name); - addRequirements(chassis); - } +// /** +// * +// * @param name +// * @param chassis +// * @param x +// * @param y +// * @param turn +// * @param timeout +// */ +// public ChassisConstant(String name, Chassis chassis, double x, double y, double turn, double timeout) { +// move = new ChassisMove(chassis, this).withTimeout(timeout); +// this.timeout = timeout; +// this.x = x; +// this.y = y; +// this.turn = turn; +// setName(name); +// addRequirements(chassis); +// } - /** - * - * @param chassis - * @param x - * @param y - * @param turn - * @param timeout - */ - public ChassisConstant(Chassis chassis, double x, double y, double turn, double timeout) { - this("Chassis Constant", chassis, x, y, turn, timeout); - } +// /** +// * +// * @param chassis +// * @param x +// * @param y +// * @param turn +// * @param timeout +// */ +// public ChassisConstant(Chassis chassis, double x, double y, double turn, double timeout) { +// this("Chassis Constant", chassis, x, y, turn, timeout); +// } - @Override - public double getX() { - return x; - } +// @Override +// public double getX() { +// return x; +// } - @Override - public double getY() { - return y; - } +// @Override +// public double getY() { +// return y; +// } - @Override - public double getTurnSpeed() { - return turn; - } +// @Override +// public double getTurnSpeed() { +// return turn; +// } - @Override - public void initialize() { - move.schedule(); - } +// @Override +// public void initialize() { +// move.schedule(); +// } - /** - * - * The command has timed out if the time since scheduled is greater than the - * timeout - * - * @return finished - * - */ - @Override - public boolean isFinished() { - return move.isFinished() || CommandScheduler.getInstance().timeSinceScheduled(this) >= this.timeout; - } +// /** +// * +// * The command has timed out if the time since scheduled is greater than the +// * timeout +// * +// * @return finished +// * +// */ +// @Override +// public boolean isFinished() { +// return move.isFinished() || CommandScheduler.getInstance().timeSinceScheduled(this) >= this.timeout; +// } - @Override - public void end(boolean interrupted) { - move.cancel(); - } -} +// @Override +// public void end(boolean interrupted) { +// move.cancel(); +// } +// } diff --git a/commands/chassis/ChassisIdle.java b/commands/chassis/ChassisIdle.java index 023afcbf..142d97ef 100644 --- a/commands/chassis/ChassisIdle.java +++ b/commands/chassis/ChassisIdle.java @@ -1,37 +1,37 @@ -package org.usfirst.frc4904.standard.commands.chassis; +// package org.usfirst.frc4904.standard.commands.chassis; -import org.usfirst.frc4904.standard.commands.motor.MotorIdle; -import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; -import org.usfirst.frc4904.standard.subsystems.motor.Motor; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.Command; +// import org.usfirst.frc4904.standard.commands.motor.MotorIdle; +// import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; +// import org.usfirst.frc4904.standard.subsystems.motor.Motor; +// import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +// import edu.wpi.first.wpilibj2.command.Command; -/** - * - * This command causes the Chassis to idle by spawning a MotorIdle for every - * Motor in the Chassis. - * - */ -public class ChassisIdle extends ParallelCommandGroup { - /** - * - * @param name - * @param chassis The robot Chassis to idle. - */ - public ChassisIdle(String name, Chassis chassis) { - for (Motor motor : chassis.getMotors()) { - addCommands((Command) new MotorIdle(motor)); - } +// /** +// * +// * This command causes the Chassis to idle by spawning a MotorIdle for every +// * Motor in the Chassis. +// * +// */ +// public class ChassisIdle extends ParallelCommandGroup { +// /** +// * +// * @param name +// * @param chassis The robot Chassis to idle. +// */ +// public ChassisIdle(String name, Chassis chassis) { +// for (Motor motor : chassis.getMotors()) { +// addCommands((Command) new MotorIdle(motor)); +// } - setName(name); - addRequirements(chassis); - } +// setName(name); +// addRequirements(chassis); +// } - /** - * - * @param chassis The robot Chassis to idle. - */ - public ChassisIdle(Chassis chassis) { - this("ChassisIdle", chassis); - } -} +// /** +// * +// * @param chassis The robot Chassis to idle. +// */ +// public ChassisIdle(Chassis chassis) { +// this("ChassisIdle", chassis); +// } +// } diff --git a/commands/chassis/ChassisMinimumDistance.java b/commands/chassis/ChassisMinimumDistance.java index 5813921d..4ea67b4f 100644 --- a/commands/chassis/ChassisMinimumDistance.java +++ b/commands/chassis/ChassisMinimumDistance.java @@ -1,128 +1,128 @@ -package org.usfirst.frc4904.standard.commands.chassis; +// package org.usfirst.frc4904.standard.commands.chassis; -import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.custom.sensors.CustomEncoder; -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; +// import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.custom.sensors.CustomEncoder; +// import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; +// import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; -public class ChassisMinimumDistance extends ChassisConstant { - protected CustomEncoder[] encoders; - protected final ChassisConstant fallbackCommand; - protected double distance; - protected double[] initialDistances; +// public class ChassisMinimumDistance extends ChassisConstant { +// protected CustomEncoder[] encoders; +// protected final ChassisConstant fallbackCommand; +// protected double distance; +// protected double[] initialDistances; - /** - * Constructor. This command moves the chassis forward a known distance via a - * set of encoders. The distance is calculated as the average of the provided - * encoders. Control is purely bang (as opposed to bang-bang/pid). The chassis - * will move at the speed until it passes the point, then will stop. - * - * @param name - * @param chassis - * @param distance distance to move in encoder ticks - * @param speed the speed to move at - * @param fallbackCommand If the sensor fails for some reason, this command will - * be cancelled, then the fallbackCommand will start - * @param encoders - */ - public ChassisMinimumDistance(String name, Chassis chassis, double distance, double speed, - ChassisConstant fallbackCommand, CustomEncoder... encoders) { - super(name, chassis, 0.0, speed, 0.0, Double.MAX_VALUE); - this.encoders = encoders; - this.distance = distance; - this.fallbackCommand = fallbackCommand; - initialDistances = new double[encoders.length]; - } +// /** +// * Constructor. This command moves the chassis forward a known distance via a +// * set of encoders. The distance is calculated as the average of the provided +// * encoders. Control is purely bang (as opposed to bang-bang/pid). The chassis +// * will move at the speed until it passes the point, then will stop. +// * +// * @param name +// * @param chassis +// * @param distance distance to move in encoder ticks +// * @param speed the speed to move at +// * @param fallbackCommand If the sensor fails for some reason, this command will +// * be cancelled, then the fallbackCommand will start +// * @param encoders +// */ +// public ChassisMinimumDistance(String name, Chassis chassis, double distance, double speed, +// ChassisConstant fallbackCommand, CustomEncoder... encoders) { +// super(name, chassis, 0.0, speed, 0.0, Double.MAX_VALUE); +// this.encoders = encoders; +// this.distance = distance; +// this.fallbackCommand = fallbackCommand; +// initialDistances = new double[encoders.length]; +// } - /** - * Constructor. This command moves the chassis forward a known distance via a - * set of encoders. The distance is calculated as the average of the provided - * encoders. Control is purely bang (as opposed to bang-bang/pid). The chassis - * will move at the speed until it passes the point, then will stop. - * - * @param chassis - * @param distance distance to move in encoder ticks - * @param speed the speed to move at - * @param fallbackCommand If the sensor fails for some reason, this command will - * be cancelled, then the fallbackCommand will start - * @param encoders - */ - public ChassisMinimumDistance(Chassis chassis, double distance, double speed, ChassisConstant fallbackCommand, - CustomEncoder... encoders) { - this("Chassis Minimum Distance", chassis, distance, speed, fallbackCommand, encoders); - } +// /** +// * Constructor. This command moves the chassis forward a known distance via a +// * set of encoders. The distance is calculated as the average of the provided +// * encoders. Control is purely bang (as opposed to bang-bang/pid). The chassis +// * will move at the speed until it passes the point, then will stop. +// * +// * @param chassis +// * @param distance distance to move in encoder ticks +// * @param speed the speed to move at +// * @param fallbackCommand If the sensor fails for some reason, this command will +// * be cancelled, then the fallbackCommand will start +// * @param encoders +// */ +// public ChassisMinimumDistance(Chassis chassis, double distance, double speed, ChassisConstant fallbackCommand, +// CustomEncoder... encoders) { +// this("Chassis Minimum Distance", chassis, distance, speed, fallbackCommand, encoders); +// } - /** - * Constructor. This command moves the chassis forward a known distance via a - * set of encoders. The distance is calculated as the average of the provided - * encoders. Control is purely bang (as opposed to bang-bang/pid). The chassis - * will move at the speed until it passes the point, then will stop. - * - * @param name - * @param chassis - * @param distance distance to move in encoder ticks - * @param speed the speed to move at - * @param encoders - */ - public ChassisMinimumDistance(String name, Chassis chassis, double distance, double speed, - CustomEncoder... encoders) { - this(name, chassis, distance, speed, null, encoders); - } +// /** +// * Constructor. This command moves the chassis forward a known distance via a +// * set of encoders. The distance is calculated as the average of the provided +// * encoders. Control is purely bang (as opposed to bang-bang/pid). The chassis +// * will move at the speed until it passes the point, then will stop. +// * +// * @param name +// * @param chassis +// * @param distance distance to move in encoder ticks +// * @param speed the speed to move at +// * @param encoders +// */ +// public ChassisMinimumDistance(String name, Chassis chassis, double distance, double speed, +// CustomEncoder... encoders) { +// this(name, chassis, distance, speed, null, encoders); +// } - /** - * Constructor. This command moves the chassis forward a known distance via a - * set of encoders. The distance is calculated as the average of the provided - * encoders. Control is purely bang (as opposed to bang-bang/pid). The chassis - * will move at the speed until it passes the point, then will stop. - * - * @param chassis - * @param distance distance to move in encoder ticks - * @param speed the speed to move at - * @param encoders - */ - public ChassisMinimumDistance(Chassis chassis, double distance, double speed, CustomEncoder... encoders) { - this(chassis, distance, speed, null, encoders); - } +// /** +// * Constructor. This command moves the chassis forward a known distance via a +// * set of encoders. The distance is calculated as the average of the provided +// * encoders. Control is purely bang (as opposed to bang-bang/pid). The chassis +// * will move at the speed until it passes the point, then will stop. +// * +// * @param chassis +// * @param distance distance to move in encoder ticks +// * @param speed the speed to move at +// * @param encoders +// */ +// public ChassisMinimumDistance(Chassis chassis, double distance, double speed, CustomEncoder... encoders) { +// this(chassis, distance, speed, null, encoders); +// } - @Override - public void initialize() { - super.initialize(); - for (int i = 0; i < encoders.length; i++) { - try { - initialDistances[i] = encoders[i].getDistanceSafely(); - } catch (InvalidSensorException e) { - cancel(); - return; - } - } - } +// @Override +// public void initialize() { +// super.initialize(); +// for (int i = 0; i < encoders.length; i++) { +// try { +// initialDistances[i] = encoders[i].getDistanceSafely(); +// } catch (InvalidSensorException e) { +// cancel(); +// return; +// } +// } +// } - @Override - public boolean isFinished() { - double distanceSum = 0; - for (int i = 0; i < encoders.length; i++) { - try { - distanceSum += encoders[i].getDistanceSafely() - initialDistances[i]; - LogKitten.d("Encoder " + i + " reads " + encoders[i].getDistanceSafely() + " out of " + distance); - } catch (InvalidSensorException e) { - cancel(); - if (fallbackCommand != null) { - fallbackCommand.schedule(); - } - return true; - } - } +// @Override +// public boolean isFinished() { +// double distanceSum = 0; +// for (int i = 0; i < encoders.length; i++) { +// try { +// distanceSum += encoders[i].getDistanceSafely() - initialDistances[i]; +// LogKitten.d("Encoder " + i + " reads " + encoders[i].getDistanceSafely() + " out of " + distance); +// } catch (InvalidSensorException e) { +// cancel(); +// if (fallbackCommand != null) { +// fallbackCommand.schedule(); +// } +// return true; +// } +// } - return distanceSum / (double) encoders.length >= distance; - } +// return distanceSum / (double) encoders.length >= distance; +// } - @Override - public void end(boolean interrupted) { - super.end(interrupted); - if (interrupted) { - LogKitten.w("Interrupted! " + getName() + " is in undefined location."); - } else { - LogKitten.v("Finished traveling " + distance + " units (as set by setDistancePerPulse)"); - } - } -} \ No newline at end of file +// @Override +// public void end(boolean interrupted) { +// super.end(interrupted); +// if (interrupted) { +// LogKitten.w("Interrupted! " + getName() + " is in undefined location."); +// } else { +// LogKitten.v("Finished traveling " + distance + " units (as set by setDistancePerPulse)"); +// } +// } +// } \ No newline at end of file diff --git a/commands/chassis/ChassisMove.java b/commands/chassis/ChassisMove.java index f1f3b208..080bf2a5 100644 --- a/commands/chassis/ChassisMove.java +++ b/commands/chassis/ChassisMove.java @@ -1,157 +1,155 @@ -// THIS FILE IS TESTED post wpilibj2 +// package org.usfirst.frc4904.standard.commands.chassis; -package org.usfirst.frc4904.standard.commands.chassis; +// import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.commands.motor.MotorSet; +// import org.usfirst.frc4904.standard.custom.ChassisController; +// import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; +// import org.usfirst.frc4904.standard.subsystems.motor.Motor; +// import org.usfirst.frc4904.standard.subsystems.motor.VelocitySensorMotor; +// import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.commands.motor.MotorSet; -import org.usfirst.frc4904.standard.custom.ChassisController; -import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; -import org.usfirst.frc4904.standard.subsystems.motor.Motor; -import org.usfirst.frc4904.standard.subsystems.motor.VelocitySensorMotor; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +// /** +// * +// * This command moves the chassis. It move based on the driver class. Note that +// * it supports all types of chassis. The chassis is used to calculate the motor +// * movement. The command works by creating a movement command for each motor. +// * This is the best way to handle this because it allows each motor to be a full +// * subsystem. +// */ +// public class ChassisMove extends ParallelCommandGroup { +// protected final MotorSet[] motorSpins; +// protected double[] motorSpeeds; +// protected final Motor[] motors; +// protected final boolean usePID; +// protected final Chassis chassis; +// protected final ChassisController controller; -/** - * - * This command moves the chassis. It move based on the driver class. Note that - * it supports all types of chassis. The chassis is used to calculate the motor - * movement. The command works by creating a movement command for each motor. - * This is the best way to handle this because it allows each motor to be a full - * subsystem. - */ -public class ChassisMove extends ParallelCommandGroup { - protected final MotorSet[] motorSpins; - protected double[] motorSpeeds; - protected final Motor[] motors; - protected final boolean usePID; - protected final Chassis chassis; - protected final ChassisController controller; +// /** +// * +// * @param name +// * @param chassis The robot's Chassis. +// * @param controller A ChassisController to control the Chassis, such as a +// * Driver or autonomous routine. +// * @param usePID Whether to enable PID using any SensorMotors that Chassis +// * has. +// */ +// public ChassisMove(String name, Chassis chassis, ChassisController controller, boolean usePID) { +// super(); +// this.chassis = chassis; +// this.controller = controller; +// this.usePID = usePID; +// addRequirements(chassis); +// setName(name); +// motors = chassis.getMotors(); +// motorSpins = new MotorSet[motors.length]; +// for (int i = 0; i < motors.length; i++) { +// motorSpins[i] = new MotorSet(motors[i].getName(), motors[i]); +// } +// addCommands(motorSpins); +// } - /** - * - * @param name - * @param chassis The robot's Chassis. - * @param controller A ChassisController to control the Chassis, such as a - * Driver or autonomous routine. - * @param usePID Whether to enable PID using any SensorMotors that Chassis - * has. - */ - public ChassisMove(String name, Chassis chassis, ChassisController controller, boolean usePID) { - super(); - this.chassis = chassis; - this.controller = controller; - this.usePID = usePID; - addRequirements(chassis); - setName(name); - motors = chassis.getMotors(); - motorSpins = new MotorSet[motors.length]; - for (int i = 0; i < motors.length; i++) { - motorSpins[i] = new MotorSet(motors[i].getName(), motors[i]); - } - addCommands(motorSpins); - } +// /** +// * +// * @param name +// * @param chassis The robot's chassis. +// * @param controller A ChassisController to control the Chassis, such as a +// * Driver or autonomous routine. +// */ +// public ChassisMove(String name, Chassis chassis, ChassisController controller) { +// this(name, chassis, controller, false); +// } - /** - * - * @param name - * @param chassis The robot's chassis. - * @param controller A ChassisController to control the Chassis, such as a - * Driver or autonomous routine. - */ - public ChassisMove(String name, Chassis chassis, ChassisController controller) { - this(name, chassis, controller, false); - } +// /** +// * +// * @param chassis The robot's Chassis. +// * @param controller A ChassisController to control the Chassis, such as a +// * Driver or autonomous routine. +// * @param usePID Whether to enable PID using any SensorMotors that Chassis +// * has. +// */ +// public ChassisMove(Chassis chassis, ChassisController controller, boolean usePID) { +// this("Chassis Move", chassis, controller, usePID); +// } - /** - * - * @param chassis The robot's Chassis. - * @param controller A ChassisController to control the Chassis, such as a - * Driver or autonomous routine. - * @param usePID Whether to enable PID using any SensorMotors that Chassis - * has. - */ - public ChassisMove(Chassis chassis, ChassisController controller, boolean usePID) { - this("Chassis Move", chassis, controller, usePID); - } +// /** +// * +// * +// * @param chassis The robot's chassis. +// * @param controller A ChassisController to control the Chassis, such as a +// * Driver or autonomous routine. +// * +// */ +// public ChassisMove(Chassis chassis, ChassisController controller) { +// this("Chassis Move", chassis, controller); +// } - /** - * - * - * @param chassis The robot's chassis. - * @param controller A ChassisController to control the Chassis, such as a - * Driver or autonomous routine. - * - */ - public ChassisMove(Chassis chassis, ChassisController controller) { - this("Chassis Move", chassis, controller); - } +// /** +// * +// * VelocitySensorMotors will attempt to very precisely achieve the speed set by +// * this command when PID is enabled PositionSensorMotors will either attempt to +// * maintain their previous position, or worse, will try to move to somewhere +// * between -1.0 and 1.0, which is probably not the correct position regardless +// * of the scaling. +// * +// */ +// @Override +// public void initialize() { +// super.initialize(); +// for (Motor motor : motors) { +// if (motor instanceof VelocitySensorMotor) { +// if (usePID) { +// ((VelocitySensorMotor) motor).enableMotionController(); +// } else { +// ((VelocitySensorMotor) motor).disableMotionController(); +// } +// } +// } +// LogKitten.v("ChassisMove initialized"); +// } - /** - * - * VelocitySensorMotors will attempt to very precisely achieve the speed set by - * this command when PID is enabled PositionSensorMotors will either attempt to - * maintain their previous position, or worse, will try to move to somewhere - * between -1.0 and 1.0, which is probably not the correct position regardless - * of the scaling. - * - */ - @Override - public void initialize() { - super.initialize(); - for (Motor motor : motors) { - if (motor instanceof VelocitySensorMotor) { - if (usePID) { - ((VelocitySensorMotor) motor).enableMotionController(); - } else { - ((VelocitySensorMotor) motor).disableMotionController(); - } - } - } - LogKitten.v("ChassisMove initialized"); - } +// @Override +// public void execute() { +// super.execute(); +// chassis.moveCartesian(controller.getX(), controller.getY(), controller.getTurnSpeed()); +// motorSpeeds = chassis.getMotorSpeeds(); +// StringBuilder motorSpeedsString = new StringBuilder(); +// motorSpeedsString.append("Motor speeds:"); +// for (int i = 0; i < motorSpins.length; i++) { +// LogKitten.d(Double.toString(motorSpeeds[i])); +// motorSpins[i].set(motorSpeeds[i]); +// motorSpeedsString.append(' '); +// motorSpeedsString.append(motorSpeeds[i]); +// } +// LogKitten.d("ChassisMove executing"); +// LogKitten.d(motorSpeedsString.toString()); +// } - @Override - public void execute() { - super.execute(); - chassis.moveCartesian(controller.getX(), controller.getY(), controller.getTurnSpeed()); - motorSpeeds = chassis.getMotorSpeeds(); - StringBuilder motorSpeedsString = new StringBuilder(); - motorSpeedsString.append("Motor speeds:"); - for (int i = 0; i < motorSpins.length; i++) { - LogKitten.d(Double.toString(motorSpeeds[i])); - motorSpins[i].set(motorSpeeds[i]); - motorSpeedsString.append(' '); - motorSpeedsString.append(motorSpeeds[i]); - } - LogKitten.d("ChassisMove executing"); - LogKitten.d(motorSpeedsString.toString()); - } +// @Override +// public boolean isFinished() { +// return false; +// } - @Override - public boolean isFinished() { - return false; - } +// /** +// * It's important to stop motor spins before the ChassisMove command stops. +// * Otherwise, the spins might briefly (for one tick) use the previously set +// * values if the ChassisMove command is reused. +// */ +// protected void stopMotorSpins() { +// for (int i = 0; i < motors.length; i++) { +// motorSpins[i].set(0); +// } +// } - /** - * It's important to stop motor spins before the ChassisMove command stops. - * Otherwise, the spins might briefly (for one tick) use the previously set - * values if the ChassisMove command is reused. - */ - protected void stopMotorSpins() { - for (int i = 0; i < motors.length; i++) { - motorSpins[i].set(0); - } - } +// @Override +// public void end(boolean interrupted) { +// super.end(interrupted); +// stopMotorSpins(); - @Override - public void end(boolean interrupted) { - super.end(interrupted); - stopMotorSpins(); +// if (interrupted) { +// LogKitten.w("ChassisMove interrupted"); +// } else { +// LogKitten.v("ChassisMove ended"); +// } - if (interrupted) { - LogKitten.w("ChassisMove interrupted"); - } else { - LogKitten.v("ChassisMove ended"); - } - - } -} +// } +// } diff --git a/commands/chassis/ChassisMoveDistance.java b/commands/chassis/ChassisMoveDistance.java index c09f1832..77dce660 100644 --- a/commands/chassis/ChassisMoveDistance.java +++ b/commands/chassis/ChassisMoveDistance.java @@ -1,153 +1,153 @@ -package org.usfirst.frc4904.standard.commands.chassis; +// package org.usfirst.frc4904.standard.commands.chassis; -import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.custom.ChassisController; -import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; +// import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.custom.ChassisController; +// import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; +// import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; +// import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.CommandBase; -public class ChassisMoveDistance extends CommandBase implements ChassisController { - protected final ChassisMove chassisMove; - protected final MotionController motionController; - protected final Command fallbackCommand; - protected final double distance; - protected boolean runOnce; +// public class ChassisMoveDistance extends CommandBase implements ChassisController { +// protected final ChassisMove chassisMove; +// protected final MotionController motionController; +// protected final Command fallbackCommand; +// protected final double distance; +// protected boolean runOnce; - /** - * Constructor. This command moves the chassis forward a known distance via a - * set of encoders. The distance is calculated as the average of the provided - * encoders. The speed is decided by the provided motionController. - * - * @param name - * @param chassis - * @param distance distance to move in encoder ticks - * @param motionController - * @param fallbackCommand If the sensor fails for some reason, this command - * will be cancelled, then the fallbackCommand will - * start - * @param encoders - */ - public ChassisMoveDistance(String name, Chassis chassis, double distance, MotionController motionController, - Command fallbackCommand) { - chassisMove = new ChassisMove(chassis, this, false); - this.motionController = motionController; - this.distance = distance; - this.fallbackCommand = fallbackCommand; - runOnce = false; - setName(name); - addRequirements(chassis); - } +// /** +// * Constructor. This command moves the chassis forward a known distance via a +// * set of encoders. The distance is calculated as the average of the provided +// * encoders. The speed is decided by the provided motionController. +// * +// * @param name +// * @param chassis +// * @param distance distance to move in encoder ticks +// * @param motionController +// * @param fallbackCommand If the sensor fails for some reason, this command +// * will be cancelled, then the fallbackCommand will +// * start +// * @param encoders +// */ +// public ChassisMoveDistance(String name, Chassis chassis, double distance, MotionController motionController, +// Command fallbackCommand) { +// chassisMove = new ChassisMove(chassis, this, false); +// this.motionController = motionController; +// this.distance = distance; +// this.fallbackCommand = fallbackCommand; +// runOnce = false; +// setName(name); +// addRequirements(chassis); +// } - /** - * - * Constructor. This command moves the chassis forward a known distance via a - * set of encoders. The distance is calculated as the average of the provided - * encoders. The speed is decided by the provided motionController. - * - * @param chassis - * @param distance distance to move in encoder ticks - * @param motionController - * @param encoders - * @param name - */ - public ChassisMoveDistance(String name, Chassis chassis, double distance, MotionController motionController) { - this(name, chassis, distance, motionController, null); - } +// /** +// * +// * Constructor. This command moves the chassis forward a known distance via a +// * set of encoders. The distance is calculated as the average of the provided +// * encoders. The speed is decided by the provided motionController. +// * +// * @param chassis +// * @param distance distance to move in encoder ticks +// * @param motionController +// * @param encoders +// * @param name +// */ +// public ChassisMoveDistance(String name, Chassis chassis, double distance, MotionController motionController) { +// this(name, chassis, distance, motionController, null); +// } - /** - * - * Constructor. This command moves the chassis forward a known distance via a - * set of encoders. The distance is calculated as the average of the provided - * encoders. The speed is decided by the provided motionController. - * - * @param chassis - * @param distance distance to move in encoder ticks - * @param motionController - * @param encoders - */ - public ChassisMoveDistance(Chassis chassis, double distance, MotionController motionController) { - this("ChassisMoveDistance", chassis, distance, motionController); - } +// /** +// * +// * Constructor. This command moves the chassis forward a known distance via a +// * set of encoders. The distance is calculated as the average of the provided +// * encoders. The speed is decided by the provided motionController. +// * +// * @param chassis +// * @param distance distance to move in encoder ticks +// * @param motionController +// * @param encoders +// */ +// public ChassisMoveDistance(Chassis chassis, double distance, MotionController motionController) { +// this("ChassisMoveDistance", chassis, distance, motionController); +// } - /** - * Constructor. This command moves the chassis forward a known distance via a - * set of encoders. The distance is calculated as the average of the provided - * encoders. The speed is decided by the provided motionController. - * - * @param chassis - * @param distance distance to move in encoder ticks - * @param motionController - * @param fallbackCommand If the sensor fails for some reason, this command - * will be cancelled, then the fallbackCommand will - * start - * @param encoders - */ - public ChassisMoveDistance(Chassis chassis, double distance, MotionController motionController, - Command fallbackCommand) { - this("ChassisMoveDistance", chassis, distance, motionController, fallbackCommand); - } +// /** +// * Constructor. This command moves the chassis forward a known distance via a +// * set of encoders. The distance is calculated as the average of the provided +// * encoders. The speed is decided by the provided motionController. +// * +// * @param chassis +// * @param distance distance to move in encoder ticks +// * @param motionController +// * @param fallbackCommand If the sensor fails for some reason, this command +// * will be cancelled, then the fallbackCommand will +// * start +// * @param encoders +// */ +// public ChassisMoveDistance(Chassis chassis, double distance, MotionController motionController, +// Command fallbackCommand) { +// this("ChassisMoveDistance", chassis, distance, motionController, fallbackCommand); +// } - @Override - public void initialize() { - chassisMove.schedule(); - try { - motionController.resetSafely(); - } catch (InvalidSensorException e) { - LogKitten.w("Cancelling ChassisMoveDistance with InvalidSensorException"); - chassisMove.cancel(); - cancel(); - if (fallbackCommand != null) { - fallbackCommand.schedule(); - } - return; - } - motionController.setSetpoint(motionController.getSensorValue() + distance); - motionController.enable(); - } +// @Override +// public void initialize() { +// chassisMove.schedule(); +// try { +// motionController.resetSafely(); +// } catch (InvalidSensorException e) { +// LogKitten.w("Cancelling ChassisMoveDistance with InvalidSensorException"); +// chassisMove.cancel(); +// cancel(); +// if (fallbackCommand != null) { +// fallbackCommand.schedule(); +// } +// return; +// } +// motionController.setSetpoint(motionController.getSensorValue() + distance); +// motionController.enable(); +// } - @Override - public double getX() { - return 0; - } +// @Override +// public double getX() { +// return 0; +// } - @Override - public double getY() { - double speed; - try { - speed = motionController.getSafely(); - } catch (InvalidSensorException e) { - LogKitten.w("Cancelling ChassisMoveDistance with InvalidSensorException"); - chassisMove.cancel(); - cancel(); - if (fallbackCommand != null) { - fallbackCommand.schedule(); - } - speed = 0; - } - LogKitten.d("MotionProfileSpeed: " + speed); - return speed; - } +// @Override +// public double getY() { +// double speed; +// try { +// speed = motionController.getSafely(); +// } catch (InvalidSensorException e) { +// LogKitten.w("Cancelling ChassisMoveDistance with InvalidSensorException"); +// chassisMove.cancel(); +// cancel(); +// if (fallbackCommand != null) { +// fallbackCommand.schedule(); +// } +// speed = 0; +// } +// LogKitten.d("MotionProfileSpeed: " + speed); +// return speed; +// } - @Override - public double getTurnSpeed() { - return 0; - } +// @Override +// public double getTurnSpeed() { +// return 0; +// } - @Override - public void end(boolean interrupted) { - chassisMove.cancel(); - motionController.disable(); - motionController.reset(); - runOnce = false; - } +// @Override +// public void end(boolean interrupted) { +// chassisMove.cancel(); +// motionController.disable(); +// motionController.reset(); +// runOnce = false; +// } - @Override - public boolean isFinished() { - if (chassisMove.isScheduled() && !runOnce) { - runOnce = true; - } - return (motionController.onTarget() || !chassisMove.isScheduled()) && runOnce; - } -} +// @Override +// public boolean isFinished() { +// if (chassisMove.isScheduled() && !runOnce) { +// runOnce = true; +// } +// return (motionController.onTarget() || !chassisMove.isScheduled()) && runOnce; +// } +// } diff --git a/commands/chassis/ChassisTurn.java b/commands/chassis/ChassisTurn.java index 1c77590d..99870bcb 100644 --- a/commands/chassis/ChassisTurn.java +++ b/commands/chassis/ChassisTurn.java @@ -1,156 +1,156 @@ -package org.usfirst.frc4904.standard.commands.chassis; +// package org.usfirst.frc4904.standard.commands.chassis; -import org.usfirst.frc4904.standard.custom.ChassisController; -import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; -import org.usfirst.frc4904.standard.custom.sensors.IMU; -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; -import edu.wpi.first.wpilibj2.command.CommandBase; +// import org.usfirst.frc4904.standard.custom.ChassisController; +// import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; +// import org.usfirst.frc4904.standard.custom.sensors.IMU; +// import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; +// import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; +// import edu.wpi.first.wpilibj2.command.CommandBase; -public class ChassisTurn extends CommandBase implements ChassisController { - protected final ChassisMove move; - protected double initialAngle; - protected final double finalAngle; - protected final MotionController motionController; - protected final CommandBase fallbackCommand; - protected final IMU imu; - protected boolean runOnce; +// public class ChassisTurn extends CommandBase implements ChassisController { +// protected final ChassisMove move; +// protected double initialAngle; +// protected final double finalAngle; +// protected final MotionController motionController; +// protected final CommandBase fallbackCommand; +// protected final IMU imu; +// protected boolean runOnce; - /** - * - * Constructor This command rotates the chassis to a position relative to the - * current angle of the robot - * - * @param name - * @param chassis - * @param finalAngle - * @param imu - * @param fallbackCommand If the sensor fails for some reason, this command - * will be cancelled, then the fallbackCommand will - * start - * @param motionController - */ - public ChassisTurn(String name, final Chassis chassis, final double finalAngle, final IMU imu, - final CommandBase fallbackCommand, MotionController motionController) { - move = new ChassisMove(chassis, this); - this.finalAngle = -((finalAngle + 360) % 360 - 180); - this.imu = imu; - this.motionController = motionController; - this.fallbackCommand = fallbackCommand; - setName(name); - } +// /** +// * +// * Constructor This command rotates the chassis to a position relative to the +// * current angle of the robot +// * +// * @param name +// * @param chassis +// * @param finalAngle +// * @param imu +// * @param fallbackCommand If the sensor fails for some reason, this command +// * will be cancelled, then the fallbackCommand will +// * start +// * @param motionController +// */ +// public ChassisTurn(String name, final Chassis chassis, final double finalAngle, final IMU imu, +// final CommandBase fallbackCommand, MotionController motionController) { +// move = new ChassisMove(chassis, this); +// this.finalAngle = -((finalAngle + 360) % 360 - 180); +// this.imu = imu; +// this.motionController = motionController; +// this.fallbackCommand = fallbackCommand; +// setName(name); +// } - /** - * - * Constructor This command rotates the chassis to a position relative to the - * current angle of the robot - * - * @param name - * @param chassis - * @param finalAngle - * @param imu - * @param motionController - */ - public ChassisTurn(String name, Chassis chassis, double finalAngle, IMU imu, MotionController motionController) { - this(name, chassis, finalAngle, imu, null, motionController); - } +// /** +// * +// * Constructor This command rotates the chassis to a position relative to the +// * current angle of the robot +// * +// * @param name +// * @param chassis +// * @param finalAngle +// * @param imu +// * @param motionController +// */ +// public ChassisTurn(String name, Chassis chassis, double finalAngle, IMU imu, MotionController motionController) { +// this(name, chassis, finalAngle, imu, null, motionController); +// } - /** - * - * Constructor This command rotates the chassis to a position relative to the - * current angle of the robot - * - * @param chassis - * @param finalAngle - * @param imu - * @param motionController - */ - public ChassisTurn(Chassis chassis, double finalAngle, IMU imu, MotionController motionController) { - this("Chassis Turn", chassis, finalAngle, imu, null, motionController); - } +// /** +// * +// * Constructor This command rotates the chassis to a position relative to the +// * current angle of the robot +// * +// * @param chassis +// * @param finalAngle +// * @param imu +// * @param motionController +// */ +// public ChassisTurn(Chassis chassis, double finalAngle, IMU imu, MotionController motionController) { +// this("Chassis Turn", chassis, finalAngle, imu, null, motionController); +// } - /** - * - * Constructor This command rotates the chassis to a position relative to the - * current angle of the robot - * - * @param chassis - * @param finalAngle - * @param imu - * @param fallbackCommand If the sensor fails for some reason, this command - * will be cancelled, then the fallbackCommand will - * start - * @param motionController - */ - public ChassisTurn(Chassis chassis, double finalAngle, IMU imu, CommandBase fallbackCommand, - MotionController motionController) { - this("Chassis Turn", chassis, finalAngle, imu, fallbackCommand, motionController); - } +// /** +// * +// * Constructor This command rotates the chassis to a position relative to the +// * current angle of the robot +// * +// * @param chassis +// * @param finalAngle +// * @param imu +// * @param fallbackCommand If the sensor fails for some reason, this command +// * will be cancelled, then the fallbackCommand will +// * start +// * @param motionController +// */ +// public ChassisTurn(Chassis chassis, double finalAngle, IMU imu, CommandBase fallbackCommand, +// MotionController motionController) { +// this("Chassis Turn", chassis, finalAngle, imu, fallbackCommand, motionController); +// } - @Override - public double getX() { - return 0.0; - } +// @Override +// public double getX() { +// return 0.0; +// } - @Override - public double getY() { - return 0.0; - } +// @Override +// public double getY() { +// return 0.0; +// } - @Override - public double getTurnSpeed() { - try { - return motionController.getSafely(); - } catch (final InvalidSensorException e) { - move.cancel(); - cancel(); - if (fallbackCommand != null) { - fallbackCommand.schedule(); - } - return 0; - } - } +// @Override +// public double getTurnSpeed() { +// try { +// return motionController.getSafely(); +// } catch (final InvalidSensorException e) { +// move.cancel(); +// cancel(); +// if (fallbackCommand != null) { +// fallbackCommand.schedule(); +// } +// return 0; +// } +// } - @Override - public void initialize() { - move.schedule(); - initialAngle = imu.getYaw(); - try { - motionController.resetSafely(); - } catch (final InvalidSensorException e) { - move.cancel(); - cancel(); - if (fallbackCommand != null) { - fallbackCommand.schedule(); - } - return; - } - } +// @Override +// public void initialize() { +// move.schedule(); +// initialAngle = imu.getYaw(); +// try { +// motionController.resetSafely(); +// } catch (final InvalidSensorException e) { +// move.cancel(); +// cancel(); +// if (fallbackCommand != null) { +// fallbackCommand.schedule(); +// } +// return; +// } +// } - @Override - public void execute() { - motionController.setSetpoint(((finalAngle + initialAngle) + 360) % 360 - 180); - if (!motionController.isEnabled()) { - motionController.enable(); - } - } +// @Override +// public void execute() { +// motionController.setSetpoint(((finalAngle + initialAngle) + 360) % 360 - 180); +// if (!motionController.isEnabled()) { +// motionController.enable(); +// } +// } - @Override - public boolean isFinished() { - if (move.isScheduled() && !runOnce) { - runOnce = true; - } - return (motionController.onTarget() || !move.isScheduled()) && runOnce; - } +// @Override +// public boolean isFinished() { +// if (move.isScheduled() && !runOnce) { +// runOnce = true; +// } +// return (motionController.onTarget() || !move.isScheduled()) && runOnce; +// } - @Override - public void end(boolean interrupted) { - motionController.disable(); - move.cancel(); +// @Override +// public void end(boolean interrupted) { +// motionController.disable(); +// move.cancel(); - if (fallbackCommand != null && fallbackCommand.isScheduled()) { - fallbackCommand.cancel(); - } - runOnce = false; - } -} \ No newline at end of file +// if (fallbackCommand != null && fallbackCommand.isScheduled()) { +// fallbackCommand.cancel(); +// } +// runOnce = false; +// } +// } \ No newline at end of file diff --git a/commands/chassis/ChassisTurnAbsolute.java b/commands/chassis/ChassisTurnAbsolute.java index 6431be57..489dcfe5 100644 --- a/commands/chassis/ChassisTurnAbsolute.java +++ b/commands/chassis/ChassisTurnAbsolute.java @@ -1,80 +1,80 @@ -package org.usfirst.frc4904.standard.commands.chassis; +// package org.usfirst.frc4904.standard.commands.chassis; -import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; -import org.usfirst.frc4904.standard.custom.sensors.IMU; -import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; -import edu.wpi.first.wpilibj2.command.CommandBase; +// import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; +// import org.usfirst.frc4904.standard.custom.sensors.IMU; +// import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; +// import edu.wpi.first.wpilibj2.command.CommandBase; -public class ChassisTurnAbsolute extends ChassisTurn { - /** - * Constructor This command rotates the chassis to a position relative to the - * starting point of the robot (e.g. the position where the imu was last reset). - * - * @param chassis - * @param finalAngle - * @param imu - * @param motionController - */ - public ChassisTurnAbsolute(Chassis chassis, double finalAngle, IMU imu, MotionController motionController) { - super("Chassis Turn Absolute", chassis, finalAngle, imu, motionController); - } +// public class ChassisTurnAbsolute extends ChassisTurn { +// /** +// * Constructor This command rotates the chassis to a position relative to the +// * starting point of the robot (e.g. the position where the imu was last reset). +// * +// * @param chassis +// * @param finalAngle +// * @param imu +// * @param motionController +// */ +// public ChassisTurnAbsolute(Chassis chassis, double finalAngle, IMU imu, MotionController motionController) { +// super("Chassis Turn Absolute", chassis, finalAngle, imu, motionController); +// } - /** - * Constructor This command rotates the chassis to a position relative to the - * starting point of the robot (e.g. the position where the imu was last reset). - * - * @param chassis - * @param finalAngle - * @param imu - * @param fallbackCommand If the sensor fails for some reason, this command - * will be cancelled, then the fallbackCommand will - * start - * @param motionController - */ - public ChassisTurnAbsolute(Chassis chassis, double finalAngle, IMU imu, CommandBase fallbackCommand, - MotionController motionController) { - super("Chassis Turn Absolute", chassis, finalAngle, imu, fallbackCommand, motionController); - } +// /** +// * Constructor This command rotates the chassis to a position relative to the +// * starting point of the robot (e.g. the position where the imu was last reset). +// * +// * @param chassis +// * @param finalAngle +// * @param imu +// * @param fallbackCommand If the sensor fails for some reason, this command +// * will be cancelled, then the fallbackCommand will +// * start +// * @param motionController +// */ +// public ChassisTurnAbsolute(Chassis chassis, double finalAngle, IMU imu, CommandBase fallbackCommand, +// MotionController motionController) { +// super("Chassis Turn Absolute", chassis, finalAngle, imu, fallbackCommand, motionController); +// } - /** - * Constructor This command rotates the chassis to a position relative to the - * starting point of the robot (e.g. the position where the imu was last reset). - * - * @param name - * @param chassis - * @param finalAngle - * @param imu - * @param motionController - */ - public ChassisTurnAbsolute(String name, Chassis chassis, double finalAngle, IMU imu, - MotionController motionController) { - super(name, chassis, finalAngle, imu, motionController); - } +// /** +// * Constructor This command rotates the chassis to a position relative to the +// * starting point of the robot (e.g. the position where the imu was last reset). +// * +// * @param name +// * @param chassis +// * @param finalAngle +// * @param imu +// * @param motionController +// */ +// public ChassisTurnAbsolute(String name, Chassis chassis, double finalAngle, IMU imu, +// MotionController motionController) { +// super(name, chassis, finalAngle, imu, motionController); +// } - /** - * Constructor This command rotates the chassis to a position relative to the - * starting point of the robot (e.g. the position where the imu was last reset). - * - * @param name - * @param chassis - * @param finalAngle - * @param imu - * @param fallbackCommand If the sensor fails for some reason, this command - * will be cancelled, then the fallbackCommand will - * start - * @param motionController - */ - public ChassisTurnAbsolute(String name, Chassis chassis, double finalAngle, IMU imu, CommandBase fallbackCommand, - MotionController motionController) { - super(name, chassis, finalAngle, imu, fallbackCommand, motionController); - } +// /** +// * Constructor This command rotates the chassis to a position relative to the +// * starting point of the robot (e.g. the position where the imu was last reset). +// * +// * @param name +// * @param chassis +// * @param finalAngle +// * @param imu +// * @param fallbackCommand If the sensor fails for some reason, this command +// * will be cancelled, then the fallbackCommand will +// * start +// * @param motionController +// */ +// public ChassisTurnAbsolute(String name, Chassis chassis, double finalAngle, IMU imu, CommandBase fallbackCommand, +// MotionController motionController) { +// super(name, chassis, finalAngle, imu, fallbackCommand, motionController); +// } - @Override - public void initialize() { - // ChassisTurnDegrees measures an initial angle and compensates for it - // to make its turns relative; - super.initialize(); - // Not anymore - super.initialAngle = 0.0; - } -} +// @Override +// public void initialize() { +// // ChassisTurnDegrees measures an initial angle and compensates for it +// // to make its turns relative; +// super.initialize(); +// // Not anymore +// super.initialAngle = 0.0; +// } +// } diff --git a/custom/CustomPIDSourceType.java b/custom/CustomPIDSourceType.java deleted file mode 100644 index 75020e4e..00000000 --- a/custom/CustomPIDSourceType.java +++ /dev/null @@ -1,5 +0,0 @@ -package org.usfirst.frc4904.standard.custom; - -public enum CustomPIDSourceType { - kDisplacement, kRate -} diff --git a/custom/MCChassisController.java b/custom/MCChassisController.java index c3c75f98..249e1cb2 100644 --- a/custom/MCChassisController.java +++ b/custom/MCChassisController.java @@ -20,7 +20,6 @@ public MCChassisController(ChassisController controller, IMU imu, MotionControll this.maxDegreesPerSecond = maxDegreesPerSecond; this.imu = imu; this.imu.reset(); - this.imu.setCustomPIDSourceType(CustomPIDSourceType.kDisplacement); this.motionController = motionController; motionController.setInputRange(-180.0f, 180.0f); motionController.setOutputRange(-1.0f, 1.0f); diff --git a/custom/controllers/CustomCommandJoystick.java b/custom/controllers/CustomCommandJoystick.java index dbf068ef..bdbed8c3 100644 --- a/custom/controllers/CustomCommandJoystick.java +++ b/custom/controllers/CustomCommandJoystick.java @@ -1,5 +1,3 @@ -// THIS FILE IS TESTED post wpilibj2 - package org.usfirst.frc4904.standard.custom.controllers; import edu.wpi.first.wpilibj.DriverStation; diff --git a/custom/motioncontrollers/MotionController.java b/custom/motioncontrollers/MotionController.java index 4cb33a4f..fb001d43 100644 --- a/custom/motioncontrollers/MotionController.java +++ b/custom/motioncontrollers/MotionController.java @@ -3,7 +3,6 @@ import java.util.function.DoubleConsumer; import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.custom.sensors.PIDSensor; import edu.wpi.first.hal.util.BoundaryException; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -16,7 +15,6 @@ */ public abstract class MotionController implements Subsystem { protected DoubleConsumer output; - protected final PIDSensor sensor; protected double setpoint; protected double absoluteTolerance; protected boolean continuous; @@ -31,8 +29,7 @@ public abstract class MotionController implements Subsystem { private volatile boolean justReset; private final Object lock = new Object(); - public MotionController(PIDSensor sensor, DoubleConsumer output) { - this.sensor = sensor; + public MotionController(DoubleConsumer output) { setOutput(output); enable = false; overridden = false; diff --git a/custom/motorcontrollers/CANTalonFX.java b/custom/motorcontrollers/CANTalonFX.java index c86e8cfa..9ee74885 100644 --- a/custom/motorcontrollers/CANTalonFX.java +++ b/custom/motorcontrollers/CANTalonFX.java @@ -1,4 +1,4 @@ -package org.usfirst.frc4904.standard.custom.motioncontrollers; +package org.usfirst.frc4904.standard.custom.motorcontrollers; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; diff --git a/custom/motorcontrollers/CANTalonSRX.java b/custom/motorcontrollers/CANTalonSRX.java index 065ff288..17a48dde 100644 --- a/custom/motorcontrollers/CANTalonSRX.java +++ b/custom/motorcontrollers/CANTalonSRX.java @@ -1,6 +1,6 @@ // THIS FILE IS TESTED post wpilibj2 -package org.usfirst.frc4904.standard.custom.motioncontrollers; +package org.usfirst.frc4904.standard.custom.motorcontrollers; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; diff --git a/custom/sensors/CANEncoder.java b/custom/sensors/CANEncoder.java index f58123ab..09c50255 100644 --- a/custom/sensors/CANEncoder.java +++ b/custom/sensors/CANEncoder.java @@ -1,15 +1,13 @@ package org.usfirst.frc4904.standard.custom.sensors; - +// WAS PID SOURCE import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.Util; -import org.usfirst.frc4904.standard.custom.CustomPIDSourceType; /** * Encoder over CAN Implements CustomEncoder generic encoder class * */ public class CANEncoder extends CANSensor implements CustomEncoder { - private CustomPIDSourceType pidSource; private double distancePerPulse; private boolean reverseDirection; /** @@ -24,7 +22,6 @@ public CANEncoder(String name, int id, boolean reverseDirection, double distance this.reverseDirection = reverseDirection; this.distancePerPulse = distancePerPulse; this.offset = 0.0; - setCustomPIDSourceType(CustomPIDSourceType.kDisplacement); } public CANEncoder(String name, int id, boolean reverseDirection) { @@ -80,26 +77,6 @@ public void setReverseDirection(boolean reverseDirection) { this.reverseDirection = reverseDirection; } - @Override - public double pidGetSafely() throws InvalidSensorException { - if (pidSource == CustomPIDSourceType.kDisplacement) { - return getDistance(); - } - return getRate(); - } - - /** - * Returns the raw number of ticks - */ - @Override - public int getSafely() throws InvalidSensorException { - if (reverseDirection) { - return super.readSensor()[0] * -1; - } else { - return super.readSensor()[0]; - } - } - /** * Returns the scaled distance rotated by the encoder. */ @@ -157,27 +134,7 @@ public void resetViaOffset(double setpoint) { public void resetViaOffset() { resetViaOffset(0.0); } - - @Override - public double pidGet() { - try { - return pidGetSafely(); - } catch (Exception e) { - LogKitten.ex(e); - return 0; - } - } - - @Override - public int get() { - try { - return getSafely(); - } catch (Exception e) { - LogKitten.ex(e); - return 0; - } - } - + @Override public double getDistance() { try { @@ -217,14 +174,4 @@ public double getRate() { return 0; } } - - @Override - public void setCustomPIDSourceType(CustomPIDSourceType pidSource) { - this.pidSource = pidSource; - } - - @Override - public CustomPIDSourceType getCustomPIDSourceType() { - return pidSource; - } } diff --git a/custom/sensors/CANTalonEncoder.java b/custom/sensors/CANTalonEncoder.java index fed7f656..315fe415 100644 --- a/custom/sensors/CANTalonEncoder.java +++ b/custom/sensors/CANTalonEncoder.java @@ -1,12 +1,10 @@ -// THIS FILE IS TESTED post wpilibj2 - +// WAS PID SOURCE package org.usfirst.frc4904.standard.custom.sensors; import org.usfirst.frc4904.standard.Util; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.VelocityMeasPeriod; import com.ctre.phoenix.motorcontrol.can.BaseTalon; -import org.usfirst.frc4904.standard.custom.CustomPIDSourceType; /** * Encoder class for the Built-in Encoders on Talon Motor Controllers @@ -20,36 +18,27 @@ public class CANTalonEncoder implements CustomEncoder { protected static final double DECI_SECONDS_TO_SECONDS = 10.0; // getSpeed must be converted from ticks per 100ms to // ticks per second, so *10. protected static final int PID_IDX = 1; - protected static final CustomPIDSourceType DEFAULT_CUSTOM_PID_SOURCE_TYPE = CustomPIDSourceType.kDisplacement; protected static final FeedbackDevice DEFAULT_FEEDBACK_DEVICE = FeedbackDevice.IntegratedSensor; protected final BaseTalon talon; - protected CustomPIDSourceType pidSource; protected double distancePerPulse; protected boolean reverseDirection; - public CANTalonEncoder(String name, BaseTalon talon, boolean reverseDirection, double distancePerPulse, - CustomPIDSourceType sensorType, FeedbackDevice feedbackDevice, double period) { + public CANTalonEncoder(String name, BaseTalon talon, boolean reverseDirection, double distancePerPulse, FeedbackDevice feedbackDevice, double period) { this.talon = talon; setReverseDirection(reverseDirection); setDistancePerPulse(distancePerPulse); - setCustomPIDSourceType(sensorType); setFeedbackDevice(feedbackDevice); this.talon.configVelocityMeasurementPeriod(VelocityMeasPeriod.valueOf(period)); } public CANTalonEncoder(String name, BaseTalon talon, boolean reverseDirection, double distancePerPulse, - CustomPIDSourceType sensorType, FeedbackDevice feedbackDevice) { - this(name, talon, reverseDirection, distancePerPulse, sensorType, feedbackDevice, DEFAULT_PERIOD); - } - - public CANTalonEncoder(String name, BaseTalon talon, boolean reverseDirection, double distancePerPulse, - CustomPIDSourceType sensorType) { - this(name, talon, reverseDirection, distancePerPulse, DEFAULT_CUSTOM_PID_SOURCE_TYPE, DEFAULT_FEEDBACK_DEVICE); - + FeedbackDevice feedbackDevice) { + this(name, talon, reverseDirection, distancePerPulse, feedbackDevice, DEFAULT_PERIOD); } public CANTalonEncoder(String name, BaseTalon talon, boolean reverseDirection, double distancePerPulse) { - this(name, talon, reverseDirection, distancePerPulse, DEFAULT_CUSTOM_PID_SOURCE_TYPE); + this(name, talon, reverseDirection, distancePerPulse, DEFAULT_FEEDBACK_DEVICE); + } public CANTalonEncoder(String name, BaseTalon talon, boolean reverseDirection) { @@ -80,29 +69,6 @@ public void setFeedbackDevice(FeedbackDevice feedbackDevice) { talon.configSelectedFeedbackSensor(feedbackDevice); } - @Override - public void setCustomPIDSourceType(CustomPIDSourceType pidSource) { - this.pidSource = pidSource; - } - - @Override - public CustomPIDSourceType getCustomPIDSourceType() { - return pidSource; - } - - @Override - public double pidGet() { - if (pidSource == CustomPIDSourceType.kDisplacement) { - return getDistance(); - } - return getRate(); - } - - @Override - public int get() { - return (int) talon.getSelectedSensorPosition(PID_IDX); - } - @Override public double getDistance() { if (reverseDirection) { @@ -164,16 +130,6 @@ public void reset() { talon.setSelectedSensorPosition(0); } - @Override - public double pidGetSafely() { - return pidGet(); - } - - @Override - public int getSafely() { - return get(); - } - @Override public double getDistanceSafely() { return getDistance(); diff --git a/custom/sensors/CustomCANCoder.java b/custom/sensors/CustomCANCoder.java index 28fab05b..b39ba9d3 100644 --- a/custom/sensors/CustomCANCoder.java +++ b/custom/sensors/CustomCANCoder.java @@ -1,5 +1,3 @@ -// THIS FILE IS TESTED post wpilibj2 - package org.usfirst.frc4904.standard.custom.sensors; import com.ctre.phoenix.sensors.CANCoder; @@ -8,41 +6,25 @@ import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.Util; -import org.usfirst.frc4904.standard.custom.CustomPIDSourceType; public class CustomCANCoder extends CANCoder implements CustomEncoder { - protected static final CustomPIDSourceType DEFAULT_SOURCE_TYPE = CustomPIDSourceType.kDisplacement; protected static final boolean DEFAULT_REVERSE_DIRECTION = false; - protected CustomPIDSourceType sensorType; protected double distancePerPulse; protected boolean reverseDirection; protected CANCoderConfiguration config; - public CustomCANCoder(int deviceNum, double distancePerPulse, CustomPIDSourceType sensorType, + public CustomCANCoder(int deviceNum, double distancePerPulse, boolean reverseDirection) { super(deviceNum); config = new CANCoderConfiguration(); super.configAllSettings(config); setDistancePerPulse(distancePerPulse); - setCustomPIDSourceType(sensorType); setReverseDirection(reverseDirection); reset(); } - public CustomCANCoder(int deviceNum, double distancePerPulse, CustomPIDSourceType sensorType) { - this(deviceNum, distancePerPulse, sensorType, DEFAULT_REVERSE_DIRECTION); - } - public CustomCANCoder(int deviceNum, double distancePerPulse) { - this(deviceNum, distancePerPulse, DEFAULT_SOURCE_TYPE); - } - - public CustomPIDSourceType getCustomPIDSourceType() { - return sensorType; - } - - public void setCustomPIDSourceType(CustomPIDSourceType sensorType) { - this.sensorType = sensorType; + this(deviceNum, distancePerPulse, DEFAULT_REVERSE_DIRECTION); } public void setDistancePerPulse(double pulse, SensorTimeBase collectionPeriod) { @@ -54,24 +36,6 @@ public void setDistancePerPulse(double pulse) { setDistancePerPulse(pulse, SensorTimeBase.PerSecond); } - @Override - public double pidGet() { - try { - return pidGetSafely(); - } catch (Exception e) { - LogKitten.ex(e); - return 0; - } - } - - @Override - public double pidGetSafely() throws InvalidSensorException { - if (getCustomPIDSourceType() == CustomPIDSourceType.kDisplacement) { - return getDistance(); - } - return getRate(); - } - @Override public double getRate() { try { @@ -88,24 +52,6 @@ public double getRateSafely() throws InvalidSensorException { } - @Override - public int get() { - try { - return getSafely(); - } catch (Exception e) { - LogKitten.ex(e); - return 0; - } - } - - @Override - public int getSafely() throws InvalidSensorException { - if (getCustomPIDSourceType() == CustomPIDSourceType.kDisplacement) { - return (int) getDistance(); - } - return (int) getRate(); - } - @Override public double getDistance() { try { diff --git a/custom/sensors/CustomDigitalEncoder.java b/custom/sensors/CustomDigitalEncoder.java index 2c7a0cee..b8784f09 100644 --- a/custom/sensors/CustomDigitalEncoder.java +++ b/custom/sensors/CustomDigitalEncoder.java @@ -1,70 +1,59 @@ +// WAS PID SOURCE package org.usfirst.frc4904.standard.custom.sensors; import edu.wpi.first.wpilibj.CounterBase; import edu.wpi.first.wpilibj.DigitalSource; import edu.wpi.first.wpilibj.Encoder; -import org.usfirst.frc4904.standard.custom.CustomPIDSourceType; /** * A RoboRIO encoder that implements the generic encoder class. * */ public class CustomDigitalEncoder extends Encoder implements CustomEncoder { - private CustomPIDSourceType pidSource; private double distancePerPulse; private boolean reverseDirection; public CustomDigitalEncoder(DigitalSource aSource, DigitalSource bSource) { super(aSource, bSource); - setCustomPIDSourceType(CustomPIDSourceType.kDisplacement); } public CustomDigitalEncoder(DigitalSource aSource, DigitalSource bSource, boolean reverseDirection) { super(aSource, bSource, reverseDirection); - setCustomPIDSourceType(CustomPIDSourceType.kDisplacement); } public CustomDigitalEncoder(DigitalSource aSource, DigitalSource bSource, boolean reverseDirection, CounterBase.EncodingType encodingType) { super(aSource, bSource, reverseDirection, encodingType); - setCustomPIDSourceType(CustomPIDSourceType.kDisplacement); } public CustomDigitalEncoder(DigitalSource aSource, DigitalSource bSource, DigitalSource indexSource) { super(aSource, bSource, indexSource); - setCustomPIDSourceType(CustomPIDSourceType.kDisplacement); } public CustomDigitalEncoder(DigitalSource aSource, DigitalSource bSource, DigitalSource indexSource, boolean reverseDirection) { super(aSource, bSource, indexSource, reverseDirection); - setCustomPIDSourceType(CustomPIDSourceType.kDisplacement); } public CustomDigitalEncoder(int aChannel, int bChannel) { super(aChannel, bChannel); - setCustomPIDSourceType(CustomPIDSourceType.kDisplacement); } public CustomDigitalEncoder(int aChannel, int bChannel, boolean reverseDirection) { super(aChannel, bChannel, reverseDirection); - setCustomPIDSourceType(CustomPIDSourceType.kDisplacement); } public CustomDigitalEncoder(int aChannel, int bChannel, boolean reverseDirection, CounterBase.EncodingType encodingType) { super(aChannel, bChannel, reverseDirection, encodingType); - setCustomPIDSourceType(CustomPIDSourceType.kDisplacement); } public CustomDigitalEncoder(int aChannel, int bChannel, int indexChannel) { super(aChannel, bChannel, indexChannel); - setCustomPIDSourceType(CustomPIDSourceType.kDisplacement); } public CustomDigitalEncoder(int aChannel, int bChannel, int indexChannel, boolean reverseDirection) { super(aChannel, bChannel, indexChannel, reverseDirection); - setCustomPIDSourceType(CustomPIDSourceType.kDisplacement); } @Override @@ -89,24 +78,6 @@ public void setReverseDirection(boolean reverseDirection) { this.reverseDirection = reverseDirection; } - @Override - public double pidGet() { - if (pidSource == CustomPIDSourceType.kDisplacement) { - return getDistance(); - } - return getRate(); - } - - @Override - public double pidGetSafely() { - return pidGet(); - } - - @Override - public int getSafely() { - return get(); - } - @Override public double getDistanceSafely() { return getDistance(); @@ -126,14 +97,4 @@ public boolean getStoppedSafely() { public double getRateSafely() { return getRate(); } - - @Override - public void setCustomPIDSourceType(CustomPIDSourceType pidSource) { - this.pidSource = pidSource; - } - - @Override - public CustomPIDSourceType getCustomPIDSourceType() { - return pidSource; - } } \ No newline at end of file diff --git a/custom/sensors/CustomEncoder.java b/custom/sensors/CustomEncoder.java index ae07bfef..fc0b9c64 100644 --- a/custom/sensors/CustomEncoder.java +++ b/custom/sensors/CustomEncoder.java @@ -1,3 +1,5 @@ +// WAS PID SOURCE + package org.usfirst.frc4904.standard.custom.sensors; /** @@ -5,7 +7,7 @@ * CAN encoder. * */ -public interface CustomEncoder extends PIDSensor, NativeDerivativeSensor { +public interface CustomEncoder extends NativeDerivativeSensor { /** * Gets current count * @@ -13,11 +15,6 @@ public interface CustomEncoder extends PIDSensor, NativeDerivativeSensor { */ int get(); - /** - * Gets current count - */ - int getSafely() throws InvalidSensorException; - /** * Gets current distance * diff --git a/custom/sensors/EncoderPair.java b/custom/sensors/EncoderPair.java index 5ef99569..bb19fc37 100644 --- a/custom/sensors/EncoderPair.java +++ b/custom/sensors/EncoderPair.java @@ -1,10 +1,7 @@ -// THIS FILE IS TESTED post wpilibj2 - package org.usfirst.frc4904.standard.custom.sensors; import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.Util; -import org.usfirst.frc4904.standard.custom.CustomPIDSourceType; /** * Amalgamates the data of several encoders for the purpose of controlling a @@ -16,8 +13,7 @@ */ public class EncoderPair implements CustomEncoder { private final CustomEncoder[] encoders; - private final int[] offset; // Do not reset encoders, just store an offset value - private CustomPIDSourceType pidSource; + private final double[] offset; // Do not reset encoders, just store an offset value private boolean reverseDirection; private double distancePerPulse; private final double distanceTolerance; @@ -41,10 +37,9 @@ public class EncoderPair implements CustomEncoder { */ public EncoderPair(CustomEncoder encoder1, CustomEncoder encoder2, double distanceTolerance, double rateTolerance) { encoders = new CustomEncoder[] { encoder1, encoder2 }; - offset = new int[] { 0, 0 }; + offset = new double[] { 0.0, 0.0 }; this.distanceTolerance = distanceTolerance; this.rateTolerance = rateTolerance; - pidSource = CustomPIDSourceType.kDisplacement; reverseDirection = false; } @@ -62,51 +57,6 @@ public EncoderPair(CustomEncoder encoder1, CustomEncoder encoder2) { this(encoder1, encoder2, EncoderPair.DEFAULT_DISTANCE_TOLERANCE, EncoderPair.DEFAULT_RATE_TOLERANCE); } - @Override - public CustomPIDSourceType getCustomPIDSourceType() { - return pidSource; - } - - @Override - public double pidGet() { - if (pidSource == CustomPIDSourceType.kDisplacement) { - return getDistance(); - } - return getRate(); - } - - @Override - public double pidGetSafely() throws InvalidSensorException { - if (pidSource == CustomPIDSourceType.kDisplacement) { - return getDistanceSafely(); - } - return getRateSafely(); - } - - @Override - public void setCustomPIDSourceType(CustomPIDSourceType pidSource) { - if (pidSource != null) { - this.pidSource = pidSource; - } - } - - @Override - public int getSafely() throws InvalidSensorException { - return (int) Math - .round(((double) (encoders[0].getSafely() - offset[0]) + (double) (encoders[1].getSafely() - offset[1])) - / 2.0); - } - - @Override - public int get() { - try { - return getSafely(); - } catch (InvalidSensorException e) { - LogKitten.ex(e); - return 0; - } - } - @Override public double getDistanceSafely() throws InvalidSensorException { return ((encoders[0].getDistanceSafely() - offset[0] * encoders[0].getDistancePerPulse()) @@ -198,8 +148,8 @@ public void setDistancePerPulse(double distancePerPulse) { @Override public void reset() { - offset[0] = encoders[0].get(); - offset[1] = encoders[1].get(); + offset[0] = encoders[0].getDistance(); + offset[1] = encoders[1].getDistance(); } public double getDifference() { @@ -242,40 +192,5 @@ public boolean isInSyncSafely() throws InvalidSensorException { return Math.abs(getDifferenceSafely()) < distanceTolerance && Math.abs(getRateDifferenceSafely()) < rateTolerance; } - - public class EncoderDifference implements PIDSensor { - private CustomPIDSourceType pidSource; // Needs to be seperate in case of multiple threads changing source type - - public EncoderDifference() { - pidSource = CustomPIDSourceType.kDisplacement; - } - - @Override - public void setCustomPIDSourceType(CustomPIDSourceType pidSource) { - this.pidSource = pidSource; - } - - @Override - public CustomPIDSourceType getCustomPIDSourceType() { - return pidSource; - } - - @Override - public double pidGet() { - try { - return pidGetSafely(); - } catch (InvalidSensorException e) { - LogKitten.ex(e); - return 0.0; - } - } - - @Override - public double pidGetSafely() throws InvalidSensorException { - if (pidSource == CustomPIDSourceType.kDisplacement) { - return getDifferenceSafely(); - } - return getRateDifferenceSafely(); - } - } +// WAS PID SOURCE EncoderDifference class here } diff --git a/custom/sensors/IMU.java b/custom/sensors/IMU.java index 06c1cc74..94124ec9 100644 --- a/custom/sensors/IMU.java +++ b/custom/sensors/IMU.java @@ -1,7 +1,5 @@ package org.usfirst.frc4904.standard.custom.sensors; -import org.usfirst.frc4904.standard.custom.CustomPIDSourceType; - public interface IMU { /** * Resets the IMU. @@ -28,6 +26,4 @@ public interface IMU { */ public float getRoll(); - public void setCustomPIDSourceType(CustomPIDSourceType kdisplacement); - } diff --git a/custom/sensors/NativeDerivativeSensor.java b/custom/sensors/NativeDerivativeSensor.java index 623a7342..50d6d97e 100644 --- a/custom/sensors/NativeDerivativeSensor.java +++ b/custom/sensors/NativeDerivativeSensor.java @@ -1,3 +1,4 @@ +// MARK MAYBE DELETE package org.usfirst.frc4904.standard.custom.sensors; /** diff --git a/custom/sensors/NavX.java b/custom/sensors/NavX.java index 172b5fca..33b99631 100644 --- a/custom/sensors/NavX.java +++ b/custom/sensors/NavX.java @@ -1,9 +1,6 @@ -// THIS FILE IS TESTED post wpilibj2 - +// WAS PID SOURCE package org.usfirst.frc4904.standard.custom.sensors; -import org.usfirst.frc4904.standard.custom.CustomPIDSourceType; - import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.SerialPort; @@ -13,10 +10,8 @@ * Local NavX interface. * */ -public class NavX extends AHRS implements IMU, PIDSensor { - - private CustomPIDSourceType pidSource; - protected float lastYaw; +public class NavX extends AHRS implements IMU { + protected float lastYaw; protected float lastPitch; protected float lastRoll; protected double lastYawRate; @@ -42,15 +37,6 @@ public NavX(I2C.Port port) { getYawCalls = 0; } - @Override - public double pidGet() { - if (getCustomPIDSourceType() == CustomPIDSourceType.kRate) { - return getRate(); - } else { - return getYaw(); - } - } - @Override public double getRate() { double rate = super.getRate(); @@ -123,24 +109,5 @@ public void zeroYaw() { super.zeroYaw(); lastYaw = 0; } - - @Override - public double pidGetSafely() throws InvalidSensorException { - if (getCustomPIDSourceType() == CustomPIDSourceType.kRate) { - return getRate(); - } else { - return getYaw(); - } - } - - @Override - public void setCustomPIDSourceType(CustomPIDSourceType sourceType) { - this.pidSource = sourceType; - } - - @Override - public CustomPIDSourceType getCustomPIDSourceType() { - return this.pidSource; - } } diff --git a/custom/sensors/PDP.java b/custom/sensors/PDP.java index db3cfa3c..1f3a3db0 100644 --- a/custom/sensors/PDP.java +++ b/custom/sensors/PDP.java @@ -1,5 +1,3 @@ -// THIS FILE IS TESTED post wpilibj2 - package org.usfirst.frc4904.standard.custom.sensors; import org.usfirst.frc4904.standard.LogKitten; diff --git a/subsystems/chassis/SensorDrive.java b/subsystems/chassis/SensorDrive.java index 4c35e6d5..a6e4027d 100644 --- a/subsystems/chassis/SensorDrive.java +++ b/subsystems/chassis/SensorDrive.java @@ -7,11 +7,6 @@ package org.usfirst.frc4904.standard.subsystems.chassis; - -import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.custom.CustomPIDSourceType; -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.custom.sensors.PIDSensor; import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder; import org.usfirst.frc4904.standard.custom.sensors.NavX; @@ -22,65 +17,33 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Subsystem; -public class SensorDrive implements Subsystem, PIDSensor { // Based largely on +public class SensorDrive implements Subsystem { // Based largely on // https://github.com/wpilibsuite/allwpilib/blob/master/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java private final TankDrive driveBase; private final CANTalonEncoder leftEncoder; private final CANTalonEncoder rightEncoder; private final NavX gyro; private final DifferentialDriveOdometry odometry; - private CustomPIDSourceType sensorType; /** * Creates a new DriveSubsystem. */ - public SensorDrive(TankDrive driveBase, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, NavX gyro, - CustomPIDSourceType sensorType, Pose2d initialPose) { + public SensorDrive(TankDrive driveBase, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, NavX gyro, Pose2d initialPose) { this.driveBase = driveBase; this.leftEncoder = leftEncoder; this.rightEncoder = rightEncoder; this.gyro = gyro; - setCustomPIDSourceType(sensorType); resetEncoders(); odometry = new DifferentialDriveOdometry(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), initialPose); CommandScheduler.getInstance().registerSubsystem(this); } - public SensorDrive(TankDrive driveBase, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, NavX gyro, Pose2d initialPose) { - this(driveBase, leftEncoder, rightEncoder, gyro, CustomPIDSourceType.kDisplacement, initialPose); - } - @Override public void periodic() { odometry.update(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance()); } - @Override - public void setCustomPIDSourceType(CustomPIDSourceType sensorType) { - this.sensorType = sensorType; - } - - @Override - public CustomPIDSourceType getCustomPIDSourceType() { - return sensorType; - } - - @Override - public double pidGetSafely() throws InvalidSensorException { - return getPose().getRotation().getDegrees(); - } - - @Override - public double pidGet() { - try { - return pidGetSafely(); - } catch (Exception e) { - LogKitten.ex(e); - return 0; - } - } - /** * Returns the currently-estimated pose of the robot. * diff --git a/subsystems/chassis/TankDrive.java b/subsystems/chassis/TankDrive.java index 304aca7e..15e3b4c2 100644 --- a/subsystems/chassis/TankDrive.java +++ b/subsystems/chassis/TankDrive.java @@ -1,5 +1,3 @@ -// THIS FILE IS TESTED post wpilibj2 - package org.usfirst.frc4904.standard.subsystems.chassis; import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; diff --git a/subsystems/motor/Motor.java b/subsystems/motor/Motor.java index 7dbdec93..6a88690a 100644 --- a/subsystems/motor/Motor.java +++ b/subsystems/motor/Motor.java @@ -1,5 +1,3 @@ -// THIS FILE IS TESTED post wpilibj2 - package org.usfirst.frc4904.standard.subsystems.motor; import org.usfirst.frc4904.standard.LogKitten; diff --git a/subsystems/motor/MotorController.java b/subsystems/motor/MotorController.java index c20707b9..4a0bb44b 100644 --- a/subsystems/motor/MotorController.java +++ b/subsystems/motor/MotorController.java @@ -1,6 +1,4 @@ -// THIS FILE IS TESTED post wpilibj2 - package org.usfirst.frc4904.standard.subsystems.motor; /** From b4d6acefaded339033f33db42ab74e3a3b337832 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 16 Feb 2023 19:22:39 -0800 Subject: [PATCH 040/134] delete motorSubsystem.get() --- subsystems/motor/MotorSubsystem.java | 16 +--------------- 1 file changed, 1 insertion(+), 15 deletions(-) diff --git a/subsystems/motor/MotorSubsystem.java b/subsystems/motor/MotorSubsystem.java index c3af4742..284da71e 100644 --- a/subsystems/motor/MotorSubsystem.java +++ b/subsystems/motor/MotorSubsystem.java @@ -4,7 +4,6 @@ import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -23,7 +22,6 @@ public class MotorSubsystem extends SubsystemBase { protected final MC[] motors; protected final SpeedModifier speedModifier; // NOTE: maybe change to be called PowerModifier protected final String name; - protected double prevPower; /** * A class that wraps around a variable number of MotorController objects to @@ -43,7 +41,6 @@ public MotorSubsystem(String name, SpeedModifier speedModifier, MC... motors) { this.name = name; this.speedModifier = speedModifier; this.motors = motors; - prevPower = 0; for (MotorController motor : motors) { motor.set(0); } @@ -112,16 +109,7 @@ public void stopMotor() { } } - /** - * Get the most recently set power. If the mostly called set was a - * setVoltage, return the estimated power. - * - * @return The most recently set power between -1.0 and 1.0. - */ - // TODO: do we even need .get()? if not, also remove prevPower and the edge casing in setVoltage? - // public double get() { - // return prevPower; - // } + // if you implement a .get() to get the power, make sure you update it in setVoltage() too (eg. with voltage/RobotController.getBatteryVoltage()) /** * Set the motor power. Passes through SpeedModifier. @@ -131,7 +119,6 @@ public void stopMotor() { public void set(double power) { LogKitten.v("Motor " + getName() + " @ " + power); double newPower = speedModifier.modify(power); - prevPower = newPower; for (MotorController motor : motors) { motor.set(newPower); } @@ -158,7 +145,6 @@ public void setPower(double power) { */ public void setVoltage(double voltage) { LogKitten.v("Motor " + getName() + " @ " + voltage + "v"); - prevPower = voltage / RobotController.getBatteryVoltage(); // TODO: use something with less latency than RobotController.getBatteryVoltage(); cite @zbuster05 for (var motor : motors) { motor.setVoltage(voltage); } From 717a76ab68ca0dae293685670fe83f4388e8d433 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 16 Feb 2023 19:34:06 -0800 Subject: [PATCH 041/134] talonmotorsubsystem neutraldeadband by default; docs --- subsystems/motor/TalonMotorSubsystem.java | 66 ++++++++++++++--------- 1 file changed, 41 insertions(+), 25 deletions(-) diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index a8c59558..f33e5981 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -10,15 +10,36 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; +/** + * A group of Talon motors (either TalonFX or TalonSRX) with the modern motor controller features. + * + * Provided constructors: + * - (name, speedmodifier=identity, neutralMode, deadband=0.01, limitswitches=false, voltageCompensation, leadMotor, followMotors) + * - (name, SpeedModifier, neutralMode, neutralDeadbandPercent, respectLeadMotorLimitSwitches, voltageCompensation, leadMotor, followMotors) + * + * Features included: + * - Neutral mode + * - Neutral deadband percent + * - Hardwired, normally open limit switches + * - Voltage compensation + * - Follow mode + * + * Gotchas: + * - Brake mode on a TalonSRX running a brushed motor (eg. 775) will probably do nothing + * - When voltage has a physical meaning (eg. from a Ramsete controller), use voltageCompensation=0 + * - .neutralOutput() clears follow mode, as do other control modes (eg. MotionMagic, MotionProfiled). For example, I believe the trigger of a limit switch causes follow motors to enter brake mode and thus exit follow mode. + * - Minimum neutral deadband is 0.001 (according to the docs, https://v5.docs.ctr-electronics.com/en/latest/ch13_MC.html#neutral-deadband) + * - Configure normallyClosed limit switches manually on the underlying motorControllers. We usually use normallyOpen limit switches, so you probably don't need to do this. + */ public class TalonMotorSubsystem extends BrakeableMotorSubsystem { private final int configTimeoutMs = 50; // milliseconds until the Talon gives up trying to configure public final TalonMotorController leadMotor; public final TalonMotorController[] followMotors; - // TODO: voltage ramping (closed and open) - - // TODO: limit switches - // TODO: add voltage/slew limit to drivetrain motors because we don't want the pid to actively try to stop the motor (negative power) when the driver just lets go of the controls. + // TODO: stator current limits? also makes brake mode stronger? https://www.chiefdelphi.com/t/programming-current-limiting-for-talonfx-java/371860 + // TODO: peak nominal outputs + // TODO: add voltage/slew limit to drivetrain motors because we don't want the pid to actively try to stop the motor (negative power) when the driver just lets go of the controls. diff ones for closed and open + // TODO: control speed near soft limits, so that you can't go full throttle near the soft limit? impl as a speed modifier?? /** * Motor Subsystem for a group of Talon motor controllers (Falcons, 775s). @@ -27,30 +48,33 @@ public class TalonMotorSubsystem extends BrakeableMotorSubsystem 0) { motor.configVoltageCompSaturation(voltageCompensation, configTimeoutMs); motor.enableVoltageCompensation(true); + } else { + motor.enableVoltageCompensation(false); } } setFollowMode(); @@ -116,24 +144,19 @@ public TalonMotorSubsystem(String name, SpeedModifier speedModifier, NeutralMode * @param neutralMode Whether the motor should brake or coast * when the the output is near zero, or * .disable() or .stopMotor() are called. - * @param neutralDeadbandPercent Percent output range around zero to enable - * neutralOutput (brake/coast mode) instead. - * For more info, see - * https://v5.docs.ctr-electronics.com/en/latest/ch13_MC.html#neutral-deadband * @param voltageCompensation 0 to disable, 10 is a good default. Set * the voltage corresponding to power=1.0 * This way, setting a power will lead to * consistent output even when other * components are running. Basically nerf all * motors so that they have a consistent - * output. when the battery is low. + * output when the battery is low. * @param leadMotor * @param followMotors */ - public TalonMotorSubsystem(String name, NeutralMode neutralMode, double neutralDeadbandPercent, - double voltageCompensation, + public TalonMotorSubsystem(String name, NeutralMode neutralMode, double voltageCompensation, TalonMotorController leadMotor, TalonMotorController... followMotors) { - this(name, new IdentityModifier(), neutralMode, neutralDeadbandPercent, false, voltageCompensation, leadMotor, followMotors); + this(name, new IdentityModifier(), neutralMode, 0.001, false, voltageCompensation, leadMotor, followMotors); } /** @@ -153,13 +176,6 @@ public void set(double power) { leadMotor.set(power); } - // @Override - // public double get() { - // return prevPower; - // // if we actually need this, we'll need to store prevPower and add an edge case in setVoltage and update it in set() - // // or maybe just get the leadMotorController.get()? - // } - @Override /** * if using a Ramsete controller, make sure to disable voltage compensation, From 5f9b8f9eef7d42995cdd98fe5f12ab0897e3a628 Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Thu, 16 Feb 2023 21:40:37 -0600 Subject: [PATCH 042/134] Forgot to change package lines --- custom/motioncontrollers/MotionController.java | 14 +++++++------- custom/motorcontrollers/CANTalonFX.java | 2 +- custom/motorcontrollers/CANTalonSRX.java | 4 +++- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/custom/motioncontrollers/MotionController.java b/custom/motioncontrollers/MotionController.java index fb001d43..7f065a27 100644 --- a/custom/motioncontrollers/MotionController.java +++ b/custom/motioncontrollers/MotionController.java @@ -52,8 +52,8 @@ public MotionController(DoubleConsumer output) { * * @param sensor The sensor associated with the output you are trying to control */ - public MotionController(PIDSensor sensor) { - this(sensor, null); + public MotionController() { + this(null); } @Override @@ -99,7 +99,7 @@ public void setOutput(DoubleConsumer output) { */ public final void reset() { resetErrorToZero(); - setpoint = sensor.pidGet(); + //setpoint = sensor.pidGet(); justReset = true; } @@ -108,7 +108,7 @@ public final void reset() { */ public final void resetSafely() throws InvalidSensorException { resetErrorToZero(); - setpoint = sensor.pidGetSafely(); + //setpoint = sensor.pidGetSafely(); justReset = true; } @@ -150,7 +150,7 @@ public final void resetSafely() throws InvalidSensorException { * @warning this does not indicate sensor errors */ public double getSensorValue() { - return sensor.pidGet(); + return 0; //return sensor.pidGet(); } /** @@ -159,7 +159,7 @@ public double getSensorValue() { * @return the current value of the sensor */ public double getInputSafely() throws InvalidSensorException { - return sensor.pidGetSafely(); + return 0; //sensor.pidGetSafely(); } /** @@ -276,7 +276,7 @@ public void disable() { return; } enable = false; - setpoint = sensor.pidGet(); + setpoint = 0; //sensor.pidGet(); } /** diff --git a/custom/motorcontrollers/CANTalonFX.java b/custom/motorcontrollers/CANTalonFX.java index 814fc140..2643be98 100644 --- a/custom/motorcontrollers/CANTalonFX.java +++ b/custom/motorcontrollers/CANTalonFX.java @@ -1,4 +1,4 @@ -package org.usfirst.frc4904.standard.subsystems.motor; +package org.usfirst.frc4904.standard.custom.motorcontrollers; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; diff --git a/custom/motorcontrollers/CANTalonSRX.java b/custom/motorcontrollers/CANTalonSRX.java index a4883403..a407c666 100644 --- a/custom/motorcontrollers/CANTalonSRX.java +++ b/custom/motorcontrollers/CANTalonSRX.java @@ -1,6 +1,8 @@ // THIS FILE IS TESTED post wpilibj2 -package org.usfirst.frc4904.standard.subsystems.motor; +package org.usfirst.frc4904.standard.custom.motorcontrollers; + +import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorController; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; From 971d39a3f1b915d7fb0445dea23f3aee67805fd4 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 16 Feb 2023 19:54:32 -0800 Subject: [PATCH 043/134] reorganize files harder --- .../motorcontrollers}/BrakeableMotorController.java | 2 +- custom/motorcontrollers/CANTalonSRX.java | 2 -- .../CustomCANSparkMax.java | 11 +++++------ .../motorcontrollers}/MotorController.java | 2 +- custom/motorcontrollers/TalonMotorController.java | 2 +- subsystems/motor/BrakeableMotorSubsystem.java | 1 + subsystems/motor/MotorSubsystem.java | 1 + subsystems/motor/TalonMotorSubsystem.java | 1 + 8 files changed, 11 insertions(+), 11 deletions(-) rename {subsystems/motor => custom/motorcontrollers}/BrakeableMotorController.java (81%) rename custom/{motioncontrollers => motorcontrollers}/CustomCANSparkMax.java (75%) rename {subsystems/motor => custom/motorcontrollers}/MotorController.java (92%) diff --git a/subsystems/motor/BrakeableMotorController.java b/custom/motorcontrollers/BrakeableMotorController.java similarity index 81% rename from subsystems/motor/BrakeableMotorController.java rename to custom/motorcontrollers/BrakeableMotorController.java index e6cf552b..6a671154 100644 --- a/subsystems/motor/BrakeableMotorController.java +++ b/custom/motorcontrollers/BrakeableMotorController.java @@ -1,7 +1,7 @@ // THIS FILE IS TESTED post wpilibj2 -package org.usfirst.frc4904.standard.subsystems.motor; +package org.usfirst.frc4904.standard.custom.motorcontrollers; public interface BrakeableMotorController extends MotorController { public abstract BrakeableMotorController setBrakeOnNeutral(); diff --git a/custom/motorcontrollers/CANTalonSRX.java b/custom/motorcontrollers/CANTalonSRX.java index a407c666..f7c67e48 100644 --- a/custom/motorcontrollers/CANTalonSRX.java +++ b/custom/motorcontrollers/CANTalonSRX.java @@ -2,8 +2,6 @@ package org.usfirst.frc4904.standard.custom.motorcontrollers; -import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorController; - import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; diff --git a/custom/motioncontrollers/CustomCANSparkMax.java b/custom/motorcontrollers/CustomCANSparkMax.java similarity index 75% rename from custom/motioncontrollers/CustomCANSparkMax.java rename to custom/motorcontrollers/CustomCANSparkMax.java index 8cf2dcf1..fc9b333b 100644 --- a/custom/motioncontrollers/CustomCANSparkMax.java +++ b/custom/motorcontrollers/CustomCANSparkMax.java @@ -1,10 +1,8 @@ -package org.usfirst.frc4904.standard.custom.motioncontrollers; +package org.usfirst.frc4904.standard.custom.motorcontrollers; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import org.usfirst.frc4904.standard.subsystems.motor.BrakeableMotorController; public class CustomCANSparkMax extends CANSparkMax implements BrakeableMotorController { protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; @@ -16,13 +14,14 @@ public CustomCANSparkMax(int deviceNumber, MotorType motorType, IdleMode neutral setInverted(inverted); } - public void setBrakeMode() { + public BrakeableMotorController setBrakeOnNeutral() { setIdleMode(IdleMode.kBrake); - + return this; } - public void setCoastMode() { + public BrakeableMotorController setCoastOnNeutral() { setIdleMode(IdleMode.kCoast); + return this; } @Override diff --git a/subsystems/motor/MotorController.java b/custom/motorcontrollers/MotorController.java similarity index 92% rename from subsystems/motor/MotorController.java rename to custom/motorcontrollers/MotorController.java index 68770207..dbd250cd 100644 --- a/subsystems/motor/MotorController.java +++ b/custom/motorcontrollers/MotorController.java @@ -1,5 +1,5 @@ -package org.usfirst.frc4904.standard.subsystems.motor; +package org.usfirst.frc4904.standard.custom.motorcontrollers; /** * An abstract motor interface that replaces the WPILib motor interface diff --git a/custom/motorcontrollers/TalonMotorController.java b/custom/motorcontrollers/TalonMotorController.java index da119425..61956d73 100644 --- a/custom/motorcontrollers/TalonMotorController.java +++ b/custom/motorcontrollers/TalonMotorController.java @@ -1,4 +1,4 @@ -package org.usfirst.frc4904.standard.subsystems.motor; +package org.usfirst.frc4904.standard.custom.motorcontrollers; import com.ctre.phoenix.motorcontrol.IMotorController; import com.ctre.phoenix.motorcontrol.IMotorControllerEnhanced; diff --git a/subsystems/motor/BrakeableMotorSubsystem.java b/subsystems/motor/BrakeableMotorSubsystem.java index eb2237d0..f51259f3 100644 --- a/subsystems/motor/BrakeableMotorSubsystem.java +++ b/subsystems/motor/BrakeableMotorSubsystem.java @@ -1,5 +1,6 @@ package org.usfirst.frc4904.standard.subsystems.motor; +import org.usfirst.frc4904.standard.custom.motorcontrollers.BrakeableMotorController; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; diff --git a/subsystems/motor/MotorSubsystem.java b/subsystems/motor/MotorSubsystem.java index 284da71e..9f14d23c 100644 --- a/subsystems/motor/MotorSubsystem.java +++ b/subsystems/motor/MotorSubsystem.java @@ -1,6 +1,7 @@ package org.usfirst.frc4904.standard.subsystems.motor; import org.usfirst.frc4904.standard.LogKitten; +import org.usfirst.frc4904.standard.custom.motorcontrollers.MotorController; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index f33e5981..f4031ef5 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -2,6 +2,7 @@ import java.util.stream.Stream; +import org.usfirst.frc4904.standard.custom.motorcontrollers.TalonMotorController; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; From 9b274ca7718f894e1e2672fc208f728f2d679160 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 <102199775+roboticsteam4904-2@users.noreply.github.com> Date: Mon, 27 Feb 2023 20:25:53 -0800 Subject: [PATCH 044/134] pid sensor setup Co-authored-by: Exr0n --- subsystems/motor/TalonMotorSubsystem.java | 48 ++++++++++++++++++++++- 1 file changed, 47 insertions(+), 1 deletion(-) diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index f4031ef5..c48fb5b8 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -1,15 +1,22 @@ package org.usfirst.frc4904.standard.subsystems.motor; +import java.util.function.DoubleSupplier; import java.util.stream.Stream; +import org.usfirst.frc4904.robot.RobotMap.PID; import org.usfirst.frc4904.standard.custom.motorcontrollers.TalonMotorController; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; +import com.ctre.phoenix.motorcontrol.RemoteSensorSource; + +import edu.wpi.first.wpilibj2.command.Command; /** * A group of Talon motors (either TalonFX or TalonSRX) with the modern motor controller features. @@ -34,11 +41,13 @@ */ public class TalonMotorSubsystem extends BrakeableMotorSubsystem { private final int configTimeoutMs = 50; // milliseconds until the Talon gives up trying to configure + private final int pid_idx = 0; // TODO: add support for auxillary pid + private final int follow_motors_remote_filter_id = 0; // which filter (0 or 1) will be used to configure reading from the integrated encoder on the lead motor public final TalonMotorController leadMotor; public final TalonMotorController[] followMotors; // TODO: stator current limits? also makes brake mode stronger? https://www.chiefdelphi.com/t/programming-current-limiting-for-talonfx-java/371860 - // TODO: peak nominal outputs + // TODO: peak/nominal outputs // TODO: add voltage/slew limit to drivetrain motors because we don't want the pid to actively try to stop the motor (negative power) when the driver just lets go of the controls. diff ones for closed and open // TODO: control speed near soft limits, so that you can't go full throttle near the soft limit? impl as a speed modifier?? @@ -113,6 +122,14 @@ public TalonMotorSubsystem(String name, SpeedModifier speedModifier, NeutralMode } } + // feedback sensor configuration (for PID) + leadMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, pid_idx, configTimeoutMs); + // for (var fm : followMotors) { // don't think this is needed because we are using follow mode. only needed for aux output + // fm.configRemoteFeedbackFilter(leadMotor.getDeviceID(), RemoteSensorSource.TalonFX_SelectedSensor /* enum internals has TalonFX = TalonSRX as of 2023 */, follow_motors_remote_filter_id, configTimeoutMs); + // fm.configSelectedFeedbackSensor(follow_motors_remote_filter_id == 0 ? FeedbackDevice.RemoteSensor0 : FeedbackDevice.RemoteSensor1 /* enum internals has TalonFX_SelectedSensor = TalonSRX_SelectedSensor as of 2023 */, pid_idx, configTimeoutMs); + // // TODO: change sensor polarity? https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/blob/master/Java%20Talon%20FX%20(Falcon%20500)/VelocityClosedLoop_AuxStraightIntegratedSensor/src/main/java/frc/robot/Robot.java#L306 + // } + // other configuration (neutral mode, neutral deadband, voltagecomp) for (var motor : motors) { motor.setNeutralMode(neutralMode); @@ -168,6 +185,35 @@ private void setFollowMode() { motor.follow(leadMotor); } } + + // TODO the following methods are not thought out or documented + /** + * The F value provided here will be overwritten if provided to subsystem.leadMotor.set; note that if you do that, it will bypass the subystem requirements check + * + * See docstrings on the methods used in the implementation for physical units + */ + public void configPIDF(double p, double i, double d, double f) { + leadMotor.config_kP(pid_idx, p, configTimeoutMs); + leadMotor.config_kI(pid_idx, i, configTimeoutMs); + leadMotor.config_kD(pid_idx, d, configTimeoutMs); + leadMotor.config_kF(pid_idx, f, configTimeoutMs); + leadMotor.configClosedLoopPeriod(pid_idx, 10, configTimeoutMs); // fast enough for 100Hz per second + // TODO: integral zone and closedLoopPeakOUtput? + // other things in the example: motionmagic config and statusframeperiod (for updating sensor status to the aux motor?) + } + /** + * + * @param setpointSupplier a function that returns a double, units = encoder ticks per 100ms + * + * @return + */ + public Command c_setVelocityControl(DoubleSupplier setpointSupplier) { + return this.run(() -> leadMotor.set(ControlMode.Velocity, setpointSupplier.getAsDouble())); + } + // public void zeroSensors() { + // // leadMotor.getSensorCollection // ?? doesn't exist; but it's in the CTRE falcon example + // // also should we zero the sensors on the follow motors just in case they're being used? + // } // don't override disable() or stop() because we *should* indeed use the base implementation of disabling/stopping each motor controller individually. Otherwise the following motors will try to follow a disabled motor, which may cause unexpected behavior (although realistically, it likely just gets set to zero and neutrallized by the deadband). From 1a5f5f93be0ed4c8e824938af2c6e2466510dcc5 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Tue, 28 Feb 2023 17:26:43 -0800 Subject: [PATCH 045/134] snap: partial plumbing sparkmax to talon interface --- .../motorcontrollers/CustomCANSparkMax.java | 34 +++++++++++++++++-- .../TalonMotorController.java | 21 ++++++++++-- subsystems/motor/TalonMotorSubsystem.java | 11 +++--- 3 files changed, 57 insertions(+), 9 deletions(-) diff --git a/custom/motorcontrollers/CustomCANSparkMax.java b/custom/motorcontrollers/CustomCANSparkMax.java index fc9b333b..9c1ebde9 100644 --- a/custom/motorcontrollers/CustomCANSparkMax.java +++ b/custom/motorcontrollers/CustomCANSparkMax.java @@ -1,17 +1,19 @@ package org.usfirst.frc4904.standard.custom.motorcontrollers; +import com.ctre.phoenix.ErrorCode; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax; -public class CustomCANSparkMax extends CANSparkMax implements BrakeableMotorController { +public class CustomCANSparkMax extends CANSparkMax implements TalonMotorController { protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; protected static final InvertType DEFAULT_INVERT_TYPE = InvertType.FollowMaster; + protected Double saturation_voltage = null; // remember the configured saturation voltage to conform to the talon api of having separate config() and enable() methods; error if we try to enable without configuring it. - public CustomCANSparkMax(int deviceNumber, MotorType motorType, IdleMode neutralMode, boolean inverted) { + public CustomCANSparkMax(int deviceNumber, MotorType motorType, IdleMode neutralMode) { super(deviceNumber, motorType); setIdleMode(neutralMode); - setInverted(inverted); + super.restoreFactoryDefaults(); } public BrakeableMotorController setBrakeOnNeutral() { @@ -28,4 +30,30 @@ public BrakeableMotorController setCoastOnNeutral() { public void neutralOutput() { stopMotor(); } + + /// talon compatibilty interface, so that you can shove CustomCANSparkMaxes into the motorsubsystem classes + // TODO: return the right error codes + public ErrorCode configVoltageCompSaturation(double saturation_voltage, int timeout_ms__unused) { + // we need to store the saturation voltage ourselves in order to mimick the talon interface of configuring seprately from enabling + this.saturation_voltage = saturation_voltage; + return ErrorCode.OK; + } + public void enableVoltageCompensation(boolean enable) { + if (enable) { + if (this.saturation_voltage == null) // fail if we have yet to configure the voltage comp + throw new NullPointerException("Tried to enable voltage compensation on SparkMax via the Talon compatibility interface without first configuring it."); + this.enableVoltageCompensation(this.saturation_voltage); + } else { + this.disableVoltageCompensation(); + } + } + + public void setNeutralMode(NeutralMode talon_neutral_mode) { + var idle_mode = switch (talon_neutral_mode) { + case NeutralMode.Brake -> IdleMode.kBrake; + case NeutralMode.Coast -> IdleMode.kCoast; + default -> throw new IllegalArgumentException("CANSparkMax neutralMode must be set to Brake or Coast"); + } + this.setIdleMode(IdleMode.kBrake); + } } diff --git a/custom/motorcontrollers/TalonMotorController.java b/custom/motorcontrollers/TalonMotorController.java index 61956d73..62f4dea3 100644 --- a/custom/motorcontrollers/TalonMotorController.java +++ b/custom/motorcontrollers/TalonMotorController.java @@ -1,7 +1,11 @@ package org.usfirst.frc4904.standard.custom.motorcontrollers; +import com.ctre.phoenix.ErrorCode; import com.ctre.phoenix.motorcontrol.IMotorController; -import com.ctre.phoenix.motorcontrol.IMotorControllerEnhanced; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; /** * A base class for CANTalonFX and CANTalonSRX that extends 4904 MotorController @@ -10,7 +14,8 @@ * May be converted to a general interface for motor controllers in the future, * given that SparkMaxes can also do brake mode, follow mode, etc. */ -public interface TalonMotorController extends BrakeableMotorController, IMotorControllerEnhanced { +// TODO: call this SmartMotorController +public interface TalonMotorController extends BrakeableMotorController { //TODO: add all the things // TODO: implement setVoltage with native APIs? or just use voltageComp? @@ -24,4 +29,16 @@ public interface TalonMotorController extends BrakeableMotorController, IMotorCo * @param leader the motor to follow */ public void follow(IMotorController leader); // are we able to return self to allow builder pattern? prob have to change the method name, else polymorphism breaks bc name collision with the one in base that takes IMotorController + + // interfaces we use for the talon. See CTRE IMotorControllerEnhanced for the docs. + public ErrorCode configVoltageCompSaturation(double saturation_voltage, int timeout); + public void enableVoltageCompensation(boolean enable); + public int getDeviceID(); + public ErrorCode configNeutralDeadband(double neutral_deadband_percent, int timeout); + public ErrorCode configForwardLimitSwitchSource(LimitSwitchSource source, LimitSwitchNormal normal_mode, int timeout); + public ErrorCode configReverseLimitSwitchSource(LimitSwitchSource source, LimitSwitchNormal normal_mode, int timeout); + public ErrorCode configForwardLimitSwitchSource(RemoteLimitSwitchSource source, LimitSwitchNormal normal_mode, int device_id, int timeout); + public ErrorCode configReverseLimitSwitchSource(RemoteLimitSwitchSource source, LimitSwitchNormal normal_mode, int device_id, int timeout); + public void overrideLimitSwitchesEnable(boolean enable); + public void setNeutralMode(NeutralMode neutral_mode); } diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index f4031ef5..f19dc6ff 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -2,10 +2,12 @@ import java.util.stream.Stream; +import org.usfirst.frc4904.standard.custom.motorcontrollers.MotorController; import org.usfirst.frc4904.standard.custom.motorcontrollers.TalonMotorController; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; +import com.ctre.phoenix.motorcontrol.IMotorControllerEnhanced; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -32,9 +34,10 @@ * - Minimum neutral deadband is 0.001 (according to the docs, https://v5.docs.ctr-electronics.com/en/latest/ch13_MC.html#neutral-deadband) * - Configure normallyClosed limit switches manually on the underlying motorControllers. We usually use normallyOpen limit switches, so you probably don't need to do this. */ -public class TalonMotorSubsystem extends BrakeableMotorSubsystem { +// TODO: rename to SmartMotorSubsystem or something +public class TalonMotorSubsystem extends BrakeableMotorSubsystem { private final int configTimeoutMs = 50; // milliseconds until the Talon gives up trying to configure - public final TalonMotorController leadMotor; + public final LMC leadMotor; // generic type parameter LMC because it should be able to be either WPI_TalonSRX or WPI_TalonFX, but we need both the "smart" stuff from IMotorControllerEnhanced and the .set and .setVoltage from 4904::MotorController public final TalonMotorController[] followMotors; // TODO: stator current limits? also makes brake mode stronger? https://www.chiefdelphi.com/t/programming-current-limiting-for-talonfx-java/371860 @@ -88,7 +91,7 @@ public class TalonMotorSubsystem extends BrakeableMotorSubsystem Date: Tue, 28 Feb 2023 19:55:31 -0800 Subject: [PATCH 046/134] UT: SparkMax, mv to SmartMotorController --- .../BrakeableMotorController.java | 10 - custom/motorcontrollers/CANTalonFX.java | 6 + custom/motorcontrollers/CANTalonSRX.java | 6 + .../motorcontrollers/CustomCANSparkMax.java | 14 +- custom/motorcontrollers/MotorController.java | 19 -- .../SmartMotorController.java | 19 ++ .../TalonMotorController.java | 2 +- subsystems/motor/BrakeableMotorSubsystem.java | 126 --------- ...ubsystem.java => SmartMotorSubsystem.java} | 172 ++++++------ subsystems/motor/SparkMaxMotorSubsystem.java | 257 ++++++++++++++++++ subsystems/motor/TalonMotorSubsystem.java | 56 ++-- 11 files changed, 426 insertions(+), 261 deletions(-) delete mode 100644 custom/motorcontrollers/BrakeableMotorController.java delete mode 100644 custom/motorcontrollers/MotorController.java create mode 100644 custom/motorcontrollers/SmartMotorController.java delete mode 100644 subsystems/motor/BrakeableMotorSubsystem.java rename subsystems/motor/{MotorSubsystem.java => SmartMotorSubsystem.java} (55%) create mode 100644 subsystems/motor/SparkMaxMotorSubsystem.java diff --git a/custom/motorcontrollers/BrakeableMotorController.java b/custom/motorcontrollers/BrakeableMotorController.java deleted file mode 100644 index 6a671154..00000000 --- a/custom/motorcontrollers/BrakeableMotorController.java +++ /dev/null @@ -1,10 +0,0 @@ - -// THIS FILE IS TESTED post wpilibj2 - -package org.usfirst.frc4904.standard.custom.motorcontrollers; - -public interface BrakeableMotorController extends MotorController { - public abstract BrakeableMotorController setBrakeOnNeutral(); - public abstract BrakeableMotorController setCoastOnNeutral(); - public abstract void neutralOutput(); -} diff --git a/custom/motorcontrollers/CANTalonFX.java b/custom/motorcontrollers/CANTalonFX.java index 2643be98..760de5a0 100644 --- a/custom/motorcontrollers/CANTalonFX.java +++ b/custom/motorcontrollers/CANTalonFX.java @@ -28,6 +28,12 @@ public CANTalonFX(int deviceNumber, InvertType inverted) { setInverted(inverted); } + /** + * Alias for .set() on power + * @param power + */ + public void setPower(double power) { set(power); } + /** * Setting to enable brake mode on neutral (when .neutralOutput(), * .disable(), or .stopMotor() is called, or when output percent is within diff --git a/custom/motorcontrollers/CANTalonSRX.java b/custom/motorcontrollers/CANTalonSRX.java index f7c67e48..700e7b4e 100644 --- a/custom/motorcontrollers/CANTalonSRX.java +++ b/custom/motorcontrollers/CANTalonSRX.java @@ -29,6 +29,12 @@ public CANTalonSRX(int deviceNumber, InvertType inverted) { setInverted(inverted); } + /** + * Alias for .set() on power + * @param power + */ + public void setPower(double power) { set(power); } + /** * Setting to enable brake mode on neutral (when .neutralOutput(), * .disable(), or .stopMotor() is called, or when output percent is within diff --git a/custom/motorcontrollers/CustomCANSparkMax.java b/custom/motorcontrollers/CustomCANSparkMax.java index 54a2fe01..ab59aebd 100644 --- a/custom/motorcontrollers/CustomCANSparkMax.java +++ b/custom/motorcontrollers/CustomCANSparkMax.java @@ -4,10 +4,10 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax; -public class CustomCANSparkMax extends CANSparkMax implements BrakeableMotorController { +public class CustomCANSparkMax extends CANSparkMax implements SmartMotorController { protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; protected static final InvertType DEFAULT_INVERT_TYPE = InvertType.FollowMaster; - protected Double saturation_voltage = null; // remember the configured saturation voltage to conform to the talon api of having separate config() and enable() methods; error if we try to enable without configuring it. + // protected Double voltage_compensation_max = null; // remember the configured saturation voltage to conform to the talon api of having separate config() and enable() methods; error if we try to enable without configuring it. public CustomCANSparkMax(int deviceNumber, MotorType motorType, IdleMode neutralMode) { super(deviceNumber, motorType); @@ -15,12 +15,18 @@ public CustomCANSparkMax(int deviceNumber, MotorType motorType, IdleMode neutral super.restoreFactoryDefaults(); } - public BrakeableMotorController setBrakeOnNeutral() { + /** + * Alias for .set() on power + * @param power + */ + public void setPower(double power) { set(power); } + + public SmartMotorController setBrakeOnNeutral() { setIdleMode(IdleMode.kBrake); return this; } - public BrakeableMotorController setCoastOnNeutral() { + public SmartMotorController setCoastOnNeutral() { setIdleMode(IdleMode.kCoast); return this; } diff --git a/custom/motorcontrollers/MotorController.java b/custom/motorcontrollers/MotorController.java deleted file mode 100644 index dbd250cd..00000000 --- a/custom/motorcontrollers/MotorController.java +++ /dev/null @@ -1,19 +0,0 @@ - -package org.usfirst.frc4904.standard.custom.motorcontrollers; - -/** - * An abstract motor interface that replaces the WPILib motor interface - * - to force custom implementation of setVoltage (WPILib's uses RobotController.getBatteryVoltage() which is slow, cite @zbuster05) - */ -public interface MotorController extends edu.wpi.first.wpilibj.motorcontrol.MotorController { - /** - * Implementations should implement this using a better voltage reading than - * RobotController.getBatteryVoltage(), preferably a native interface. - * - * NOTE FROM BASE CLASS: This function *must* be called regularly in order - * for voltage compensation to work properly - unlike the ordinary set - * function, it is not "set it and forget it." - */ - @Override - public abstract void setVoltage(double voltage); -} diff --git a/custom/motorcontrollers/SmartMotorController.java b/custom/motorcontrollers/SmartMotorController.java new file mode 100644 index 00000000..deda35c7 --- /dev/null +++ b/custom/motorcontrollers/SmartMotorController.java @@ -0,0 +1,19 @@ + +// THIS FILE IS TESTED post wpilibj2 + +package org.usfirst.frc4904.standard.custom.motorcontrollers; + +/** + * Represents a "smart" motor controller, like the TalonFX, TalonSRX, or SparkMax + * + * These should support brake mode, follow mode, limit switches, and various closed-loop control modes. + */ +public interface SmartMotorController { + public abstract void set(double power); + public abstract void setPower(double power); + public abstract void setVoltage(double voltage); + + public abstract SmartMotorController setBrakeOnNeutral(); + public abstract SmartMotorController setCoastOnNeutral(); + public abstract void neutralOutput(); +} diff --git a/custom/motorcontrollers/TalonMotorController.java b/custom/motorcontrollers/TalonMotorController.java index 61956d73..2712b3a5 100644 --- a/custom/motorcontrollers/TalonMotorController.java +++ b/custom/motorcontrollers/TalonMotorController.java @@ -10,7 +10,7 @@ * May be converted to a general interface for motor controllers in the future, * given that SparkMaxes can also do brake mode, follow mode, etc. */ -public interface TalonMotorController extends BrakeableMotorController, IMotorControllerEnhanced { +public interface TalonMotorController extends SmartMotorController, IMotorControllerEnhanced { //TODO: add all the things // TODO: implement setVoltage with native APIs? or just use voltageComp? diff --git a/subsystems/motor/BrakeableMotorSubsystem.java b/subsystems/motor/BrakeableMotorSubsystem.java deleted file mode 100644 index f51259f3..00000000 --- a/subsystems/motor/BrakeableMotorSubsystem.java +++ /dev/null @@ -1,126 +0,0 @@ -package org.usfirst.frc4904.standard.subsystems.motor; - -import org.usfirst.frc4904.standard.custom.motorcontrollers.BrakeableMotorController; -import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; -import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; - -import edu.wpi.first.wpilibj2.command.Command; - - -public class BrakeableMotorSubsystem extends MotorSubsystem { // generic to allow inherited class (eg. TalonMotorSubsystem) to directly use TalonMotorController APIs on super.motors (not possible if this.motors here was BrakeableMotorController) - // TODO: should this take general MotorControllers and just brake the ones that implement BrakeableMotorController - - /** - * A class that wraps around a variable number of BrakeableMotorController - * objects to give them Subsystem functionality. Can also modify their power - * with a SpeedModifier for things like scaling or brownout protection. - * Enables brake mode on each motor by default. - * - * @param name The name for the motor - * @param speedModifier A SpeedModifier changes the input to every motor based - * on some factor. The default is an IdentityModifier, - * which does not affect anything. - * @param motors The MotorControllers in this subsystem. Can be a single - * MotorController or multiple MotorControllers. - */ - public BrakeableMotorSubsystem(String name, SpeedModifier speedModifier, BMC... motors) { - super(name, speedModifier, motors); - setDefaultCommand(this.runOnce(() -> this.neutralOutput())); - } - - /** - * A class that wraps around a variable number of BrakeableMotorController - * objects to give them Subsystem functionality. Can also modify their speed - * with a SpeedModifier for things like scaling or brownout protection. - * Enables brake mode on each motor by default. - * - * @param name The name for the motor - * @param motors The MotorControllers in this subsystem. Can be a single - * MotorController or multiple MotorControllers. - */ - public BrakeableMotorSubsystem(String name, BMC... motors) { - this(name, new IdentityModifier(), motors); - } - - /** - * A class that wraps around a variable number of BrakeableMotorController - * objects to give them Subsystem functionality. Can also modify their speed - * with a SpeedModifier for things like scaling or brownout protection. - * Enables brake mode on each motor by default. - * - * @param speedModifier A SpeedModifier changes the input to every motor based - * on some factor. The default is an IdentityModifier, - * which does not affect anything. - * @param motors The MotorControllers in this subsystem. Can be a single - * MotorController or multiple MotorControllers. - */ - public BrakeableMotorSubsystem(SpeedModifier speedModifier, BMC... motors) { - this("Motor", speedModifier, motors); - } - - /** - * A class that wraps around a variable number of BrakeableMotorController - * objects to give them Subsystem functionality. Can also modify their speed - * with a SpeedModifier for things like scaling or brownout protection. - * Enables brake mode on each motor by default. - * - * @param motors The MotorControllers in this subsystem. Can be a single - * MotorController or multiple MotorControllers. - */ - public BrakeableMotorSubsystem(BMC... motors) { - this("Motor", motors); - } - - /** - * Sets the neutral output mode to brake. Motor will continue to run, but - * future calls to neutralOutput() will cause motor to brake. - */ - public void setBrakeOnNeutral() { for (var motor : motors) motor.setBrakeOnNeutral(); } - /** - * Sets the neutral output mode to coast. Motor will continue to run, but - * future calls to neutralOutput() will cause motor to coast. - */ - public void setCoastOnNeutral() { for (var motor : motors) motor.setCoastOnNeutral(); } - - /** - * Sets the output mode to neutral, which is either break or coast (default - * brake). Use setBrakeModeOnNeutral or setCoastModeOnNeutral to set this - * mode. - * - * Uses the underlying .neutralOutput() method. - */ - public void neutralOutput() { for (var motor : motors) motor.neutralOutput(); } - - /** - * Enables brake mode on and brakes each motor. - */ - public void brake() { - setBrakeOnNeutral(); - neutralOutput(); - } - - /** - * Enables coast mode on and coasts each motor. - */ - public void coast() { - setCoastOnNeutral(); - neutralOutput(); - } - - /** - * Enables brake mode on and brakes each motor. Command version of brake(). - * - * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_brake()) - */ - public Command c_brake() { - return this.runOnce(() -> this.brake()); - } - /** - * Enables coast mode on and coasts each motor. Command version of coast(). - * - * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_coast()) - */ - public Command c_coast() { - return this.runOnce(() -> this.coast()); - } -} diff --git a/subsystems/motor/MotorSubsystem.java b/subsystems/motor/SmartMotorSubsystem.java similarity index 55% rename from subsystems/motor/MotorSubsystem.java rename to subsystems/motor/SmartMotorSubsystem.java index 9f14d23c..5734264d 100644 --- a/subsystems/motor/MotorSubsystem.java +++ b/subsystems/motor/SmartMotorSubsystem.java @@ -1,70 +1,66 @@ package org.usfirst.frc4904.standard.subsystems.motor; import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.custom.motorcontrollers.MotorController; +import org.usfirst.frc4904.standard.custom.motorcontrollers.SmartMotorController; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; -/** - * Wraps MotorControllers to represent as a subsystem. Should be used as the - * atomic, primary motor class for subsystem compositions. - * - * Does not include inversion logic, instead, MotorControllers should be - * inverted before constructing this class. Please use 4904custom - * MotorControllers rather than wpilibj2 MotorControllers. - * - * Replaces Motor.java in pre-2023 standard, except without CTRE voltage comp by - * default and without inversion logic. - */ -public class MotorSubsystem extends SubsystemBase { - protected final MC[] motors; + +public abstract class SmartMotorSubsystem extends SubsystemBase { // generic to allow inherited class (eg. TalonMotorSubsystem) to directly use TalonMotorController APIs on super.motors (not possible if this.motors here was BrakeableMotorController) + protected final SMC[] motors; protected final SpeedModifier speedModifier; // NOTE: maybe change to be called PowerModifier protected final String name; + // TODO: should this take general MotorControllers and just brake the ones that implement BrakeableMotorController - /** - * A class that wraps around a variable number of MotorController objects to - * give them Subsystem functionality. Can also modify their power with a - * SpeedModifier for things like scaling or brownout protection. - * - * @param name The name for the motor - * @param speedModifier A SpeedModifier changes the input to every motor based - * on some factor. The default is an IdentityModifier, - * which does not affect anything. - * @param motors The MotorControllers in this subsystem. Can be a single - * MotorController or multiple MotorControllers. - */ - public MotorSubsystem(String name, SpeedModifier speedModifier, MC... motors) { + /** + * A class that wraps around a variable number of BrakeableMotorController + * objects to give them Subsystem functionality. Can also modify their power + * with a SpeedModifier for things like scaling or brownout protection. + * Enables brake mode on each motor by default. + * + * @param name The name for the motor + * @param speedModifier A SpeedModifier changes the input to every motor based + * on some factor. The default is an IdentityModifier, + * which does not affect anything. + * @param motors The MotorControllers in this subsystem. Can be a single + * MotorController or multiple MotorControllers. + */ + public SmartMotorSubsystem(String name, SpeedModifier speedModifier, SMC... motors) { super(); setName(name); this.name = name; this.speedModifier = speedModifier; this.motors = motors; - for (MotorController motor : motors) { + for (var motor : motors) { motor.set(0); } setDefaultCommand(this.c_idle()); - } + + setDefaultCommand(this.runOnce(() -> this.neutralOutput())); + } /** - * A class that wraps around a variable number of MotorController objects to - * give them Subsystem functionality. Can also modify their speed with a - * SpeedModifier for things like scaling or brownout protection. + * A class that wraps around a variable number of BrakeableMotorController + * objects to give them Subsystem functionality. Can also modify their speed + * with a SpeedModifier for things like scaling or brownout protection. + * Enables brake mode on each motor by default. * * @param name The name for the motor * @param motors The MotorControllers in this subsystem. Can be a single * MotorController or multiple MotorControllers. */ - public MotorSubsystem(String name, MC... motors) { + public SmartMotorSubsystem(String name, SMC... motors) { this(name, new IdentityModifier(), motors); } /** - * A class that wraps around a variable number of MotorController objects to - * give them Subsystem functionality. Can also modify their speed with a - * SpeedModifier for things like scaling or brownout protection. + * A class that wraps around a variable number of BrakeableMotorController + * objects to give them Subsystem functionality. Can also modify their speed + * with a SpeedModifier for things like scaling or brownout protection. + * Enables brake mode on each motor by default. * * @param speedModifier A SpeedModifier changes the input to every motor based * on some factor. The default is an IdentityModifier, @@ -72,31 +68,28 @@ public MotorSubsystem(String name, MC... motors) { * @param motors The MotorControllers in this subsystem. Can be a single * MotorController or multiple MotorControllers. */ - public MotorSubsystem(SpeedModifier speedModifier, MC... motors) { + public SmartMotorSubsystem(SpeedModifier speedModifier, SMC... motors) { this("Motor", speedModifier, motors); } /** - * A class that wraps around a variable number of MotorController objects to - * give them Subsystem functionality. Can also modify their speed with a - * SpeedModifier for things like scaling or brownout protection. + * A class that wraps around a variable number of BrakeableMotorController + * objects to give them Subsystem functionality. Can also modify their speed + * with a SpeedModifier for things like scaling or brownout protection. + * Enables brake mode on each motor by default. * * @param motors The MotorControllers in this subsystem. Can be a single * MotorController or multiple MotorControllers. */ - public MotorSubsystem(MC... motors) { - this("Motor", motors); - } - + public SmartMotorSubsystem(SMC... motors) { + this("Motor Subsystem", motors); + } + /// METHODS /** * Disables the motor with underlying .disable() */ - public void disable() { - for (MotorController motor : motors) { - motor.disable(); - } - } + public void disable() { for (var motor : motors) motor.disable(); } /** * Stops the motor with underlying stopMotor() @@ -104,11 +97,7 @@ public void disable() { * In theory this should stop the motor without disabling, but wpilib seems * to just call disable under the hood. */ - public void stopMotor() { - for (MotorController motor : motors) { - motor.stopMotor(); - } - } + public void stopMotor() { for (var motor : motors) motor.stopMotor(); } // if you implement a .get() to get the power, make sure you update it in setVoltage() too (eg. with voltage/RobotController.getBatteryVoltage()) @@ -120,9 +109,7 @@ public void stopMotor() { public void set(double power) { LogKitten.v("Motor " + getName() + " @ " + power); double newPower = speedModifier.modify(power); - for (MotorController motor : motors) { - motor.set(newPower); - } + for (var motor : motors) motor.set(newPower); } /** @@ -150,8 +137,42 @@ public void setVoltage(double voltage) { motor.setVoltage(voltage); } } - + /** + * Sets the neutral output mode to brake. Motor will continue to run, but + * future calls to neutralOutput() will cause motor to brake. + */ + public void setBrakeOnNeutral() { for (var motor : motors) motor.setBrakeOnNeutral(); } + /** + * Sets the neutral output mode to coast. Motor will continue to run, but + * future calls to neutralOutput() will cause motor to coast. + */ + public void setCoastOnNeutral() { for (var motor : motors) motor.setCoastOnNeutral(); } + /** + * Sets the output mode to neutral, which is either break or coast (default + * brake). Use setBrakeModeOnNeutral or setCoastModeOnNeutral to set this + * mode. + * + * Uses the underlying .neutralOutput() method. + */ + public void neutralOutput() { for (var motor : motors) motor.neutralOutput(); } + + /** + * Enables brake mode on and brakes each motor. + */ + public void brake() { + setBrakeOnNeutral(); + neutralOutput(); + } + + /** + * Enables coast mode on and coasts each motor. + */ + public void coast() { + setCoastOnNeutral(); + neutralOutput(); + } + /// COMMANDS // TODO: remove the "Replaces *.java in pre-2023 standard" and "eg. button1.onTrue(motor.c_idle())" when they become unnecessary. @@ -207,23 +228,20 @@ public void setVoltage(double voltage) { * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_setVoltageHold()) */ public Command c_setVoltageHold(double voltage) { return this.run(() -> this.setVoltage(voltage)); } - - // /// ERRORS (copied from 2022 standard Motor.java, not clear what the use is) - // protected class UnsynchronizedMotorControllerRuntimeException extends RuntimeException { - // private static final long serialVersionUID = 8688590919561059584L; - - // public UnsynchronizedMotorControllerRuntimeException() { - // super(getName() + "'s MotorControllers report different speeds"); - // } - // } - - // @Deprecated - // protected class StrangeCANMotorControllerModeRuntimeException extends RuntimeException { - // private static final long serialVersionUID = -539917227288371271L; - - // public StrangeCANMotorControllerModeRuntimeException() { - // super("One of " + getName() - // + "'s MotorControllers is a CANMotorController with a non-zero mode. This might mess up it's .get(), so Motor cannot verify safety."); - // } - // } + /** + * Enables brake mode on and brakes each motor. Command version of brake(). + * + * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_brake()) + */ + public Command c_brake() { + return this.runOnce(() -> this.brake()); + } + /** + * Enables coast mode on and coasts each motor. Command version of coast(). + * + * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_coast()) + */ + public Command c_coast() { + return this.runOnce(() -> this.coast()); + } } diff --git a/subsystems/motor/SparkMaxMotorSubsystem.java b/subsystems/motor/SparkMaxMotorSubsystem.java new file mode 100644 index 00000000..b90aac87 --- /dev/null +++ b/subsystems/motor/SparkMaxMotorSubsystem.java @@ -0,0 +1,257 @@ +package org.usfirst.frc4904.standard.subsystems.motor; + +import java.util.function.DoubleSupplier; +import java.util.stream.Stream; + +import org.usfirst.frc4904.standard.custom.CustomCAN; +import org.usfirst.frc4904.standard.custom.motorcontrollers.CustomCANSparkMax; +import org.usfirst.frc4904.standard.custom.motorcontrollers.TalonMotorController; +import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; +import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; +import com.revrobotics.REVLibError; +import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.CANSparkMax.IdleMode; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.Command; + +/** + * A group of SparkMax motor controllers the modern motor controller features. + * + * Provided constructors: + * - (name, speedmodifier=identity, neutralMode, deadband=0.01, limitswitches=false, voltageCompensation, leadMotor, followMotors) + * - (name, SpeedModifier, neutralMode, neutralDeadbandPercent, respectLeadMotorLimitSwitches, voltageCompensation, leadMotor, followMotors) + * + * Features included: + * - Neutral mode + * - Neutral deadband percent + * - Hardwired, normally open limit switches + * - Voltage compensation + * - Follow mode + * + * Gotchas: + * - Brake mode on a brushed motor will do nothing. + * - When voltage has a physical meaning (eg. from a Ramsete controller), use voltageCompensation=0 + * - Configure normallyClosed limit switches manually on the underlying motorControllers. We usually use normallyOpen limit switches, so you probably don't need to do this. + * - Specifying respectLeadMotorLimitSwitches=false will not disable the limit switches if they were previously enabled. This is to allow you to configure limit switches on the underlying motor controllers before passing them in if necessary. + */ +public class SparkMaxMotorSubsystem extends SmartMotorSubsystem { + private final int configTimeoutMs = 50; // milliseconds until the Talon gives up trying to configure + private final int pid_idx = 0; // TODO: add support for auxillary pid + private final int follow_motors_remote_filter_id = 0; // DONOT REMOVE, USED IN COMMENTED CODE BELOW; which filter (0 or 1) will be used to configure reading from the integrated encoder on the lead motor + private SparkMaxPIDController controller = null; + public final CustomCANSparkMax leadMotor; + public final CustomCANSparkMax[] followMotors; + + // TODO: stator current limits? also makes brake mode stronger? https://www.chiefdelphi.com/t/programming-current-limiting-for-talonfx-java/371860 + // TODO: peak/nominal outputs + // TODO: add voltage/slew limit to drivetrain motors because we don't want the pid to actively try to stop the motor (negative power) when the driver just lets go of the controls. diff ones for closed and open + // TODO: control speed near soft limits, so that you can't go full throttle near the soft limit? impl as a speed modifier?? + + /** + * Motor Subsystem for a group of Talon motor controllers (Falcons, 775s). + * Uses Talon-specific APIs like follow mode and motionProfile control mode to + * offload work from the RoboRIO. + * + * You probably want to set inverted mode on the TalonMotorController using + * FollowLeader or OpposeLeader + * + * @param name + * @param speedModifier + * @param idleMode Whether the motor should brake or coast + * when the the output is near zero, or + * .disable() or .stopMotor() are called. + * @param neutralDeadbandPercent Power output range around zero to enable + * neutralOutput (brake/coast mode) instead. + * Can be in the range [0.001, 0.25], CTRE + * default is 0.04. For more info, see + * https://v5.docs.ctr-electronics.com/en/latest/ch13_MC.html#neutral-deadband + * @param respectLeadMotorLimitSwitches Whether to enable the forward and + * reverse limit switches wired to the + * lead motor. This is the easiest way to + * use forward and reverse normally open + * limit switches (or just forward or + * just reverse, as not plugging a limit + * switch in is equivalent to a normally + * open switch being unpressed). NOTE + * This configuration is relatively + * limited, consider configuring limit + * switches manually on the + * MotorController for advanced + * configuration (eg. normally closed + * switches). NOTE passing false will not + * disable limit switches; just unplug + * them instead. + * @param voltageCompensation 0 to disable, 10 is a good default. Set + * the voltage corresponding to power=1.0 + * This way, setting a power will lead to + * consistent output even when other + * components are running. Basically nerf all + * motors so that they have a consistent + * output. when the battery is low. + * @param leadMotor + * @param followMotors + */ + public SparkMaxMotorSubsystem(String name, SpeedModifier speedModifier, IdleMode idleMode, double neutralDeadbandPercent, + Boolean respectLeadMotorLimitSwitches, double voltageCompensation, + CustomCANSparkMax leadMotor, CustomCANSparkMax... followMotors) { + super(name, speedModifier, Stream.concat(Stream.of(leadMotor), Stream.of(followMotors)).toArray(CustomCANSparkMax[]::new)); // java has no spread operator, so you have to concat. best way i could find is to do it in a stream. please make this not bad if you know how + + this.leadMotor = leadMotor; + this.followMotors = followMotors; + + // limit switch configuration + if (respectLeadMotorLimitSwitches) { + // when extending to SparkMAX: you have to sparkmax.getForward/ReverseLimitSwitch.enable() or something. may need custom polling/plugin logic. https://codedocs.revrobotics.com/java/com/revrobotics/cansparkmax#getReverseLimitSwitch(com.revrobotics.SparkMaxLimitSwitch.Type) + + // TODO: spark max limit switches are untested + var fwdLimit = leadMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + var revLimit = leadMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + REVLibThrowable.guard(fwdLimit.enableLimitSwitch(true)); // REVLibThrowable.guard will throw a runtime exception if the configuration fails. + REVLibThrowable.guard(revLimit.enableLimitSwitch(true)); + // TODO: do following spark maxes need to be configured to use remote limit switch? or can they just auto-brake w/ a neutral-deadband equivalent? + } // no else { disableLimitSwitches() } here because we don't want to overwrite on false; user may be trying to use their own configuration. + + // other configuration (neutral mode, neutral deadband, voltagecomp) + for (var motor : motors) { + motor.setIdleMode(idleMode); + //motor.configNeutralDeadband(neutralDeadbandPercent, configTimeoutMs); // TODO: no neutral deadband setting on sparkmax? + if (voltageCompensation > 0) { + motor.enableVoltageCompensation(voltageCompensation); + } else { + motor.disableVoltageCompensation(); + } + } + setFollowMode(); + // do we need to burnFlash? CD says it can wear out the EEPROM (which has limited write cycles) and also appears to be somewhat buggy. But it otherwise the motor config may get reset in case the motor gets power cycled (eg. during a brownout)? + } + /** + * Motor Subsystem for a group of Talon motor controllers (Falcons, 775s). + * Uses Talon-specific APIs like follow mode and motionProfile control mode to + * offload work from the RoboRIO. + * + * You probably want to set inverted mode on the TalonMotorController using + * FollowLeader or OpposeLeader + * + * Consider using the advanced constructor for limit switches and voltage + * compensation, and using the .cfg...() methods for voltage ramping and + * nominal output. + * + * When using SparkMaxes: + * - can limit switches be enabled/disabled? can you have a sparkmax respect a talon limit switch? https://codedocs.revrobotics.com/java/com/revrobotics/cansparkmax + * + * @param name + * @param idleMode Whether the motor should brake or coast + * when the the output is near zero, or + * .disable() or .stopMotor() are called. + * @param voltageCompensation 0 to disable, 10 is a good default. Set + * the voltage corresponding to power=1.0 + * This way, setting a power will lead to + * consistent output even when other + * components are running. Basically nerf all + * motors so that they have a consistent + * output when the battery is low. + * @param leadMotor + * @param followMotors + */ + public SparkMaxMotorSubsystem(String name, IdleMode idleMode, double voltageCompensation, + CustomCANSparkMax leadMotor, CustomCANSparkMax... followMotors) { + this(name, new IdentityModifier(), idleMode, 0.001, false, voltageCompensation, leadMotor, followMotors); + } + + /** + * Make all follow motors follow the lead motor. + */ + private void setFollowMode() { + for (var motor : this.followMotors) { + motor.follow(leadMotor); + } + } + + // TODO the following methods are not thought out or documented + /** + * The F value provided here will be overwritten if provided to subsystem.leadMotor.set; note that if you do that, it will bypass the subystem requirements check + * + * See docstrings on the methods used in the implementation for physical units + */ + public void configPIDF(double p, double i, double d, double f) { + // set sensor + var sensor = leadMotor.getEncoder(); + sensor.setInverted(false); + // docs says you need to sparkMax.setFeedbackDevice but that appears to not exist.. + // is this even needed for brushless? migration docs says it's not: https://docs.revrobotics.com/sparkmax/software-resources/migrating-ctre-to-rev#select-encoder + + // set pid constants + controller = leadMotor.getPIDController(); + controller.setP(p); + controller.setI(i); + controller.setD(d); + controller.setFF(f); + // TODO: integral zone and outputRange? + } + /** + * + * @param setpointSupplier a function that returns a double, units = RPM + * + * @return the command to schedule + */ + public Command c_controlVelocity(DoubleSupplier setpointSupplier) { + if (controller == null) throw new IllegalArgumentException(name + " tried to use c_controlVelocity without first configPIDF()-ing."); + return this.run(() -> controller.setReference(setpointSupplier.getAsDouble(), ControlType.kVelocity)); + } + // public void zeroSensors() { + // // leadMotor.getSensorCollection // ?? doesn't exist; but it's in the CTRE falcon example + // // also should we zero the sensors on the follow motors just in case they're being used? + // } + + // don't override disable() or stop() because we *should* indeed use the base implementation of disabling/stopping each motor controller individually. Otherwise the following motors will try to follow a disabled motor, which may cause unexpected behavior (although realistically, it likely just gets set to zero and neutrallized by the deadband). + + @Override + public void set(double power) { + setFollowMode(); // make sure all motors are following the lead as we expect. Possible OPTIMIZATION: store we set the output type to something else on the follow motors (eg. Neutral), and only re-set follow mode if we did. + leadMotor.set(power); + } + + @Override + /** + * if using a Ramsete controller, make sure to disable voltage compensation, + * because voltages have an actual physical meaning from sysid. + * + * NOTE: CTRE BaseMotorController setVoltage just uses set() under the hood. + * Follow motors will use the same percent output as calculated by the + * setVoltage of the lead motor. This is fine, and equivalent to the base + * implementation (calling setVoltage on each motor) as that will just run the + * same voltage->power calculation for each motor. + */ + public void setVoltage(double voltage) { + setFollowMode(); + leadMotor.setVoltage(voltage); + } + + // no need to override setPower because the base class just uses set + // don't override setBrakeOnNeutral, setCoastOnNeutral, neutralOutput because we indeed want to set it individually on each motor. Otherwise, the followers might try to follow a disabled/neutral motor which might cause unexpected behavior. + // TODO: except do we actually yes need to? since it seems to not persist, see the warning about auto-disable here: https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces + + // ERROR HANDLING + static class REVLibThrowable extends IllegalStateException { + public REVLibThrowable(REVLibError err) { + super(err.toString()); + } + public REVLibThrowable(String msg, REVLibError err) { + super(msg + err.toString()); + } + public static void guard(REVLibError err) { + if (err != REVLibError.kOk) throw new REVLibThrowable(err); + } + } +} + diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index 8860637a..483a17e3 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -37,10 +37,11 @@ * - Minimum neutral deadband is 0.001 (according to the docs, https://v5.docs.ctr-electronics.com/en/latest/ch13_MC.html#neutral-deadband) * - Configure normallyClosed limit switches manually on the underlying motorControllers. We usually use normallyOpen limit switches, so you probably don't need to do this. */ -public class TalonMotorSubsystem extends BrakeableMotorSubsystem { +public class TalonMotorSubsystem extends SmartMotorSubsystem { private final int configTimeoutMs = 50; // milliseconds until the Talon gives up trying to configure private final int pid_idx = 0; // TODO: add support for auxillary pid private final int follow_motors_remote_filter_id = 0; // DONOT REMOVE, USED IN COMMENTED CODE BELOW; which filter (0 or 1) will be used to configure reading from the integrated encoder on the lead motor + private boolean pid_configured = false; // flag for command factories to check whether PID was configured public final TalonMotorController leadMotor; public final TalonMotorController[] followMotors; @@ -83,13 +84,15 @@ public class TalonMotorSubsystem extends BrakeableMotorSubsystem leadMotor.set(ControlMode.Velocity, setpointSupplier.getAsDouble())); } // public void zeroSensors() { From 957950bbf6a9b4e1b4194928555340f1647be4c2 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 <102199775+roboticsteam4904-2@users.noreply.github.com> Date: Tue, 28 Feb 2023 20:12:16 -0800 Subject: [PATCH 047/134] make smartMotorController extend WPILIB::MotorController Co-authored-by: Exr0n --- custom/motorcontrollers/SmartMotorController.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/custom/motorcontrollers/SmartMotorController.java b/custom/motorcontrollers/SmartMotorController.java index deda35c7..abf70b33 100644 --- a/custom/motorcontrollers/SmartMotorController.java +++ b/custom/motorcontrollers/SmartMotorController.java @@ -3,15 +3,15 @@ package org.usfirst.frc4904.standard.custom.motorcontrollers; +import edu.wpi.first.wpilibj.motorcontrol.MotorController; + /** * Represents a "smart" motor controller, like the TalonFX, TalonSRX, or SparkMax * * These should support brake mode, follow mode, limit switches, and various closed-loop control modes. */ -public interface SmartMotorController { - public abstract void set(double power); +public interface SmartMotorController extends MotorController { public abstract void setPower(double power); - public abstract void setVoltage(double voltage); public abstract SmartMotorController setBrakeOnNeutral(); public abstract SmartMotorController setCoastOnNeutral(); From f7519ee4812528965b7a5394ceb27de9d34ec355 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Wed, 1 Mar 2023 16:47:50 -0800 Subject: [PATCH 048/134] UT chassis; smartmotorsubsystem interface methods --- subsystems/chassis/WestCoastDrive.java | 94 ++++++++++++++++++++ subsystems/motor/SmartMotorSubsystem.java | 34 +++++-- subsystems/motor/SparkMaxMotorSubsystem.java | 57 ++++++++---- subsystems/motor/TalonMotorSubsystem.java | 71 ++++++++++++--- 4 files changed, 218 insertions(+), 38 deletions(-) create mode 100644 subsystems/chassis/WestCoastDrive.java diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java new file mode 100644 index 00000000..5a7c11e1 --- /dev/null +++ b/subsystems/chassis/WestCoastDrive.java @@ -0,0 +1,94 @@ +package org.usfirst.frc4904.standard.subsystems.chassis; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import org.usfirst.frc4904.standard.custom.motorcontrollers.SmartMotorController; +import org.usfirst.frc4904.standard.subsystems.motor.SmartMotorSubsystem; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.drive.DifferentialDrive.WheelSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class WestCoastDrive extends SubsystemBase { + private final SmartMotorSubsystem leftMotors; + private final SmartMotorSubsystem rightMotors; + private final DifferentialDriveKinematics kinematics; + private final double mps_to_rpm; + /** + * Represents a west coast drive chassis as a subsystem + * + * @param trackWidthMeters Track width, from horizontal center to center of the wheel, meters + * @param motorToWheelGearRatio Number of motor rotations for one wheel rotation. > 1 for gearing the motors down, eg. 13.88/1 + * @param wheelDiameterMeters + * @param leftMotorSubsystem SmartMotorSubsystem for the left wheels. Usually a TalonMotorSubsystem with two talons. + * @param rightMotorSubsystem SmartMotorSubsystem for the right wheels. Usually a TalonMotorSubsystem with two talons. + */ + WestCoastDrive(double trackWidthMeters, double motorToWheelGearRatio, double wheelDiameterMeters, SmartMotorSubsystem leftMotorSubsystem, SmartMotorSubsystem rightMotorSubsystem) { + leftMotors = leftMotorSubsystem; + rightMotors = rightMotorSubsystem; + kinematics = new DifferentialDriveKinematics(trackWidthMeters); // 2023 robot has track width ~19.5 inches, 5 in wheel diameter + mps_to_rpm = (Math.PI * wheelDiameterMeters) * motorToWheelGearRatio * 60; + // TODO: add requirements? + } + + /// methods + // public void moveCartesian(double xSpeed, double ySpeed, double turnSpeed) { + // if (xSpeed != 0) throw new IllegalArgumentException("West Coast Drive cannot move laterally!"); + // movePolar(ySpeed, 0, turnSpeed); + // } + + // public void movePolar(double speed, double heading, double turnSpeed) { + // if (heading != 0) throw new IllegalArgumentException("West Coast Drive cannot move at a non-zero heading!"); + // kinematics.toWheelSpeeds(ChassisSpeeds); + // } + // public void setWheelVelocities(WheelSpeeds wheelSpeeds) { + // this.leftMotors.set(wheelSpeeds.left); + // this.rightMotors.c_setVelocity() + // wheelSpeeds.left + // } + // public void setChassisVelocities(ChassisSpeeds chassisSpeeds) { + + // } + + /** + * A forever command that pulls drive velocities from a function and sends + * them to the motor's closed-loop control. + * + * For composition reasons, leftRightVelocitySupplier gets called twice per frame. OPTIMIZABLE + * + * @param leftRightVelocity A function that returns a pair containing the + * left and right velocities, m/s. + * + * @return the command to be scheduled + */ + public Command c_controlWheelVelocities(Supplier leftRightVelocitySupplier) { + var command = new ParallelCommandGroup( + this.leftMotors .c_controlRPM(() -> leftRightVelocitySupplier.get().left * mps_to_rpm), + this.rightMotors.c_controlRPM(() -> leftRightVelocitySupplier.get().right * mps_to_rpm) + ); + command.addRequirements(this); + return command; + } + /** + * A forever command that pulls chassis movement + * (forward speed and turn * radians/sec) from a * function and + * sends them to the motor's closed-loop * control. + */ + public Command c_controlChassisMovement(Supplier chassisSpeedVelocitySupplier) { + var cmd = this.run(() -> { + final var wheelRPMs = kinematics.toWheelSpeeds(chassisSpeedVelocitySupplier.get()); + this.leftMotors .setRPM(wheelRPMs.leftMetersPerSecond * mps_to_rpm); + this.rightMotors.setRPM(wheelRPMs.rightMetersPerSecond * mps_to_rpm); + }); + cmd.addRequirements(leftMotors); + cmd.addRequirements(rightMotors); + return cmd; + } +} diff --git a/subsystems/motor/SmartMotorSubsystem.java b/subsystems/motor/SmartMotorSubsystem.java index 5734264d..1f6b019a 100644 --- a/subsystems/motor/SmartMotorSubsystem.java +++ b/subsystems/motor/SmartMotorSubsystem.java @@ -1,5 +1,7 @@ package org.usfirst.frc4904.standard.subsystems.motor; +import java.util.function.DoubleSupplier; + import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.custom.motorcontrollers.SmartMotorController; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; @@ -172,7 +174,7 @@ public void coast() { setCoastOnNeutral(); neutralOutput(); } - + /// COMMANDS // TODO: remove the "Replaces *.java in pre-2023 standard" and "eg. button1.onTrue(motor.c_idle())" when they become unnecessary. @@ -213,7 +215,7 @@ public void coast() { * * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_setPowerHold()) */ - public Command c_setPowerHold(double power) { return this.run(() -> this.setPower(power)); } + public Command c_holdPower(double power) { return this.run(() -> this.setPower(power)); } /** * Repeatedly set motor voltage. Command version of setVoltagePower(). @@ -227,21 +229,35 @@ public void coast() { * * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_setVoltageHold()) */ - public Command c_setVoltageHold(double voltage) { return this.run(() -> this.setVoltage(voltage)); } + public Command c_holdVoltage(double voltage) { return this.run(() -> this.setVoltage(voltage)); } + /** * Enables brake mode on and brakes each motor. Command version of brake(). * * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_brake()) */ - public Command c_brake() { - return this.runOnce(() -> this.brake()); - } + public Command c_brake() { return this.runOnce(() -> this.brake()); } + /** * Enables coast mode on and coasts each motor. Command version of coast(). * * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_coast()) */ - public Command c_coast() { - return this.runOnce(() -> this.coast()); - } + public Command c_coast() { return this.runOnce(() -> this.coast()); } + + /** + * Write PIDF values to hardware, which will be used with the closed loop control commands + * + * TODO: replace with ezControl + */ + public abstract void setRPM(double voltage); + public abstract void configPIDF(double p, double i, double d, double f); + public abstract Command c_controlRPM(DoubleSupplier setpointSupplier); + public abstract Command c_holdRPM(double setpoint); + public abstract Command c_controlPosition(DoubleSupplier setpointSupplier); + public abstract Command c_holdPosition(double setpoint); + + // deprecate the set-and-forget commands, because commands should not end before the motor is in a "stop" state + @Deprecated public abstract Command c_setRPM(double setpoint); + @Deprecated public abstract Command c_setPosition(double setpoint); } diff --git a/subsystems/motor/SparkMaxMotorSubsystem.java b/subsystems/motor/SparkMaxMotorSubsystem.java index b90aac87..9d9eb309 100644 --- a/subsystems/motor/SparkMaxMotorSubsystem.java +++ b/subsystems/motor/SparkMaxMotorSubsystem.java @@ -3,25 +3,16 @@ import java.util.function.DoubleSupplier; import java.util.stream.Stream; -import org.usfirst.frc4904.standard.custom.CustomCAN; import org.usfirst.frc4904.standard.custom.motorcontrollers.CustomCANSparkMax; -import org.usfirst.frc4904.standard.custom.motorcontrollers.TalonMotorController; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.ctre.phoenix.motorcontrol.LimitSwitchSource; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; import com.revrobotics.REVLibError; import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.SparkMaxPIDController; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.Command; /** @@ -45,9 +36,6 @@ * - Specifying respectLeadMotorLimitSwitches=false will not disable the limit switches if they were previously enabled. This is to allow you to configure limit switches on the underlying motor controllers before passing them in if necessary. */ public class SparkMaxMotorSubsystem extends SmartMotorSubsystem { - private final int configTimeoutMs = 50; // milliseconds until the Talon gives up trying to configure - private final int pid_idx = 0; // TODO: add support for auxillary pid - private final int follow_motors_remote_filter_id = 0; // DONOT REMOVE, USED IN COMMENTED CODE BELOW; which filter (0 or 1) will be used to configure reading from the integrated encoder on the lead motor private SparkMaxPIDController controller = null; public final CustomCANSparkMax leadMotor; public final CustomCANSparkMax[] followMotors; @@ -177,6 +165,15 @@ private void setFollowMode() { } } + /** + * Set RPM of the motor, using SparkMAX internal PID. + * + * You must call .configPIDF before using this method + */ + public void setRPM(double rpm) { + controller.setReference(rpm, ControlType.kVelocity); + } + // TODO the following methods are not thought out or documented /** * The F value provided here will be overwritten if provided to subsystem.leadMotor.set; note that if you do that, it will bypass the subystem requirements check @@ -204,14 +201,38 @@ public void configPIDF(double p, double i, double d, double f) { * * @return the command to schedule */ - public Command c_controlVelocity(DoubleSupplier setpointSupplier) { + public Command c_controlRPM(DoubleSupplier setpointSupplier) { if (controller == null) throw new IllegalArgumentException(name + " tried to use c_controlVelocity without first configPIDF()-ing."); - return this.run(() -> controller.setReference(setpointSupplier.getAsDouble(), ControlType.kVelocity)); + return this.run(() -> setRPM(setpointSupplier.getAsDouble())); + } + + + @Override + public Command c_setRPM(double setpoint) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'c_setVelocity'"); + } + @Override + public Command c_holdRPM(double setpoint) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'c_holdVelocity'"); + } + // TODO: need to zero sensors? + @Override + public Command c_setPosition(double setpoint) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'c_setPosition'"); + } + @Override + public Command c_controlPosition(DoubleSupplier setpointSupplier) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'c_controlPosition'"); + } + @Override + public Command c_holdPosition(double setpoint) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'c_holdPosition'"); } - // public void zeroSensors() { - // // leadMotor.getSensorCollection // ?? doesn't exist; but it's in the CTRE falcon example - // // also should we zero the sensors on the follow motors just in case they're being used? - // } // don't override disable() or stop() because we *should* indeed use the base implementation of disabling/stopping each motor controller individually. Otherwise the following motors will try to follow a disabled motor, which may cause unexpected behavior (although realistically, it likely just gets set to zero and neutrallized by the deadband). diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index 483a17e3..ec16ec35 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -38,6 +38,7 @@ * - Configure normallyClosed limit switches manually on the underlying motorControllers. We usually use normallyOpen limit switches, so you probably don't need to do this. */ public class TalonMotorSubsystem extends SmartMotorSubsystem { + private static final int ENCODER_COUNTS_PER_REV = 2048; private final int configTimeoutMs = 50; // milliseconds until the Talon gives up trying to configure private final int pid_idx = 0; // TODO: add support for auxillary pid private final int follow_motors_remote_filter_id = 0; // DONOT REMOVE, USED IN COMMENTED CODE BELOW; which filter (0 or 1) will be used to configure reading from the integrated encoder on the lead motor @@ -187,9 +188,12 @@ private void setFollowMode() { * * See docstrings on the methods used in the implementation for physical units */ + @Override public void configPIDF(double p, double i, double d, double f) { // feedback sensor configuration (for PID) leadMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, pid_idx, configTimeoutMs); + // make sure to update the static final ENCODER_COUNTS_PER_REV if you are using different encoders. Better yet, add a feedbackSensor argument to this method + // for (var fm : followMotors) { // don't think this is needed because we are using follow mode. only needed for aux output // fm.configRemoteFeedbackFilter(leadMotor.getDeviceID(), RemoteSensorSource.TalonFX_SelectedSensor /* enum internals has TalonFX = TalonSRX as of 2023 */, follow_motors_remote_filter_id, configTimeoutMs); // fm.configSelectedFeedbackSensor(follow_motors_remote_filter_id == 0 ? FeedbackDevice.RemoteSensor0 : FeedbackDevice.RemoteSensor1 /* enum internals has TalonFX_SelectedSensor = TalonSRX_SelectedSensor as of 2023 */, pid_idx, configTimeoutMs); @@ -206,16 +210,6 @@ public void configPIDF(double p, double i, double d, double f) { // TODO: integral zone and closedLoopPeakOUtput? // other things in the example: motionmagic config and statusframeperiod (for updating sensor status to the aux motor?) } - /** - * - * @param setpointSupplier a function that returns a double, units = encoder ticks per 100ms - * - * @return - */ - public Command c_controlVelocity(DoubleSupplier setpointSupplier) { - if (pid_configured == false) throw new IllegalArgumentException(name + " tried to use c_controlVelocity without first configPIDF()-ing."); - return this.run(() -> leadMotor.set(ControlMode.Velocity, setpointSupplier.getAsDouble())); - } // public void zeroSensors() { // // leadMotor.getSensorCollection // ?? doesn't exist; but it's in the CTRE falcon example // // also should we zero the sensors on the follow motors just in case they're being used? @@ -228,8 +222,16 @@ public void set(double power) { setFollowMode(); // make sure all motors are following the lead as we expect. Possible OPTIMIZATION: store we set the output type to something else on the follow motors (eg. Neutral), and only re-set follow mode if we did. leadMotor.set(power); } + /** + * Consider using .c_controlRPM to stream RPM values to this motor in a command. + * + * You must call .configPIDF before using this method. + * @param rpm + */ + public void setRPM(double rpm) { + leadMotor.set(ControlMode.Velocity, rpm / 60 / 10 * ENCODER_COUNTS_PER_REV); // velocity units are in encoder counts per 100ms + } - @Override /** * if using a Ramsete controller, make sure to disable voltage compensation, * because voltages have an actual physical meaning from sysid. @@ -240,10 +242,57 @@ public void set(double power) { * implementation (calling setVoltage on each motor) as that will just run the * same voltage->power calculation for each motor. */ + @Override public void setVoltage(double voltage) { setFollowMode(); leadMotor.setVoltage(voltage); } + /** + * You must call .configPIDF before using this method. + * + * @param rpm + * @return the command to be scheduled. + */ + @Override @Deprecated + public Command c_setRPM(double rpm) { return this.runOnce(() -> setRPM(rpm)); } + /** + * + * @param setpointSupplier a function that returns a double, rpm + * + * @return + */ + @Override + public Command c_controlRPM(DoubleSupplier setpointSupplier) { + if (pid_configured == false) throw new IllegalArgumentException(name + " tried to use c_controlVelocity without first configPIDF()-ing."); + return this.run(() -> setRPM(setpointSupplier.getAsDouble())); + } + @Override + public Command c_holdRPM(double setpoint) { + if (pid_configured == false) throw new IllegalArgumentException(name + " tried to use c_controlVelocity without first configPIDF()-ing."); + return this.run(() -> setRPM(setpoint)); // TODO: use motionmagic + } + /** + * Command that sets the position setpoint and immedietly ends. + */ + @Override @Deprecated + public Command c_setPosition(double setpoint) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'c_setPosition'"); + } + @Override + public Command c_controlPosition(DoubleSupplier setpointSupplier) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'c_controlPosition'"); + } + /** + * Hold the position using smart motion (on-the-fly motion-profile generation). + */ + @Override + public Command c_holdPosition(double setpoint) { + // TODO: https://github.com/REVrobotics/SPARK-MAX-Examples/tree/master/Java/Smart%20Motion%20Example + // TODO: also probably have to implement a method to configure smartmotion cruise velocity + throw new UnsupportedOperationException("Unimplemented method 'c_holdPosition'"); + } // no need to override setPower because the base class just uses set // don't override setBrakeOnNeutral, setCoastOnNeutral, neutralOutput because we indeed want to set it individually on each motor. Otherwise, the followers might try to follow a disabled/neutral motor which might cause unexpected behavior. From 40887e297dd631192f991706f3f68f4238c3c96a Mon Sep 17 00:00:00 2001 From: Exr0n Date: Wed, 1 Mar 2023 17:24:57 -0800 Subject: [PATCH 049/134] replace MotorPos/Vel; WestCoastDrive partial feature parity --- commands/motor/MotorPositionConstant.java | 106 ------------ commands/motor/MotorPositionSet.java | 77 --------- commands/motor/MotorVelocitySet.java | 101 ----------- subsystems/chassis/SensorDrive.java | 196 +++++++++++----------- subsystems/chassis/WestCoastDrive.java | 103 ++++++++---- subsystems/motor/PositionSensorMotor.java | 50 ------ subsystems/motor/VelocitySensorMotor.java | 54 ------ 7 files changed, 167 insertions(+), 520 deletions(-) delete mode 100644 commands/motor/MotorPositionConstant.java delete mode 100644 commands/motor/MotorPositionSet.java delete mode 100644 commands/motor/MotorVelocitySet.java delete mode 100644 subsystems/motor/PositionSensorMotor.java delete mode 100644 subsystems/motor/VelocitySensorMotor.java diff --git a/commands/motor/MotorPositionConstant.java b/commands/motor/MotorPositionConstant.java deleted file mode 100644 index a2910e44..00000000 --- a/commands/motor/MotorPositionConstant.java +++ /dev/null @@ -1,106 +0,0 @@ -package org.usfirst.frc4904.standard.commands.motor; - -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor; -import edu.wpi.first.wpilibj2.command.CommandBase; - -/** - * MotorPositionConstant is a Command that runs while setting a SensorMotor's - * position to the provided (double) value. If (boolean) endOnArrival is - * provided and set to false, the command will run indefinitely. Otherwise, the - * command will end when the motor#onTarget() returns true (as set by internal - * PID). - * - */ -public class MotorPositionConstant extends CommandBase { - protected PositionSensorMotor motor; - protected double position; - protected boolean endOnArrival; - protected final CommandBase fallbackCommand; - - /** - * Constructor MotorPositionConstant is a Command that runs while setting a - * SensorMotor's position to the provided (double) value. - * - * @param name - * @param motor - * @param position position to set the motor to - * @param endOnArrival If (boolean) endOnArrival is provided and set to - * false, the command will run indefinitely. Otherwise, - * the command will end when the motor#onTarget() returns - * true (as set by internal PID). - * @param fallbackCommand If the sensor fails for some reason, this command will - * be cancelled, then the fallbackCommand will start - */ - public MotorPositionConstant(String name, PositionSensorMotor motor, double position, boolean endOnArrival, - CommandBase fallbackCommand) { - super(); - setName(name); - addRequirements(motor); - this.motor = motor; - this.position = position; - this.endOnArrival = endOnArrival; - this.fallbackCommand = fallbackCommand; - } - - /** - * @param motor - * @param position - * @param endOnArrival - */ - public MotorPositionConstant(PositionSensorMotor motor, double position, boolean endOnArrival) { - this("MotorPositionConstant", motor, position, endOnArrival, null); - } - - /** - * @param motor - * @param position - */ - public MotorPositionConstant(PositionSensorMotor motor, double position) { - this("MotorPositionConstant", motor, position, true, null); - } - - @Override - public void initialize() { - try { - motor.reset(); - motor.enableMotionController(); - motor.setPositionSafely(position); - } catch (InvalidSensorException e) { - cancel(); - if (fallbackCommand != null) { - fallbackCommand.schedule(); - } - return; - } - } - - @Override - public void execute() { - if (motor.checkSensorException() != null) { - cancel(); - if (fallbackCommand != null) { - fallbackCommand.schedule(); - } - } - } - - @Override - public boolean isFinished() { - if (endOnArrival) { - return motor.onTarget(); - } - return false; - } - - @Override - public void end(boolean interrupted) { - if (!interrupted) { - motor.disableMotionController(); - // Please just die in a hole and never come back - if (fallbackCommand != null && fallbackCommand.isScheduled()) { - fallbackCommand.cancel(); - } - } - } -} diff --git a/commands/motor/MotorPositionSet.java b/commands/motor/MotorPositionSet.java deleted file mode 100644 index 352a68d2..00000000 --- a/commands/motor/MotorPositionSet.java +++ /dev/null @@ -1,77 +0,0 @@ -package org.usfirst.frc4904.standard.commands.motor; - -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor; -import edu.wpi.first.wpilibj2.command.CommandBase; - -/** - * Sets a motor to a position and keeps it there using an encoder. - */ -public class MotorPositionSet extends CommandBase { - protected PositionSensorMotor motor; - protected double position; - protected final CommandBase fallbackCommand; - - /** - * Constructor. The MotorSensorHold command holds a motor to a position. - * - * @param motor A Motor that also implements PositionSensorMotor. - * @param fallbackCommand If the sensor fails for some reason, this command will - * be cancelled, then the fallbackCommand will start - */ - public MotorPositionSet(String name, PositionSensorMotor motor, CommandBase fallbackCommand) { - super(); - setName(name); - addRequirements(motor); - this.motor = motor; - this.fallbackCommand = fallbackCommand; - } - - /** - * Constructor. The MotorSensorHold command holds a motor to a position. - * - * @param motor A Motor that also implements PositionSensorMotor. - */ - public MotorPositionSet(PositionSensorMotor motor) { - this("MotorPositionSet", motor, null); - } - - /** - * Sets the motor to this position. - * - * @param position The position to set the motor to. - */ - public void setPosition(double position) { - this.position = position; - } - - @Override - public void initialize() { - try { - motor.reset(); - motor.enableMotionController(); - motor.setPositionSafely(position); - } catch (InvalidSensorException e) { - cancel(); - if (fallbackCommand != null) { - fallbackCommand.schedule(); - } - } - } - - @Override - public void execute() { - Exception potentialSensorException = motor.checkSensorException(); - if (potentialSensorException != null) { - cancel(); - if (fallbackCommand != null && !fallbackCommand.isScheduled()) { - fallbackCommand.schedule(); - } - } - } - - @Override - public boolean isFinished() { - return false; - } -} diff --git a/commands/motor/MotorVelocitySet.java b/commands/motor/MotorVelocitySet.java deleted file mode 100644 index 8af593d2..00000000 --- a/commands/motor/MotorVelocitySet.java +++ /dev/null @@ -1,101 +0,0 @@ -package org.usfirst.frc4904.standard.commands.motor; - -import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.commands.Noop; -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -import org.usfirst.frc4904.standard.subsystems.motor.VelocitySensorMotor; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -/** - * Sets a motor to a position and keeps it there using an encoder. - */ -public class MotorVelocitySet extends CommandBase { - protected VelocitySensorMotor motor; - protected double velocity; - protected final CommandBase fallbackCommand; - - /** - * Constructor. The MotorVelocitySet command brings a motor to a velocity. - * - * @param motor A Motor that also implements VelocitySensorMotor. - * @param velocity the velocity to set the motor to. - * @param fallbackCommand If the sensor fails for some reason, this command will - * be cancelled, then the fallbackCommand will start - */ - public MotorVelocitySet(String name, VelocitySensorMotor motor, double velocity, CommandBase fallbackCommand) { - super(); - setName(name); - // addRequirements(motor); - this.motor = motor; - this.fallbackCommand = fallbackCommand; - this.setVelocity(velocity); - } - - /** - * Constructor. The MotorVelocitySet command brings a motor to a velocity. - * - * @param motor A Motor that also implements VelocitySensorMotor. - * @param velocity the velocity to set the motor to. - */ - public MotorVelocitySet(String name, VelocitySensorMotor motor, double velocity) { - this(name, motor, velocity, new Noop()); - } - - /** - * Constructor. The MotorVelocitySet command brings a motor to a velocity. - * - * @param motor A Motor that also implements VelocitySensorMotor. - * @param velocity the velocity to set the motor to. - */ - public MotorVelocitySet(VelocitySensorMotor motor, double velocity) { - this("MotorVelocitySet", motor, velocity); - } - - /** - * Sets the motor to this velocity. - * - * @param position The velocity to set the motor to. - */ - public void setVelocity(double velocity) { - this.velocity = velocity; - } - - @Override - public void initialize() { - try { - motor.reset(); - motor.enableMotionController(); - motor.set(velocity); - } catch (InvalidSensorException e) { - cancel(); - fallbackCommand.schedule(); - } - } - - @Override - public void execute() { - motor.set(velocity); - Exception potentialSensorException = motor.checkSensorException(); - if (potentialSensorException != null) { - cancel(); - if (!fallbackCommand.isScheduled()) { - fallbackCommand.schedule(); - } - } - } - - @Override - public boolean isFinished() { - return false; - } - - @Override - public void end(boolean interrupted) { - motor.disableMotionController(); - motor.set(0.0); - if (!interrupted) { - motor.set(0.0); - } - } -} diff --git a/subsystems/chassis/SensorDrive.java b/subsystems/chassis/SensorDrive.java index 5421114a..42f92c52 100644 --- a/subsystems/chassis/SensorDrive.java +++ b/subsystems/chassis/SensorDrive.java @@ -1,110 +1,110 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ +// /*----------------------------------------------------------------------------*/ +// /* Copyright (c) 2019 FIRST. All Rights Reserved. */ +// /* Open Source Software - may be modified and shared by FRC teams. The code */ +// /* must be accompanied by the FIRST BSD license file in the root directory of */ +// /* the project. */ +// /*----------------------------------------------------------------------------*/ -package org.usfirst.frc4904.standard.subsystems.chassis; -// WAS PID SOURCE -import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; -import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder; -import org.usfirst.frc4904.standard.custom.sensors.NavX; +// package org.usfirst.frc4904.standard.subsystems.chassis; +// // WAS PID SOURCE +// import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; +// import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder; +// import org.usfirst.frc4904.standard.custom.sensors.NavX; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; -import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Subsystem; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; +// import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +// import edu.wpi.first.wpilibj2.command.CommandScheduler; +// import edu.wpi.first.wpilibj2.command.Subsystem; -public class SensorDrive implements Subsystem { // Based largely on - // https://github.com/wpilibsuite/allwpilib/blob/master/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java - private final TankDrive driveBase; - private final CANTalonEncoder leftEncoder; - private final CANTalonEncoder rightEncoder; - private final NavX gyro; - private final DifferentialDriveOdometry odometry; +// public class SensorDrive implements Subsystem { // Based largely on +// // https://github.com/wpilibsuite/allwpilib/blob/master/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java +// private final TankDrive driveBase; +// private final CANTalonEncoder leftEncoder; +// private final CANTalonEncoder rightEncoder; +// private final NavX gyro; +// private final DifferentialDriveOdometry odometry; - /** - * Creates a new DriveSubsystem. - */ - public SensorDrive(TankDrive driveBase, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, NavX gyro, Pose2d initialPose) { - this.driveBase = driveBase; - this.leftEncoder = leftEncoder; - this.rightEncoder = rightEncoder; - this.gyro = gyro; +// /** +// * Creates a new DriveSubsystem. +// */ +// public SensorDrive(TankDrive driveBase, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, NavX gyro, Pose2d initialPose) { +// this.driveBase = driveBase; +// this.leftEncoder = leftEncoder; +// this.rightEncoder = rightEncoder; +// this.gyro = gyro; - resetEncoders(); - odometry = new DifferentialDriveOdometry(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), initialPose); - CommandScheduler.getInstance().registerSubsystem(this); - } +// resetEncoders(); +// odometry = new DifferentialDriveOdometry(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), initialPose); +// CommandScheduler.getInstance().registerSubsystem(this); +// } - @Override - public void periodic() { - odometry.update(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance()); - } +// @Override +// public void periodic() { +// odometry.update(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance()); +// } - /** - * Returns the currently-estimated pose of the robot. - * - * @return The pose. - */ - public Pose2d getPose() { - return odometry.getPoseMeters(); - } +// /** +// * Returns the currently-estimated pose of the robot. +// * +// * @return The pose. +// */ +// public Pose2d getPose() { +// return odometry.getPoseMeters(); +// } - /** - * Returns the current wheel speeds of the robot. - * - * @return The current wheel speeds. - */ - public DifferentialDriveWheelSpeeds getWheelSpeeds() { - return new DifferentialDriveWheelSpeeds(leftEncoder.getRate(), rightEncoder.getRate()); - } +// /** +// * Returns the current wheel speeds of the robot. +// * +// * @return The current wheel speeds. +// */ +// public DifferentialDriveWheelSpeeds getWheelSpeeds() { +// return new DifferentialDriveWheelSpeeds(leftEncoder.getRate(), rightEncoder.getRate()); +// } - /** - * Resets the odometry to the specified pose. - * - * @param pose The pose to which to set the odometry. - */ - public void resetOdometry(Pose2d pose) { - resetEncoders(); - odometry.resetPosition(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), pose); - } +// /** +// * Resets the odometry to the specified pose. +// * +// * @param pose The pose to which to set the odometry. +// */ +// public void resetOdometry(Pose2d pose) { +// resetEncoders(); +// odometry.resetPosition(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), pose); +// } - // /** - // * Controls the left and right sides of the drive directly with voltages. - // * - // * @param leftVolts the commanded left output - // * @param rightVolts the commanded right output - // */ - // public void tankDriveVolts(double leftVolts, double rightVolts) { - // MotorSubsystem[] motors = driveBase.getMotors(); - // if (motors.length == 2) { - // driveBase.getMotors()[0].setVoltage(leftVolts); - // driveBase.getMotors()[1].setVoltage(rightVolts); - // } else { - // driveBase.getMotors()[0].setVoltage(leftVolts); - // driveBase.getMotors()[1].setVoltage(leftVolts); - // driveBase.getMotors()[2].setVoltage(rightVolts); - // driveBase.getMotors()[3].setVoltage(rightVolts); - // } - // } +// // /** +// // * Controls the left and right sides of the drive directly with voltages. +// // * +// // * @param leftVolts the commanded left output +// // * @param rightVolts the commanded right output +// // */ +// // public void tankDriveVolts(double leftVolts, double rightVolts) { +// // MotorSubsystem[] motors = driveBase.getMotors(); +// // if (motors.length == 2) { +// // driveBase.getMotors()[0].setVoltage(leftVolts); +// // driveBase.getMotors()[1].setVoltage(rightVolts); +// // } else { +// // driveBase.getMotors()[0].setVoltage(leftVolts); +// // driveBase.getMotors()[1].setVoltage(leftVolts); +// // driveBase.getMotors()[2].setVoltage(rightVolts); +// // driveBase.getMotors()[3].setVoltage(rightVolts); +// // } +// // } - /** - * Resets the drive encoders to currently read a position of 0. - */ - public void resetEncoders() { - leftEncoder.reset(); - rightEncoder.reset(); - } +// /** +// * Resets the drive encoders to currently read a position of 0. +// */ +// public void resetEncoders() { +// leftEncoder.reset(); +// rightEncoder.reset(); +// } - /** - * Returns the heading of the robot. - * - * @return the robot's heading in degrees, from 180 to 180 - */ - public double getHeading() { - return gyro.getYaw() * -1; - } -} +// /** +// * Returns the heading of the robot. +// * +// * @return the robot's heading in degrees, from 180 to 180 +// */ +// public double getHeading() { +// return gyro.getYaw() * -1; +// } +// } diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 5a7c11e1..42f082a4 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -1,19 +1,16 @@ package org.usfirst.frc4904.standard.subsystems.chassis; -import java.util.function.DoubleSupplier; import java.util.function.Supplier; import org.usfirst.frc4904.standard.custom.motorcontrollers.SmartMotorController; import org.usfirst.frc4904.standard.subsystems.motor.SmartMotorSubsystem; -import edu.wpi.first.math.Pair; +import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.drive.DifferentialDrive.WheelSpeeds; +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class WestCoastDrive extends SubsystemBase { @@ -39,23 +36,36 @@ public class WestCoastDrive extends SubsystemB } /// methods - // public void moveCartesian(double xSpeed, double ySpeed, double turnSpeed) { - // if (xSpeed != 0) throw new IllegalArgumentException("West Coast Drive cannot move laterally!"); - // movePolar(ySpeed, 0, turnSpeed); - // } - // public void movePolar(double speed, double heading, double turnSpeed) { - // if (heading != 0) throw new IllegalArgumentException("West Coast Drive cannot move at a non-zero heading!"); - // kinematics.toWheelSpeeds(ChassisSpeeds); - // } - // public void setWheelVelocities(WheelSpeeds wheelSpeeds) { - // this.leftMotors.set(wheelSpeeds.left); - // this.rightMotors.c_setVelocity() - // wheelSpeeds.left - // } - // public void setChassisVelocities(ChassisSpeeds chassisSpeeds) { - - // } + /** + * Convention: +x forwards, +y right, +z down + * + * @param xSpeed + * @param ySpeed + * @param turnSpeed + */ + @Deprecated + public void moveCartesian(double xSpeed, double ySpeed, double turnSpeed) { + if (ySpeed != 0) throw new IllegalArgumentException("West Coast Drive cannot move laterally!"); + setChassisVelocity(new ChassisSpeeds(xSpeed, ySpeed, turnSpeed)); + } + @Deprecated + public void movePolar(double speed, double heading, double turnSpeed) { + if (heading != 0) throw new IllegalArgumentException("West Coast Drive cannot move at a non-zero heading!"); + setChassisVelocity(new ChassisSpeeds(speed, 0, turnSpeed)); + } + public void setWheelVelocities(DifferentialDriveWheelSpeeds wheelSpeeds) { + this.leftMotors .setRPM(wheelSpeeds.leftMetersPerSecond * mps_to_rpm); + this.rightMotors.setRPM(wheelSpeeds.rightMetersPerSecond * mps_to_rpm); + } + public void setChassisVelocity(ChassisSpeeds chassisSpeeds) { + final var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); + setWheelVelocities(wheelSpeeds); + } + public void setWheelVoltages(DifferentialDriveWheelVoltages wheelVoltages) { + this.leftMotors.setVoltage(wheelVoltages.left); + this.rightMotors.setVoltage(wheelVoltages.right); + } /** * A forever command that pulls drive velocities from a function and sends @@ -68,27 +78,52 @@ public class WestCoastDrive extends SubsystemB * * @return the command to be scheduled */ - public Command c_controlWheelVelocities(Supplier leftRightVelocitySupplier) { - var command = new ParallelCommandGroup( - this.leftMotors .c_controlRPM(() -> leftRightVelocitySupplier.get().left * mps_to_rpm), - this.rightMotors.c_controlRPM(() -> leftRightVelocitySupplier.get().right * mps_to_rpm) - ); - command.addRequirements(this); - return command; + public Command c_controlWheelVelocities(Supplier leftRightVelocitySupplier) { + var cmd = this.run(() -> setWheelVelocities(leftRightVelocitySupplier.get())); // this.run() runs repeatedly + cmd.addRequirements(leftMotors); + cmd.addRequirements(rightMotors); + return cmd; } /** * A forever command that pulls chassis movement * (forward speed and turn * radians/sec) from a * function and * sends them to the motor's closed-loop * control. */ - public Command c_controlChassisMovement(Supplier chassisSpeedVelocitySupplier) { - var cmd = this.run(() -> { - final var wheelRPMs = kinematics.toWheelSpeeds(chassisSpeedVelocitySupplier.get()); - this.leftMotors .setRPM(wheelRPMs.leftMetersPerSecond * mps_to_rpm); - this.rightMotors.setRPM(wheelRPMs.rightMetersPerSecond * mps_to_rpm); - }); + public Command c_controlChassisVelocity(Supplier chassisSpeedsSupplier) { + var cmd = this.run(() -> setChassisVelocity(chassisSpeedsSupplier.get())); // this.run() runs repeatedly cmd.addRequirements(leftMotors); cmd.addRequirements(rightMotors); return cmd; } + /** + * A forever command that pulls left and right wheel voltages from a + * function. + */ + public Command c_controlWheelVoltages(Supplier wheelVoltageSupplier) { + var cmd = this.run(() -> setWheelVoltages(wheelVoltageSupplier.get())); // this.run() runs repeatedly + cmd.addRequirements(leftMotors); + cmd.addRequirements(rightMotors); + return cmd; + } + + + // convienence commands for feature parity with pre-2023 standard + /** + * moveCartesian with (x, y, turn) for timeout seconds. + * + * Replaces ChassisConstant in pre-2023 standard. + */ + @Deprecated + public Command c_chassisConstant(double x, double y, double turn, double timeout) { + return this.run(() -> moveCartesian(x, y, turn)).withTimeout(timeout); + } + /** + * Enters idle mode on underlying motor controllers. + * + * Replaces ChassisIdle in pre-2023 standard. + */ + public Command c_idle() { + return Commands.parallel(leftMotors.c_idle(), rightMotors.c_idle()); // TODO do we need to require the chassis subsystem for this? else chassis will read as unrequired, but all of it's subcomponents will be required. + } + // TODO: write the others. goodfirstissue } diff --git a/subsystems/motor/PositionSensorMotor.java b/subsystems/motor/PositionSensorMotor.java deleted file mode 100644 index 46ec3ca7..00000000 --- a/subsystems/motor/PositionSensorMotor.java +++ /dev/null @@ -1,50 +0,0 @@ -package org.usfirst.frc4904.standard.subsystems.motor; - -import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; -import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; -import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; -import edu.wpi.first.wpilibj.motorcontrol.MotorController; - -public class PositionSensorMotor extends SensorMotor { - public PositionSensorMotor(String name, boolean isInverted, SpeedModifier speedModifier, - MotionController motionController, MotorController... motors) { - super(name, isInverted, speedModifier, motionController, motors); - setName(name); - } - - public PositionSensorMotor(String name, boolean isInverted, MotionController motionController, - MotorController... motors) { - this(name, isInverted, new IdentityModifier(), motionController, motors); - } - - public PositionSensorMotor(String name, SpeedModifier speedModifier, MotionController motionController, - MotorController... motors) { - this(name, false, speedModifier, motionController, motors); - } - - public PositionSensorMotor(String name, MotionController motionController, MotorController... motors) { - this(name, false, new IdentityModifier(), motionController, motors); - } - - public PositionSensorMotor(boolean isInverted, SpeedModifier speedModifier, MotionController motionController, - MotorController... motors) { - this("PositionSensorMotor", isInverted, speedModifier, motionController, motors); - } - - public PositionSensorMotor(boolean isInverted, MotionController motionController, MotorController... motors) { - this("PositionSensorMotor", isInverted, motionController, motors); - } - - public PositionSensorMotor(SpeedModifier speedModifier, MotionController motionController, - MotorController... motors) { - this("PositionSensorMotor", speedModifier, motionController, motors); - } - - public PositionSensorMotor(MotionController motionController, MotorController... motors) { - this("PositionSensorMotor", motionController, motors); - } - - public void setPosition(double position) { - motionController.setSetpoint(position); - } -} diff --git a/subsystems/motor/VelocitySensorMotor.java b/subsystems/motor/VelocitySensorMotor.java deleted file mode 100644 index a1b4f482..00000000 --- a/subsystems/motor/VelocitySensorMotor.java +++ /dev/null @@ -1,54 +0,0 @@ -package org.usfirst.frc4904.standard.subsystems.motor; - -import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; -import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; -import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; -import edu.wpi.first.wpilibj.motorcontrol.MotorController; - -public class VelocitySensorMotor extends SensorMotor { - public VelocitySensorMotor(String name, boolean isInverted, SpeedModifier slopeController, - MotionController motionController, MotorController... motors) { - super(name, isInverted, slopeController, motionController, motors); - } - - public VelocitySensorMotor(String name, boolean isInverted, MotionController motionController, - MotorController... motors) { - this(name, isInverted, new IdentityModifier(), motionController, motors); - } - - public VelocitySensorMotor(String name, SpeedModifier speedModifier, MotionController motionController, - MotorController... motors) { - this(name, false, speedModifier, motionController, motors); - } - - public VelocitySensorMotor(String name, MotionController motionController, MotorController... motors) { - this(name, false, new IdentityModifier(), motionController, motors); - } - - public VelocitySensorMotor(boolean isInverted, SpeedModifier speedModifier, MotionController motionController, - MotorController... motors) { - this("VelocitySensorMotor", isInverted, speedModifier, motionController, motors); - } - - public VelocitySensorMotor(boolean isInverted, MotionController motionController, MotorController... motors) { - this("VelocitySensorMotor", isInverted, motionController, motors); - } - - public VelocitySensorMotor(SpeedModifier speedModifier, MotionController motionController, - MotorController... motors) { - this("VelocitySensorMotor", speedModifier, motionController, motors); - } - - public VelocitySensorMotor(MotionController motionController, MotorController... motors) { - this("VelocitySensorMotor", motionController, motors); - } - - @Override - public void set(double speed) { - if (motionController.isEnabled()) { - motionController.setSetpoint(speed); - } else { - super.set(speed); - } - } -} From b7bd4f19bf9be8ce9d3cb59808edc466b5d8d135 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Wed, 1 Mar 2023 17:41:18 -0800 Subject: [PATCH 050/134] make constructor public Co-authored-by: annvoor --- subsystems/chassis/WestCoastDrive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 42f082a4..ddc2d8bd 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -27,7 +27,7 @@ public class WestCoastDrive extends SubsystemB * @param leftMotorSubsystem SmartMotorSubsystem for the left wheels. Usually a TalonMotorSubsystem with two talons. * @param rightMotorSubsystem SmartMotorSubsystem for the right wheels. Usually a TalonMotorSubsystem with two talons. */ - WestCoastDrive(double trackWidthMeters, double motorToWheelGearRatio, double wheelDiameterMeters, SmartMotorSubsystem leftMotorSubsystem, SmartMotorSubsystem rightMotorSubsystem) { + public WestCoastDrive(double trackWidthMeters, double motorToWheelGearRatio, double wheelDiameterMeters, SmartMotorSubsystem leftMotorSubsystem, SmartMotorSubsystem rightMotorSubsystem) { leftMotors = leftMotorSubsystem; rightMotors = rightMotorSubsystem; kinematics = new DifferentialDriveKinematics(trackWidthMeters); // 2023 robot has track width ~19.5 inches, 5 in wheel diameter From f16c2c8166e0ba6d103247051b49fc341576ffc3 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 2 Mar 2023 20:44:43 -0800 Subject: [PATCH 051/134] use MotorControllerType instead of SMC for clarity --- subsystems/chassis/WestCoastDrive.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index ddc2d8bd..17077686 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -13,9 +13,9 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class WestCoastDrive extends SubsystemBase { - private final SmartMotorSubsystem leftMotors; - private final SmartMotorSubsystem rightMotors; +public class WestCoastDrive extends SubsystemBase { + private final SmartMotorSubsystem leftMotors; + private final SmartMotorSubsystem rightMotors; private final DifferentialDriveKinematics kinematics; private final double mps_to_rpm; /** @@ -27,7 +27,7 @@ public class WestCoastDrive extends SubsystemB * @param leftMotorSubsystem SmartMotorSubsystem for the left wheels. Usually a TalonMotorSubsystem with two talons. * @param rightMotorSubsystem SmartMotorSubsystem for the right wheels. Usually a TalonMotorSubsystem with two talons. */ - public WestCoastDrive(double trackWidthMeters, double motorToWheelGearRatio, double wheelDiameterMeters, SmartMotorSubsystem leftMotorSubsystem, SmartMotorSubsystem rightMotorSubsystem) { + public WestCoastDrive(double trackWidthMeters, double motorToWheelGearRatio, double wheelDiameterMeters, SmartMotorSubsystem leftMotorSubsystem, SmartMotorSubsystem rightMotorSubsystem) { leftMotors = leftMotorSubsystem; rightMotors = rightMotorSubsystem; kinematics = new DifferentialDriveKinematics(trackWidthMeters); // 2023 robot has track width ~19.5 inches, 5 in wheel diameter @@ -54,6 +54,7 @@ public void movePolar(double speed, double heading, double turnSpeed) { if (heading != 0) throw new IllegalArgumentException("West Coast Drive cannot move at a non-zero heading!"); setChassisVelocity(new ChassisSpeeds(speed, 0, turnSpeed)); } + // TODO: error if PIDF has not been configured in the underlying motorsubsystems public void setWheelVelocities(DifferentialDriveWheelSpeeds wheelSpeeds) { this.leftMotors .setRPM(wheelSpeeds.leftMetersPerSecond * mps_to_rpm); this.rightMotors.setRPM(wheelSpeeds.rightMetersPerSecond * mps_to_rpm); From a4747d265be33b41914380a637c41a8b2b83fd6b Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 2 Mar 2023 20:46:05 -0800 Subject: [PATCH 052/134] UT: smartmotorsubsys, sparkmax dynamic motion profiling --- subsystems/motor/SmartMotorSubsystem.java | 114 +++++++++++++++---- subsystems/motor/SparkMaxMotorSubsystem.java | 81 +++++++++---- subsystems/motor/TalonMotorSubsystem.java | 45 ++++++-- 3 files changed, 190 insertions(+), 50 deletions(-) diff --git a/subsystems/motor/SmartMotorSubsystem.java b/subsystems/motor/SmartMotorSubsystem.java index 1f6b019a..65a62041 100644 --- a/subsystems/motor/SmartMotorSubsystem.java +++ b/subsystems/motor/SmartMotorSubsystem.java @@ -7,15 +7,19 @@ import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public abstract class SmartMotorSubsystem extends SubsystemBase { // generic to allow inherited class (eg. TalonMotorSubsystem) to directly use TalonMotorController APIs on super.motors (not possible if this.motors here was BrakeableMotorController) - protected final SMC[] motors; +public abstract class SmartMotorSubsystem extends SubsystemBase { // generic to allow inherited class (eg. TalonMotorSubsystem) to directly use TalonMotorController APIs on super.motors (not possible if this.motors here was BrakeableMotorController) + public static final int DEFAULT_PID_SLOT = 0; // default slot for pid constants + public static final int DEFAULT_DMP_SLOT = 0; // default slot for dynamic motion profile (motionmagic or smartmotion) configuration + + protected final MotorControllerType[] motors; protected final SpeedModifier speedModifier; // NOTE: maybe change to be called PowerModifier protected final String name; - // TODO: should this take general MotorControllers and just brake the ones that implement BrakeableMotorController /** * A class that wraps around a variable number of BrakeableMotorController @@ -30,7 +34,7 @@ public abstract class SmartMotorSubsystem exte * @param motors The MotorControllers in this subsystem. Can be a single * MotorController or multiple MotorControllers. */ - public SmartMotorSubsystem(String name, SpeedModifier speedModifier, SMC... motors) { + public SmartMotorSubsystem(String name, SpeedModifier speedModifier, MotorControllerType... motors) { super(); setName(name); this.name = name; @@ -39,8 +43,6 @@ public SmartMotorSubsystem(String name, SpeedModifier speedModifier, SMC... moto for (var motor : motors) { motor.set(0); } - setDefaultCommand(this.c_idle()); - setDefaultCommand(this.runOnce(() -> this.neutralOutput())); } @@ -54,7 +56,7 @@ public SmartMotorSubsystem(String name, SpeedModifier speedModifier, SMC... moto * @param motors The MotorControllers in this subsystem. Can be a single * MotorController or multiple MotorControllers. */ - public SmartMotorSubsystem(String name, SMC... motors) { + public SmartMotorSubsystem(String name, MotorControllerType... motors) { this(name, new IdentityModifier(), motors); } @@ -70,7 +72,7 @@ public SmartMotorSubsystem(String name, SMC... motors) { * @param motors The MotorControllers in this subsystem. Can be a single * MotorController or multiple MotorControllers. */ - public SmartMotorSubsystem(SpeedModifier speedModifier, SMC... motors) { + public SmartMotorSubsystem(SpeedModifier speedModifier, MotorControllerType... motors) { this("Motor", speedModifier, motors); } @@ -83,7 +85,7 @@ public SmartMotorSubsystem(SpeedModifier speedModifier, SMC... motors) { * @param motors The MotorControllers in this subsystem. Can be a single * MotorController or multiple MotorControllers. */ - public SmartMotorSubsystem(SMC... motors) { + public SmartMotorSubsystem(MotorControllerType... motors) { this("Motor Subsystem", motors); } @@ -119,7 +121,7 @@ public void set(double power) { * * @param power The power to set. Value should be between -1.0 and 1.0. */ - public void setPower(double power) { + public final void setPower(double power) { set(power); } @@ -162,7 +164,7 @@ public void setVoltage(double voltage) { /** * Enables brake mode on and brakes each motor. */ - public void brake() { + public final void brake() { setBrakeOnNeutral(); neutralOutput(); } @@ -170,7 +172,7 @@ public void brake() { /** * Enables coast mode on and coasts each motor. */ - public void coast() { + public final void coast() { setCoastOnNeutral(); neutralOutput(); } @@ -183,21 +185,21 @@ public void coast() { * * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_disable()) */ - public Command c_disable() { return this.run(() -> this.disable()); } + public final Command c_disable() { return this.run(() -> this.disable()); } /** * Stop motor using the underlying .stopMotor(); command version of .stopMotor(). * * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_stopMotor()) */ - public Command c_stopMotor() { return this.run(() -> this.stopMotor()); } + public final Command c_stopMotor() { return this.run(() -> this.stopMotor()); } /** * Set motor power to zero. Replaces MotorIdle.java in pre-2023 standard. * * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_idle()) */ - public Command c_idle() { return this.runOnce(() -> setPower(0)); } + public final Command c_idle() { return this.runOnce(() -> setPower(0)); } /** * Set motor power to power. Command version of setPower(). @@ -206,7 +208,7 @@ public void coast() { * * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_setPower()) */ - public Command c_setPower(double power) { return this.runOnce(() -> this.setPower(power)); } + public final Command c_setPower(double power) { return this.runOnce(() -> this.setPower(power)); } /** * Repeatedly motor power to power until inturrupted. Replaces MotorConstant.java in pre-2023 standard. @@ -215,7 +217,7 @@ public void coast() { * * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_setPowerHold()) */ - public Command c_holdPower(double power) { return this.run(() -> this.setPower(power)); } + public final Command c_holdPower(double power) { return this.run(() -> this.setPower(power)); } /** * Repeatedly set motor voltage. Command version of setVoltagePower(). @@ -229,21 +231,21 @@ public void coast() { * * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_setVoltageHold()) */ - public Command c_holdVoltage(double voltage) { return this.run(() -> this.setVoltage(voltage)); } + public final Command c_holdVoltage(double voltage) { return this.run(() -> this.setVoltage(voltage)); } /** * Enables brake mode on and brakes each motor. Command version of brake(). * * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_brake()) */ - public Command c_brake() { return this.runOnce(() -> this.brake()); } + public final Command c_brake() { return this.runOnce(() -> this.brake()); } /** * Enables coast mode on and coasts each motor. Command version of coast(). * * @return Command to be scheduled or triggered, eg. button1.onTrue(motor.c_coast()) */ - public Command c_coast() { return this.runOnce(() -> this.coast()); } + public final Command c_coast() { return this.runOnce(() -> this.coast()); } /** * Write PIDF values to hardware, which will be used with the closed loop control commands @@ -251,12 +253,82 @@ public void coast() { * TODO: replace with ezControl */ public abstract void setRPM(double voltage); - public abstract void configPIDF(double p, double i, double d, double f); + public abstract void zeroSensors(); // you must zero the sensors before using position closed loop + /** + * Configure the PIDF constants, and also configure encoder inversion to be + * positive when the lead motor is sent a positive power. + * + * @param p units = rpm + * @param i + * @param d + * @param f units = voltage + * @param pid_slot + */ + public abstract void configPIDF(double p, double i, double d, double f, Integer pid_slot); + public abstract void configDMP(double minRPM, double maxRPM, double maxAccl_RPMps, double maxError_encoderTicks, Integer dmp_slot); // you must configure dynamic motion profiles (motionmagic or smartmotion) before using setPosition public abstract Command c_controlRPM(DoubleSupplier setpointSupplier); public abstract Command c_holdRPM(double setpoint); public abstract Command c_controlPosition(DoubleSupplier setpointSupplier); public abstract Command c_holdPosition(double setpoint); + /* + * Plumbing/unified interface for using dynamic motion profiling features + */ + /** + * Should be a thin wrapper around the motor-controller-specific method to + * set a dynamicmotionprofile setpoint (motionmagic or smartmotion). + * + * Make sure to convert units from motor shaft rotations (passed from here) + * to whatever the internal method uses. + */ + protected abstract void setDynamicMotionProfileTargetRotations(double rotations); + /** + * Should be a thin wrapper around the encoder position getter. + * + * @return the encoder offset from sensor zero, zeroed with .zeroSensors, in + * motor shaft rotations. + */ + protected abstract double getSensorPositionRotations(); + + /** + * A command that uses dynamic motion profiling (motionmagic/smartmotion) to + * PID to a setpoint, while monitoring the progress and ending itself when + * we arrive (when we have been within errorRange (rotations) for + * holdTimeCondition (ms)). + */ + public class HardwareDMPUntilArrival extends CommandBase { + private SmartMotorSubsystem motorSubsys; + private double target; + private double errorRangeRotations; + private double holdTimeCondition; + private double recentOffPositionTime = Double.POSITIVE_INFINITY; // start at infinity so that checks of "now() - recentOffPositionTime > holdTimeConditionMs" never succeed before initalization + public HardwareDMPUntilArrival(SmartMotorSubsystem smartMotorSubsystem, double targetPosRotations, double errorRangeRotations, double holdTimeConditionMs) { + this.target = targetPosRotations; + this.motorSubsys = smartMotorSubsystem; + this.errorRangeRotations = errorRangeRotations; + this.holdTimeCondition = holdTimeConditionMs; + addRequirements(this.motorSubsys); + } + public HardwareDMPUntilArrival(SmartMotorSubsystem smartMotorSubsystem, double targetPosRotations) { + this(smartMotorSubsystem, targetPosRotations, 1/90, 100); + } + + @Override + public void initialize() { + this.motorSubsys.setDynamicMotionProfileTargetRotations(DEFAULT_DMP_SLOT); + } + @Override + public void execute() { + if (Math.abs(this.motorSubsys.getSensorPositionRotations() - target) < errorRangeRotations) { + recentOffPositionTime = Timer.getFPGATimestamp(); + } + } + @Override + public boolean isFinished() { + return Timer.getFPGATimestamp() - recentOffPositionTime > holdTimeCondition; // finished = whether (the last time we were off position) was long enough ago + } + } + // deprecate the set-and-forget commands, because commands should not end before the motor is in a "stop" state @Deprecated public abstract Command c_setRPM(double setpoint); @Deprecated public abstract Command c_setPosition(double setpoint); diff --git a/subsystems/motor/SparkMaxMotorSubsystem.java b/subsystems/motor/SparkMaxMotorSubsystem.java index 9d9eb309..22caea8a 100644 --- a/subsystems/motor/SparkMaxMotorSubsystem.java +++ b/subsystems/motor/SparkMaxMotorSubsystem.java @@ -7,13 +7,16 @@ import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; +import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; +import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.SparkMaxPIDController; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; /** * A group of SparkMax motor controllers the modern motor controller features. @@ -36,7 +39,12 @@ * - Specifying respectLeadMotorLimitSwitches=false will not disable the limit switches if they were previously enabled. This is to allow you to configure limit switches on the underlying motor controllers before passing them in if necessary. */ public class SparkMaxMotorSubsystem extends SmartMotorSubsystem { + public static final int DEFAULT_PID_SLOT = 0; // spark max has 4 slots: [0, 3]; TODO: frecency on mathThing? + public static final int DEFAULT_DMP_SLOT = 0; // spark max has 4 smart-motion (dynamic motion profile) slots, [0, 3] + private boolean pid_configured = false; // flags to remember whether pid and dmp were configured, for error checking + private boolean dmp_configured = false; // when multiple pid/dmp slots, these will have to become slot-wise private SparkMaxPIDController controller = null; + private RelativeEncoder encoder = null; public final CustomCANSparkMax leadMotor; public final CustomCANSparkMax[] followMotors; @@ -79,6 +87,7 @@ public class SparkMaxMotorSubsystem extends SmartMotorSubsystem setRPM(setpointSupplier.getAsDouble())); } - - - @Override + @Override @Deprecated public Command c_setRPM(double setpoint) { // TODO Auto-generated method stub throw new UnsupportedOperationException("Unimplemented method 'c_setVelocity'"); } @Override public Command c_holdRPM(double setpoint) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'c_holdVelocity'"); + if (!pid_configured) throw new IllegalArgumentException(name + " tried to use c_holdRPM without first configPIDF()-ing."); + return this.run(() -> setRPM(setpoint)); } - // TODO: need to zero sensors? + // TODO: these should probably use a diff pid slot from RPM @Override public Command c_setPosition(double setpoint) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'c_setPosition'"); + if (!pid_configured) throw new IllegalArgumentException(name + " tried to use c_setPosition without first configPIDF()-ing"); + return new HardwareDMPUntilArrival(this, setpoint); } @Override public Command c_controlPosition(DoubleSupplier setpointSupplier) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'c_controlPosition'"); + if (!pid_configured) throw new IllegalArgumentException(name + " tried to use c_controlPosition without first configPIDF()-ing"); + return this.run(() -> setDynamicMotionProfileTargetRotations(setpointSupplier.getAsDouble())); } @Override public Command c_holdPosition(double setpoint) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'c_holdPosition'"); + if (!pid_configured) throw new IllegalArgumentException(name + " tried to use c_controlPosition without first configPIDF()-ing"); + return this.runOnce(() -> setDynamicMotionProfileTargetRotations(setpoint)).andThen(new CommandBase(){} /* noop forever command, blame @zbuster05 */); } // don't override disable() or stop() because we *should* indeed use the base implementation of disabling/stopping each motor controller individually. Otherwise the following motors will try to follow a disabled motor, which may cause unexpected behavior (although realistically, it likely just gets set to zero and neutrallized by the deadband). diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index ec16ec35..55528994 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -40,7 +40,7 @@ public class TalonMotorSubsystem extends SmartMotorSubsystem { private static final int ENCODER_COUNTS_PER_REV = 2048; private final int configTimeoutMs = 50; // milliseconds until the Talon gives up trying to configure - private final int pid_idx = 0; // TODO: add support for auxillary pid + private static final int DEFAULT_PID_SLOT = 0; // TODO: add support for auxillary pid private final int follow_motors_remote_filter_id = 0; // DONOT REMOVE, USED IN COMMENTED CODE BELOW; which filter (0 or 1) will be used to configure reading from the integrated encoder on the lead motor private boolean pid_configured = false; // flag for command factories to check whether PID was configured public final TalonMotorController leadMotor; @@ -187,11 +187,19 @@ private void setFollowMode() { * The F value provided here will be overwritten if provided to subsystem.leadMotor.set; note that if you do that, it will bypass the subystem requirements check * * See docstrings on the methods used in the implementation for physical units + * + * @param p in units of encoder counts per 100ms + * @param i in units of TODO + * @param d in units of TODO + * @param f in units of percent output, [-1, 1] + * @param pid_slot range [0, 3], pass null for default of zero. */ @Override - public void configPIDF(double p, double i, double d, double f) { + public void configPIDF(double p, double i, double d, double f, Integer pid_slot) { + if (pid_slot == null) pid_slot = TalonMotorSubsystem.DEFAULT_PID_SLOT; + // feedback sensor configuration (for PID) - leadMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, pid_idx, configTimeoutMs); + leadMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, pid_slot, configTimeoutMs); // make sure to update the static final ENCODER_COUNTS_PER_REV if you are using different encoders. Better yet, add a feedbackSensor argument to this method // for (var fm : followMotors) { // don't think this is needed because we are using follow mode. only needed for aux output @@ -201,11 +209,11 @@ public void configPIDF(double p, double i, double d, double f) { // } // PID constants configuration - leadMotor.config_kP(pid_idx, p, configTimeoutMs); - leadMotor.config_kI(pid_idx, i, configTimeoutMs); - leadMotor.config_kD(pid_idx, d, configTimeoutMs); - leadMotor.config_kF(pid_idx, f, configTimeoutMs); - leadMotor.configClosedLoopPeriod(pid_idx, 10, configTimeoutMs); // fast enough for 100Hz per second + leadMotor.config_kP(pid_slot, p, configTimeoutMs); + leadMotor.config_kI(pid_slot, i, configTimeoutMs); + leadMotor.config_kD(pid_slot, d, configTimeoutMs); + leadMotor.config_kF(pid_slot, f, configTimeoutMs); // TODO: enable voltage comp, then divide f by the voltage comp voltage to get feedforward in voltage units. https://www.chiefdelphi.com/t/feedforward-for-talonfx-pid/401200/8 + leadMotor.configClosedLoopPeriod(pid_slot, 10, configTimeoutMs); // fast enough for 100Hz per second pid_configured = true; // TODO: integral zone and closedLoopPeakOUtput? // other things in the example: motionmagic config and statusframeperiod (for updating sensor status to the aux motor?) @@ -293,6 +301,27 @@ public Command c_holdPosition(double setpoint) { // TODO: also probably have to implement a method to configure smartmotion cruise velocity throw new UnsupportedOperationException("Unimplemented method 'c_holdPosition'"); } + @Override + public void zeroSensors() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'zeroSensors'"); + } + @Override + public void configDMP(double minRPM, double maxRPM, double maxAccl_RPMps, double maxError_encoderTicks, + Integer dmp_slot) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'configDMP'"); + } + @Override + protected void setDynamicMotionProfileTargetRotations(double rotations) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'setDynamicMotionProfileTargetRotations'"); + } + @Override + protected double getSensorPositionRotations() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'getSensorPositionRotations'"); + } // no need to override setPower because the base class just uses set // don't override setBrakeOnNeutral, setCoastOnNeutral, neutralOutput because we indeed want to set it individually on each motor. Otherwise, the followers might try to follow a disabled/neutral motor which might cause unexpected behavior. From bcd8769b19a071fcadfee65a45f163690da124f7 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 2 Mar 2023 21:35:15 -0800 Subject: [PATCH 053/134] config software limit --- subsystems/motor/SmartMotorSubsystem.java | 1 + subsystems/motor/SparkMaxMotorSubsystem.java | 24 +++++++++++++------- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/subsystems/motor/SmartMotorSubsystem.java b/subsystems/motor/SmartMotorSubsystem.java index 65a62041..8b858d70 100644 --- a/subsystems/motor/SmartMotorSubsystem.java +++ b/subsystems/motor/SmartMotorSubsystem.java @@ -266,6 +266,7 @@ public final void coast() { */ public abstract void configPIDF(double p, double i, double d, double f, Integer pid_slot); public abstract void configDMP(double minRPM, double maxRPM, double maxAccl_RPMps, double maxError_encoderTicks, Integer dmp_slot); // you must configure dynamic motion profiles (motionmagic or smartmotion) before using setPosition + public abstract void configSoftwareLimits(double fwdBoundRotations, double revBoundRotations); public abstract Command c_controlRPM(DoubleSupplier setpointSupplier); public abstract Command c_holdRPM(double setpoint); public abstract Command c_controlPosition(DoubleSupplier setpointSupplier); diff --git a/subsystems/motor/SparkMaxMotorSubsystem.java b/subsystems/motor/SparkMaxMotorSubsystem.java index 22caea8a..25c6dac7 100644 --- a/subsystems/motor/SparkMaxMotorSubsystem.java +++ b/subsystems/motor/SparkMaxMotorSubsystem.java @@ -14,6 +14,7 @@ import com.revrobotics.SparkMaxPIDController; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -87,14 +88,12 @@ public class SparkMaxMotorSubsystem extends SmartMotorSubsystem Date: Thu, 2 Mar 2023 21:50:51 -0800 Subject: [PATCH 054/134] m --- custom/motorcontrollers/CustomCANSparkMax.java | 8 ++------ subsystems/motor/SparkMaxMotorSubsystem.java | 3 +-- subsystems/motor/TalonMotorSubsystem.java | 5 +++++ 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/custom/motorcontrollers/CustomCANSparkMax.java b/custom/motorcontrollers/CustomCANSparkMax.java index ab59aebd..1c32845c 100644 --- a/custom/motorcontrollers/CustomCANSparkMax.java +++ b/custom/motorcontrollers/CustomCANSparkMax.java @@ -1,17 +1,13 @@ package org.usfirst.frc4904.standard.custom.motorcontrollers; -import com.ctre.phoenix.motorcontrol.InvertType; -import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax; public class CustomCANSparkMax extends CANSparkMax implements SmartMotorController { - protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; - protected static final InvertType DEFAULT_INVERT_TYPE = InvertType.FollowMaster; // protected Double voltage_compensation_max = null; // remember the configured saturation voltage to conform to the talon api of having separate config() and enable() methods; error if we try to enable without configuring it. - public CustomCANSparkMax(int deviceNumber, MotorType motorType, IdleMode neutralMode) { + public CustomCANSparkMax(int deviceNumber, MotorType motorType, boolean inverted) { super(deviceNumber, motorType); - setIdleMode(neutralMode); + setInverted(inverted); super.restoreFactoryDefaults(); } diff --git a/subsystems/motor/SparkMaxMotorSubsystem.java b/subsystems/motor/SparkMaxMotorSubsystem.java index 25c6dac7..78947070 100644 --- a/subsystems/motor/SparkMaxMotorSubsystem.java +++ b/subsystems/motor/SparkMaxMotorSubsystem.java @@ -191,6 +191,7 @@ public void setRPM(double rpm) { * Call .configPIDF before using this method. TODO all these dependencies suck */ public void zeroSensors() { + encoder = leadMotor.getEncoder(); encoder.setPosition(0); } /** @@ -210,8 +211,6 @@ public void configSoftwareLimits(double fwdRotationBounds, double revRotationBou public void configPIDF(double p, double i, double d, double f, Integer pid_slot) { if (pid_slot == null) pid_slot = SparkMaxMotorSubsystem.DEFAULT_PID_SLOT; // set sensor - encoder = leadMotor.getEncoder(); // use .getAlternateEncoder for external encoder - encoder.setInverted(leadMotor.getInverted()); // docs says you need to sparkMax.setFeedbackDevice but that appears to not exist.. // is this even needed for brushless? migration docs says it's not: https://docs.revrobotics.com/sparkmax/software-resources/migrating-ctre-to-rev#select-encoder diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index 55528994..3fbff13a 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -322,6 +322,11 @@ protected double getSensorPositionRotations() { // TODO Auto-generated method stub throw new UnsupportedOperationException("Unimplemented method 'getSensorPositionRotations'"); } + @Override + public void configSoftwareLimits(double fwdBoundRotations, double revBoundRotations) { + // TODO Auto-generated method stub + + } // no need to override setPower because the base class just uses set // don't override setBrakeOnNeutral, setCoastOnNeutral, neutralOutput because we indeed want to set it individually on each motor. Otherwise, the followers might try to follow a disabled/neutral motor which might cause unexpected behavior. From ef50bdf7450d8c80462649ae614a079121363ab8 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Fri, 3 Mar 2023 17:30:22 -0800 Subject: [PATCH 055/134] fill in some motionmagic methods on talonmotorsubsystem --- subsystems/chassis/WestCoastDrive.java | 2 +- subsystems/motor/TalonMotorSubsystem.java | 59 +++++++++++++---------- 2 files changed, 35 insertions(+), 26 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 17077686..d5c0e6f8 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -30,7 +30,7 @@ public class WestCoastDrive ex public WestCoastDrive(double trackWidthMeters, double motorToWheelGearRatio, double wheelDiameterMeters, SmartMotorSubsystem leftMotorSubsystem, SmartMotorSubsystem rightMotorSubsystem) { leftMotors = leftMotorSubsystem; rightMotors = rightMotorSubsystem; - kinematics = new DifferentialDriveKinematics(trackWidthMeters); // 2023 robot has track width ~19.5 inches, 5 in wheel diameter + kinematics = new DifferentialDriveKinematics(trackWidthMeters); // 2023 robot has track width ~19.5 inches, 5 in wheel diameter, gear ratio 496/45 mps_to_rpm = (Math.PI * wheelDiameterMeters) * motorToWheelGearRatio * 60; // TODO: add requirements? } diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index 3fbff13a..4ae40487 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -15,6 +15,7 @@ import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; /** * A group of Talon motors (either TalonFX or TalonSRX) with the modern motor controller features. @@ -38,10 +39,12 @@ * - Configure normallyClosed limit switches manually on the underlying motorControllers. We usually use normallyOpen limit switches, so you probably don't need to do this. */ public class TalonMotorSubsystem extends SmartMotorSubsystem { - private static final int ENCODER_COUNTS_PER_REV = 2048; + private static final int ENCODER_COUNTS_PER_REV = 4096; // not sure why but 4096 is what makes motion magic work + private static final double RPM_TO_ENCODERCOUNTSPER100MS = ENCODER_COUNTS_PER_REV/60/10; private final int configTimeoutMs = 50; // milliseconds until the Talon gives up trying to configure private static final int DEFAULT_PID_SLOT = 0; // TODO: add support for auxillary pid private final int follow_motors_remote_filter_id = 0; // DONOT REMOVE, USED IN COMMENTED CODE BELOW; which filter (0 or 1) will be used to configure reading from the integrated encoder on the lead motor + private final double voltageComp; private boolean pid_configured = false; // flag for command factories to check whether PID was configured public final TalonMotorController leadMotor; public final TalonMotorController[] followMotors; @@ -102,6 +105,7 @@ public TalonMotorSubsystem(String name, SpeedModifier speedModifier, NeutralMode TalonMotorController leadMotor, TalonMotorController... followMotors) { super(name, speedModifier, Stream.concat(Stream.of(leadMotor), Stream.of(followMotors)).toArray(TalonMotorController[]::new)); // java has no spread operator, so you have to concat. best way i could find is to do it in a stream. please make this not bad if you know how + this.voltageComp = voltageCompensation; this.leadMotor = leadMotor; this.followMotors = followMotors; @@ -212,16 +216,28 @@ public void configPIDF(double p, double i, double d, double f, Integer pid_slot) leadMotor.config_kP(pid_slot, p, configTimeoutMs); leadMotor.config_kI(pid_slot, i, configTimeoutMs); leadMotor.config_kD(pid_slot, d, configTimeoutMs); - leadMotor.config_kF(pid_slot, f, configTimeoutMs); // TODO: enable voltage comp, then divide f by the voltage comp voltage to get feedforward in voltage units. https://www.chiefdelphi.com/t/feedforward-for-talonfx-pid/401200/8 + leadMotor.config_kF(pid_slot, voltageComp == 0 ? f/voltageComp : f/12, configTimeoutMs); leadMotor.configClosedLoopPeriod(pid_slot, 10, configTimeoutMs); // fast enough for 100Hz per second pid_configured = true; // TODO: integral zone and closedLoopPeakOUtput? // other things in the example: motionmagic config and statusframeperiod (for updating sensor status to the aux motor?) } - // public void zeroSensors() { - // // leadMotor.getSensorCollection // ?? doesn't exist; but it's in the CTRE falcon example - // // also should we zero the sensors on the follow motors just in case they're being used? - // } + @Override + /** + * Assumes that PID and DMP slots correspond (eg. use PID slot 0 for DMP slot 0) + */ + public void configDMP(double minRPM, double maxRPM, double maxAccl_RPMps, double maxError_encoderTicks, + Integer dmp_slot) { + if (dmp_slot == null) dmp_slot = DEFAULT_DMP_SLOT; + // TODO? + // _talon.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 10, Constants.kTimeoutMs); + // _talon.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, Constants.kTimeoutMs); + + leadMotor.selectProfileSlot(dmp_slot, dmp_slot); + leadMotor.configMotionCruiseVelocity(maxRPM*RPM_TO_ENCODERCOUNTSPER100MS, configTimeoutMs); + leadMotor.configMotionAcceleration(maxAccl_RPMps*RPM_TO_ENCODERCOUNTSPER100MS, configTimeoutMs); + // TODO: min rpm not used + } // don't override disable() or stop() because we *should* indeed use the base implementation of disabling/stopping each motor controller individually. Otherwise the following motors will try to follow a disabled motor, which may cause unexpected behavior (although realistically, it likely just gets set to zero and neutrallized by the deadband). @@ -277,20 +293,21 @@ public Command c_controlRPM(DoubleSupplier setpointSupplier) { @Override public Command c_holdRPM(double setpoint) { if (pid_configured == false) throw new IllegalArgumentException(name + " tried to use c_controlVelocity without first configPIDF()-ing."); - return this.run(() -> setRPM(setpoint)); // TODO: use motionmagic + return this.runOnce(() -> setRPM(setpoint)).andThen(new CommandBase(){}); // TODO: use motionmagic } /** * Command that sets the position setpoint and immedietly ends. */ - @Override @Deprecated - public Command c_setPosition(double setpoint) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'c_setPosition'"); + public Command c_setPosition(double setpoint, int dmp_slot) { + this.leadMotor.selectProfileSlot(dmp_slot, dmp_slot); + return this.runOnce(() -> setDynamicMotionProfileTargetRotations(setpoint)).andThen(new CommandBase(){}); } + @Override + public Command c_setPosition(double setpoint) { return c_setPosition(setpoint, DEFAULT_DMP_SLOT); } + @Override public Command c_controlPosition(DoubleSupplier setpointSupplier) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'c_controlPosition'"); + return this.run(() -> this.leadMotor.set(ControlMode.Position, setpointSupplier.getAsDouble())); } /** * Hold the position using smart motion (on-the-fly motion-profile generation). @@ -303,24 +320,16 @@ public Command c_holdPosition(double setpoint) { } @Override public void zeroSensors() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'zeroSensors'"); - } - @Override - public void configDMP(double minRPM, double maxRPM, double maxAccl_RPMps, double maxError_encoderTicks, - Integer dmp_slot) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'configDMP'"); + this.leadMotor.setSelectedSensorPosition(0, DEFAULT_PID_SLOT, configTimeoutMs); + // should we zero the sensors on follow motors in case they are being used? } @Override protected void setDynamicMotionProfileTargetRotations(double rotations) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setDynamicMotionProfileTargetRotations'"); + this.leadMotor.set(ControlMode.MotionMagic, rotations); } @Override protected double getSensorPositionRotations() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'getSensorPositionRotations'"); + return this.leadMotor.getSelectedSensorPosition(DEFAULT_DMP_SLOT) / ENCODER_COUNTS_PER_REV; } @Override public void configSoftwareLimits(double fwdBoundRotations, double revBoundRotations) { From cc66a49c91c50c523a828370ffc208f8625f3ce4 Mon Sep 17 00:00:00 2001 From: jac0rr Date: Fri, 3 Mar 2023 18:19:22 -0800 Subject: [PATCH 056/134] Filled out talon Todos --- subsystems/motor/TalonMotorSubsystem.java | 28 ++++++++++++----------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index 4ae40487..7d7f6f46 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -236,7 +236,7 @@ public void configDMP(double minRPM, double maxRPM, double maxAccl_RPMps, double leadMotor.selectProfileSlot(dmp_slot, dmp_slot); leadMotor.configMotionCruiseVelocity(maxRPM*RPM_TO_ENCODERCOUNTSPER100MS, configTimeoutMs); leadMotor.configMotionAcceleration(maxAccl_RPMps*RPM_TO_ENCODERCOUNTSPER100MS, configTimeoutMs); - // TODO: min rpm not used + // TODO: min rpm not used -- not sure if possible or needed } // don't override disable() or stop() because we *should* indeed use the base implementation of disabling/stopping each motor controller individually. Otherwise the following motors will try to follow a disabled motor, which may cause unexpected behavior (although realistically, it likely just gets set to zero and neutrallized by the deadband). @@ -293,14 +293,15 @@ public Command c_controlRPM(DoubleSupplier setpointSupplier) { @Override public Command c_holdRPM(double setpoint) { if (pid_configured == false) throw new IllegalArgumentException(name + " tried to use c_controlVelocity without first configPIDF()-ing."); - return this.runOnce(() -> setRPM(setpoint)).andThen(new CommandBase(){}); // TODO: use motionmagic - } + return this.runOnce(() -> setRPM(setpoint)).andThen(new CommandBase(){}); +} /** * Command that sets the position setpoint and immedietly ends. */ public Command c_setPosition(double setpoint, int dmp_slot) { this.leadMotor.selectProfileSlot(dmp_slot, dmp_slot); - return this.runOnce(() -> setDynamicMotionProfileTargetRotations(setpoint)).andThen(new CommandBase(){}); + //return this.runOnce(() -> setDynamicMotionProfileTargetRotations(setpoint)).andThen(new CommandBase(){}); + return new HardwareDMPUntilArrival(this, setpoint); } @Override public Command c_setPosition(double setpoint) { return c_setPosition(setpoint, DEFAULT_DMP_SLOT); } @@ -312,12 +313,13 @@ public Command c_controlPosition(DoubleSupplier setpointSupplier) { /** * Hold the position using smart motion (on-the-fly motion-profile generation). */ - @Override - public Command c_holdPosition(double setpoint) { - // TODO: https://github.com/REVrobotics/SPARK-MAX-Examples/tree/master/Java/Smart%20Motion%20Example - // TODO: also probably have to implement a method to configure smartmotion cruise velocity - throw new UnsupportedOperationException("Unimplemented method 'c_holdPosition'"); + public Command c_holdPosition(double setpoint, int dmp_slot) { + this.leadMotor.selectProfileSlot(dmp_slot, dmp_slot); + return this.runOnce(() -> setDynamicMotionProfileTargetRotations(setpoint)).andThen(new CommandBase(){}); } + public Command c_holdPosition(double setpoint) { return c_holdPosition(setpoint, DEFAULT_DMP_SLOT); } + + @Override public void zeroSensors() { this.leadMotor.setSelectedSensorPosition(0, DEFAULT_PID_SLOT, configTimeoutMs); @@ -325,16 +327,16 @@ public void zeroSensors() { } @Override protected void setDynamicMotionProfileTargetRotations(double rotations) { - this.leadMotor.set(ControlMode.MotionMagic, rotations); + this.leadMotor.set(ControlMode.MotionMagic, rotations*RPM_TO_ENCODERCOUNTSPER100MS); } @Override protected double getSensorPositionRotations() { return this.leadMotor.getSelectedSensorPosition(DEFAULT_DMP_SLOT) / ENCODER_COUNTS_PER_REV; } @Override - public void configSoftwareLimits(double fwdBoundRotations, double revBoundRotations) { - // TODO Auto-generated method stub - + public void configSoftwareLimits(double fwdBoundRotations, double revBoundRotations) { + this.leadMotor.configForwardSoftLimitThreshold((fwdBoundRotations*ENCODER_COUNTS_PER_REV), configTimeoutMs); + this.leadMotor.configReverseSoftLimitThreshold((revBoundRotations*ENCODER_COUNTS_PER_REV), configTimeoutMs); } // no need to override setPower because the base class just uses set From 4877792a5fe3b2db8fe83b46edd9a425b38fd4ff Mon Sep 17 00:00:00 2001 From: jac0rr Date: Fri, 3 Mar 2023 19:00:22 -0800 Subject: [PATCH 057/134] finished more todos --- subsystems/motor/SparkMaxMotorSubsystem.java | 16 ++++++++++------ subsystems/motor/TalonMotorSubsystem.java | 8 +++++++- 2 files changed, 17 insertions(+), 7 deletions(-) diff --git a/subsystems/motor/SparkMaxMotorSubsystem.java b/subsystems/motor/SparkMaxMotorSubsystem.java index 78947070..9e0a5ad3 100644 --- a/subsystems/motor/SparkMaxMotorSubsystem.java +++ b/subsystems/motor/SparkMaxMotorSubsystem.java @@ -40,7 +40,7 @@ * - Specifying respectLeadMotorLimitSwitches=false will not disable the limit switches if they were previously enabled. This is to allow you to configure limit switches on the underlying motor controllers before passing them in if necessary. */ public class SparkMaxMotorSubsystem extends SmartMotorSubsystem { - public static final int DEFAULT_PID_SLOT = 0; // spark max has 4 slots: [0, 3]; TODO: frecency on mathThing? + public static final int DEFAULT_PID_SLOT = 0; // spark max has 4 slots: [0, 3]; public static final int DEFAULT_DMP_SLOT = 0; // spark max has 4 smart-motion (dynamic motion profile) slots, [0, 3] private boolean pid_configured = false; // flags to remember whether pid and dmp were configured, for error checking private boolean dmp_configured = false; // when multiple pid/dmp slots, these will have to become slot-wise @@ -120,7 +120,8 @@ public SparkMaxMotorSubsystem(String name, SpeedModifier speedModifier, IdleMode // other configuration (neutral mode, neutral deadband, voltagecomp) for (var motor : motors) { motor.setIdleMode(idleMode); - //motor.configNeutralDeadband(neutralDeadbandPercent, configTimeoutMs); // TODO: no neutral deadband setting on sparkmax? + //motor.configNeutralDeadband(neutralDeadbandPercent, configTimeoutMs); //no neutral deadband setting on sparkmax? + // default deadband should be 5%, might be changeable through the rev hardware client, maybe through code as well, but we dont know how if (voltageCompensation > 0) { motor.enableVoltageCompensation(voltageCompensation); } else { @@ -208,7 +209,7 @@ public void configSoftwareLimits(double fwdRotationBounds, double revRotationBou * * See docstrings on the methods used in the implementation for physical units */ - public void configPIDF(double p, double i, double d, double f, Integer pid_slot) { + public void configPIDF(double p, double i, double d, double f, double accumulator, double peakOutput, Integer pid_slot) { if (pid_slot == null) pid_slot = SparkMaxMotorSubsystem.DEFAULT_PID_SLOT; // set sensor // docs says you need to sparkMax.setFeedbackDevice but that appears to not exist.. @@ -221,7 +222,10 @@ public void configPIDF(double p, double i, double d, double f, Integer pid_slot) controller.setD(d, pid_slot); controller.setFF(f, pid_slot); pid_configured = true; - // TODO: integral zone and outputRange? + + controller.setIMaxAccum(accumulator, pid_slot); + controller.setOutputRange(-peakOutput, peakOutput, pid_slot); //TODO: assuming 0-1 range for peakOutput + } public void configDMP(double minRPM, double maxRPM, double maxAccl_RPMps, double maxError_encoderTicks, Integer dmp_slot) { if (dmp_slot == null) dmp_slot = SparkMaxMotorSubsystem.DEFAULT_DMP_SLOT; @@ -255,9 +259,9 @@ public Command c_controlRPM(DoubleSupplier setpointSupplier) { } @Override @Deprecated public Command c_setRPM(double setpoint) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'c_setVelocity'"); + return this.runOnce(() -> setRPM(setpoint)); } + @Override public Command c_holdRPM(double setpoint) { if (!pid_configured) throw new IllegalArgumentException(name + " tried to use c_holdRPM without first configPIDF()-ing."); diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index 7d7f6f46..6607999d 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -196,10 +196,12 @@ private void setFollowMode() { * @param i in units of TODO * @param d in units of TODO * @param f in units of percent output, [-1, 1] + * @param accumulator in units of whatever the intergral is in + * @param peakOutput in units of percent output, [-1, 1] * @param pid_slot range [0, 3], pass null for default of zero. */ @Override - public void configPIDF(double p, double i, double d, double f, Integer pid_slot) { + public void configPIDF(double p, double i, double d, double f, double accumulator, double peakOutput, Integer pid_slot) { if (pid_slot == null) pid_slot = TalonMotorSubsystem.DEFAULT_PID_SLOT; // feedback sensor configuration (for PID) @@ -218,6 +220,10 @@ public void configPIDF(double p, double i, double d, double f, Integer pid_slot) leadMotor.config_kD(pid_slot, d, configTimeoutMs); leadMotor.config_kF(pid_slot, voltageComp == 0 ? f/voltageComp : f/12, configTimeoutMs); leadMotor.configClosedLoopPeriod(pid_slot, 10, configTimeoutMs); // fast enough for 100Hz per second + + leadMotor.configMaxIntegralAccumulator(DEFAULT_DMP_SLOT, accumulator, configTimeoutMs); + leadMotor.configClosedLoopPeakOutput(DEFAULT_DMP_SLOT, peakOutput, configTimeoutMs); + pid_configured = true; // TODO: integral zone and closedLoopPeakOUtput? // other things in the example: motionmagic config and statusframeperiod (for updating sensor status to the aux motor?) From e1688327770a756774dff3265a894a6a1bc2c03b Mon Sep 17 00:00:00 2001 From: syllars Date: Fri, 3 Mar 2023 19:03:09 -0800 Subject: [PATCH 058/134] WestCoastDrive Chassis Minimum Distance. --- subsystems/chassis/WestCoastDrive.java | 20 +++++++++++++++++++- subsystems/motor/SmartMotorSubsystem.java | 2 +- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index d5c0e6f8..352f40db 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -18,6 +18,8 @@ public class WestCoastDrive ex private final SmartMotorSubsystem rightMotors; private final DifferentialDriveKinematics kinematics; private final double mps_to_rpm; + private final double m_to_motorrots; + /** * Represents a west coast drive chassis as a subsystem * @@ -30,8 +32,9 @@ public class WestCoastDrive ex public WestCoastDrive(double trackWidthMeters, double motorToWheelGearRatio, double wheelDiameterMeters, SmartMotorSubsystem leftMotorSubsystem, SmartMotorSubsystem rightMotorSubsystem) { leftMotors = leftMotorSubsystem; rightMotors = rightMotorSubsystem; - kinematics = new DifferentialDriveKinematics(trackWidthMeters); // 2023 robot has track width ~19.5 inches, 5 in wheel diameter, gear ratio 496/45 + kinematics = new DifferentialDriveKinematics(trackWidthMeters); // 2023 robot has track width ~19.5 inches, 5 in wheel diameter mps_to_rpm = (Math.PI * wheelDiameterMeters) * motorToWheelGearRatio * 60; + m_to_motorrots = 1/wheelDiameterMeters*motorToWheelGearRatio; // TODO: add requirements? } @@ -126,5 +129,20 @@ public Command c_chassisConstant(double x, double y, double turn, double timeout public Command c_idle() { return Commands.parallel(leftMotors.c_idle(), rightMotors.c_idle()); // TODO do we need to require the chassis subsystem for this? else chassis will read as unrequired, but all of it's subcomponents will be required. } + + /** + * moveCartesian with (x, y, turn) for timeout seconds. + * + * Replaces ChassisMinimumDrive in pre-2023 standard. + */ + public Command c_chassisMinimumDistance(double distance_meters, double speed_mps) { + var average_motor_rotations = ((this.leftMotors.getSensorPositionRotations()+this.rightMotors.getSensorPositionRotations())/2); + return this.run(() -> setChassisVelocity(new ChassisSpeeds(speed_mps, 0, 0))) + .until(() -> (((this.leftMotors.getSensorPositionRotations()+this.rightMotors.getSensorPositionRotations())/2) - average_motor_rotations > Math.abs(distance_meters * m_to_motorrots) )) + .andThen(() -> this.c_idle()); + } + + + // TODO: write the others. goodfirstissue } diff --git a/subsystems/motor/SmartMotorSubsystem.java b/subsystems/motor/SmartMotorSubsystem.java index 8b858d70..9e2df7e4 100644 --- a/subsystems/motor/SmartMotorSubsystem.java +++ b/subsystems/motor/SmartMotorSubsystem.java @@ -289,7 +289,7 @@ public final void coast() { * @return the encoder offset from sensor zero, zeroed with .zeroSensors, in * motor shaft rotations. */ - protected abstract double getSensorPositionRotations(); + public abstract double getSensorPositionRotations(); /** * A command that uses dynamic motion profiling (motionmagic/smartmotion) to From c164a9959d1ae84e10e4237f42613ea1b7644cce Mon Sep 17 00:00:00 2001 From: jac0rr Date: Sun, 5 Mar 2023 10:26:33 -0800 Subject: [PATCH 059/134] add new variables to configpidf --- subsystems/motor/SmartMotorSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/subsystems/motor/SmartMotorSubsystem.java b/subsystems/motor/SmartMotorSubsystem.java index 8b858d70..3ed7a2bb 100644 --- a/subsystems/motor/SmartMotorSubsystem.java +++ b/subsystems/motor/SmartMotorSubsystem.java @@ -264,7 +264,7 @@ public final void coast() { * @param f units = voltage * @param pid_slot */ - public abstract void configPIDF(double p, double i, double d, double f, Integer pid_slot); + public abstract void configPIDF(double p, double i, double d, double f, double accumulator, double peakOutput, Integer pid_slot); public abstract void configDMP(double minRPM, double maxRPM, double maxAccl_RPMps, double maxError_encoderTicks, Integer dmp_slot); // you must configure dynamic motion profiles (motionmagic or smartmotion) before using setPosition public abstract void configSoftwareLimits(double fwdBoundRotations, double revBoundRotations); public abstract Command c_controlRPM(DoubleSupplier setpointSupplier); From 970d0928f41ca9825b9bdd16d47cccc104685907 Mon Sep 17 00:00:00 2001 From: jac0rr Date: Sun, 5 Mar 2023 13:22:48 -0800 Subject: [PATCH 060/134] Add ChassisTurn to west coast drive --- subsystems/chassis/WestCoastDrive.java | 23 ++++++++++++++++++++--- subsystems/motor/TalonMotorSubsystem.java | 2 +- 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 352f40db..a8b67433 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -136,13 +136,30 @@ public Command c_idle() { * Replaces ChassisMinimumDrive in pre-2023 standard. */ public Command c_chassisMinimumDistance(double distance_meters, double speed_mps) { - var average_motor_rotations = ((this.leftMotors.getSensorPositionRotations()+this.rightMotors.getSensorPositionRotations())/2); + var left_motor_initial = this.leftMotors.getSensorPositionRotations(); + var right_motor_initial = this.rightMotors.getSensorPositionRotations(); return this.run(() -> setChassisVelocity(new ChassisSpeeds(speed_mps, 0, 0))) - .until(() -> (((this.leftMotors.getSensorPositionRotations()+this.rightMotors.getSensorPositionRotations())/2) - average_motor_rotations > Math.abs(distance_meters * m_to_motorrots) )) + .until(() -> ( + ( + (this.leftMotors.getSensorPositionRotations() - left_motor_initial)/2 + + (this.rightMotors.getSensorPositionRotations() - right_motor_initial)/2 + ) > Math.abs(distance_meters * m_to_motorrots) )) .andThen(() -> this.c_idle()); } + public Command c_chassisTurn(double angle_degrees, double max_turnspeed) { + var left_motor_initial = this.leftMotors.getSensorPositionRotations(); + var right_motor_initial = this.rightMotors.getSensorPositionRotations(); + return this.run(() -> setChassisVelocity(new ChassisSpeeds(0, 0, max_turnspeed*Math.signum(angle_degrees)))) + .until(() -> ( + ( + Math.abs(this.leftMotors .getSensorPositionRotations()- left_motor_initial) + +Math.abs(this.rightMotors.getSensorPositionRotations()-right_motor_initial) + ) > Math.abs(Math.PI * kinematics.trackWidthMeters * angle_degrees / 180 * m_to_motorrots) + )) // when we turn, the wheels trace out a circle with trackwidth as a diameter. this checks that the wheels have traveled the right distance aroun the circle for our angle + .andThen(() -> this.c_idle()); + } + } // TODO: write the others. goodfirstissue -} diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index 6607999d..2ad898fa 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -336,7 +336,7 @@ protected void setDynamicMotionProfileTargetRotations(double rotations) { this.leadMotor.set(ControlMode.MotionMagic, rotations*RPM_TO_ENCODERCOUNTSPER100MS); } @Override - protected double getSensorPositionRotations() { + public double getSensorPositionRotations() { return this.leadMotor.getSelectedSensorPosition(DEFAULT_DMP_SLOT) / ENCODER_COUNTS_PER_REV; } @Override From 881ff0abd643153380cf255e03e7110d0f0d16a0 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 9 Mar 2023 15:34:28 -0800 Subject: [PATCH 061/134] talon softlimits --- subsystems/motor/TalonMotorSubsystem.java | 24 +++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index 2ad898fa..bf949487 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -39,7 +39,7 @@ * - Configure normallyClosed limit switches manually on the underlying motorControllers. We usually use normallyOpen limit switches, so you probably don't need to do this. */ public class TalonMotorSubsystem extends SmartMotorSubsystem { - private static final int ENCODER_COUNTS_PER_REV = 4096; // not sure why but 4096 is what makes motion magic work + private static final int ENCODER_COUNTS_PER_REV = 2048; private static final double RPM_TO_ENCODERCOUNTSPER100MS = ENCODER_COUNTS_PER_REV/60/10; private final int configTimeoutMs = 50; // milliseconds until the Talon gives up trying to configure private static final int DEFAULT_PID_SLOT = 0; // TODO: add support for auxillary pid @@ -201,18 +201,14 @@ private void setFollowMode() { * @param pid_slot range [0, 3], pass null for default of zero. */ @Override - public void configPIDF(double p, double i, double d, double f, double accumulator, double peakOutput, Integer pid_slot) { + public void configPIDF(double p, double i, double d, double f, double max_integral_accumulation, double peakOutput, Integer pid_slot) { if (pid_slot == null) pid_slot = TalonMotorSubsystem.DEFAULT_PID_SLOT; // feedback sensor configuration (for PID) leadMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, pid_slot, configTimeoutMs); // make sure to update the static final ENCODER_COUNTS_PER_REV if you are using different encoders. Better yet, add a feedbackSensor argument to this method - // for (var fm : followMotors) { // don't think this is needed because we are using follow mode. only needed for aux output - // fm.configRemoteFeedbackFilter(leadMotor.getDeviceID(), RemoteSensorSource.TalonFX_SelectedSensor /* enum internals has TalonFX = TalonSRX as of 2023 */, follow_motors_remote_filter_id, configTimeoutMs); - // fm.configSelectedFeedbackSensor(follow_motors_remote_filter_id == 0 ? FeedbackDevice.RemoteSensor0 : FeedbackDevice.RemoteSensor1 /* enum internals has TalonFX_SelectedSensor = TalonSRX_SelectedSensor as of 2023 */, pid_idx, configTimeoutMs); - // // TODO: change sensor polarity? https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/blob/master/Java%20Talon%20FX%20(Falcon%20500)/VelocityClosedLoop_AuxStraightIntegratedSensor/src/main/java/frc/robot/Robot.java#L306 - // } + // should follow motors do their own PID? no, right? // PID constants configuration leadMotor.config_kP(pid_slot, p, configTimeoutMs); @@ -221,18 +217,17 @@ public void configPIDF(double p, double i, double d, double f, double accumulato leadMotor.config_kF(pid_slot, voltageComp == 0 ? f/voltageComp : f/12, configTimeoutMs); leadMotor.configClosedLoopPeriod(pid_slot, 10, configTimeoutMs); // fast enough for 100Hz per second - leadMotor.configMaxIntegralAccumulator(DEFAULT_DMP_SLOT, accumulator, configTimeoutMs); + leadMotor.configMaxIntegralAccumulator(DEFAULT_DMP_SLOT, max_integral_accumulation, configTimeoutMs); leadMotor.configClosedLoopPeakOutput(DEFAULT_DMP_SLOT, peakOutput, configTimeoutMs); pid_configured = true; - // TODO: integral zone and closedLoopPeakOUtput? // other things in the example: motionmagic config and statusframeperiod (for updating sensor status to the aux motor?) } @Override /** * Assumes that PID and DMP slots correspond (eg. use PID slot 0 for DMP slot 0) */ - public void configDMP(double minRPM, double maxRPM, double maxAccl_RPMps, double maxError_encoderTicks, + public void configDMP(double minRPM, double cruiseRPM, double accl_RPMps, double maxError_encoderTicks, Integer dmp_slot) { if (dmp_slot == null) dmp_slot = DEFAULT_DMP_SLOT; // TODO? @@ -240,8 +235,8 @@ public void configDMP(double minRPM, double maxRPM, double maxAccl_RPMps, double // _talon.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, Constants.kTimeoutMs); leadMotor.selectProfileSlot(dmp_slot, dmp_slot); - leadMotor.configMotionCruiseVelocity(maxRPM*RPM_TO_ENCODERCOUNTSPER100MS, configTimeoutMs); - leadMotor.configMotionAcceleration(maxAccl_RPMps*RPM_TO_ENCODERCOUNTSPER100MS, configTimeoutMs); + leadMotor.configMotionCruiseVelocity(cruiseRPM*RPM_TO_ENCODERCOUNTSPER100MS, configTimeoutMs); + leadMotor.configMotionAcceleration(accl_RPMps*RPM_TO_ENCODERCOUNTSPER100MS, configTimeoutMs); // TODO: min rpm not used -- not sure if possible or needed } @@ -341,8 +336,13 @@ public double getSensorPositionRotations() { } @Override public void configSoftwareLimits(double fwdBoundRotations, double revBoundRotations) { + // this.leadMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, DEFAULT_PID_SLOT, configTimeoutMs); // select which sensor to use for soft limits + // this.leadMotor.setSensorPhase(true); this.leadMotor.configForwardSoftLimitThreshold((fwdBoundRotations*ENCODER_COUNTS_PER_REV), configTimeoutMs); this.leadMotor.configReverseSoftLimitThreshold((revBoundRotations*ENCODER_COUNTS_PER_REV), configTimeoutMs); + this.leadMotor.configForwardSoftLimitEnable(true, configTimeoutMs); + this.leadMotor.configReverseSoftLimitEnable(true, configTimeoutMs); + this.leadMotor.overrideSoftLimitsEnable(true); } // no need to override setPower because the base class just uses set From f8070588c25d0545fcd15d7b638dd7d20c8de660 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sat, 11 Mar 2023 19:26:58 -0800 Subject: [PATCH 062/134] UT: telescoping arm feedforward --- .../motor/TelescopingArmFeedForward.java | 154 ++++++++++++++++++ 1 file changed, 154 insertions(+) create mode 100644 subsystems/motor/TelescopingArmFeedForward.java diff --git a/subsystems/motor/TelescopingArmFeedForward.java b/subsystems/motor/TelescopingArmFeedForward.java new file mode 100644 index 00000000..422426e8 --- /dev/null +++ b/subsystems/motor/TelescopingArmFeedForward.java @@ -0,0 +1,154 @@ +package org.usfirst.frc4904.standard.subsystems.motor; + +import edu.wpi.first.math.controller.ArmFeedforward; + +/** + * Computes feed-forward values for an arm rotation motor, with a varying cg. + * Internally composes a wpilib ArmFeedForward. + */ +public class TelescopingArmFeedForward { + public final double ks; + public final double kv; + public final double ka; + public final double retracted_kg; + public final double extended_kg; + private final ArmFeedforward armFF; + /** + * Computes feed-forward values for an arm rotation motor, with a varying + * cg. cg is calculated by linearly interpolating between retracted and + * extended cgs. Internally composes a wpilib ArmFeedForward. + * + * Gain units (eg. voltage) will dictate feed-forward units. + * + * Use the recalc arm calculator or characterization to estimate the kg + * (gravitational gain) on the arm for both the retracted and extended + * positions. + * + * @param retracted_kg Gravity gain when the arm is not (0.0) extended. + * @param extended_kg Gravity gain when the arm is fully (1.0) extended. + * @param ks Static gain, eg. from recalc or sysID. The voltage required + * to hold the arm parallel. + * @param kv Velocity gain, eg. from recalc or sysID. + * @param ka Acceleration gain. Makes little difference, 0 is often an + * okay default. + */ + public TelescopingArmFeedForward(double retracted_kg, double extended_kg, double ks, double kv, double ka) { + this.ks = ks; + this.kv = kv; + this.ka = ka; + this.retracted_kg = retracted_kg; + this.extended_kg = extended_kg; + this.armFF = new ArmFeedforward(ks, 0, kv, ka); // passes zero for kg because we will do our own kg calculations and add the result externally + } + private double lerpedCg(double param) { + // You could pass something outside of [0, 1] here, it would just assume + // the arm can extend in the other direction (and the cg still moves + // linearly) or more than originally specified. The assumption of a + // linear relationship between extension and cg remains the same, + // because of lever arm mechanics. Prefer measuring/calculating cg for + // the full range of motion to reduce relative error. + return (this.retracted_kg * (1-param)) + (this.extended_kg * param); + } + /** + * @param armExtensionRatio The extension of the arm, relative to it's full extension. Should be between 0 and 1. Used as cg linear interpolation parameter. + * @param posRads Angle setpoint, in radians, measured from horizontal (0 = parallel with floor). + * @param velRadPerSec Velocity setpoint. + * @param accelRadPerSecSquared Acceleration setpoint. + * @return The computed feedforward. + */ + public double calculate(double armExtensionRatio, double posRads, double velRadPerSec, double accelRadPerSecSquared) { + return this.armFF.calculate(posRads, velRadPerSec, accelRadPerSecSquared) + lerpedCg(armExtensionRatio) * Math.cos(posRads); + } + + // Util methods to calculate max achievable velocity/acceleration from the + // other setpoint. Allegedly obtained from rearranging the above equations; + // see WPILib ArmFeedForward for details. + /** + * Calculates the maximum achievable velocity given a maximum voltage + * supply, a position, and an acceleration. Useful for ensuring that + * velocity and acceleration constraints for a trapezoidal profile are + * simultaneously achievable - enter the acceleration constraint, and this + * will give you a simultaneously-achievable velocity constraint. + * + * @param maxVoltage The maximum voltage that can be supplied to the arm. + * @param extension The extension of the arm, relative to the full + * extension. Should be between 0 and 1. + * @param angle The angle of the arm. This angle should be measured + * from the horizontal (i.e. if the provided angle is 0, + * the arm should be parallel with the floor). If your + * encoder does not follow this convention, an offset + * should be added. + * @param acceleration The acceleration of the arm. + * @return The maximum possible velocity at the given acceleration and + * angle. + */ + public double maxAchievableVelocity(double maxVoltage, double extension, double angle, double acceleration) { + // Assume max velocity is positive + return this.armFF.maxAchievableVelocity(maxVoltage, angle, acceleration) - Math.cos(angle) * lerpedCg(extension) / kv; + } + /** + * Calculates the minimum achievable velocity given a maximum voltage + * supply, a position, and an acceleration. Useful for ensuring that + * velocity and acceleration constraints for a trapezoidal profile are + * simultaneously achievable - enter the acceleration constraint, and this + * will give you a simultaneously-achievable velocity constraint. + * + * @param maxVoltage The maximum voltage that can be supplied to the arm. + * @param extension The extension of the arm, relative to the full + * extension. Should be between 0 and 1. + * @param angle The angle of the arm. This angle should be measured from + * the horizontal (i.e. if + * the provided angle is 0, the arm should be parallel with + * the floor). If your encoder does + * not follow this convention, an offset should be added. + * @param acceleration The acceleration of the arm. + * @return The minimum possible velocity at the given acceleration and angle. + */ + public double minAchievableVelocity(double maxVoltage, double extension, double angle, double acceleration) { + // Assume min velocity is negative, ks flips sign + return this.armFF.minAchievableVelocity(maxVoltage, angle, acceleration) - Math.cos(angle) * lerpedCg(extension) / kv; + } + + /** + * Calculates the maximum achievable acceleration given a maximum voltage + * supply, a position, and a velocity. Useful for ensuring that velocity and + * acceleration constraints for a trapezoidal profile are simultaneously + * achievable - enter the velocity constraint, and this will give you a + * simultaneously-achievable acceleration constraint. + * + * @param maxVoltage The maximum voltage that can be supplied to the arm. + * @param extension The extension of the arm, relative to the full + * extension. Should be between 0 and 1. + * @param angle The angle of the arm. This angle should be measured from + * the horizontal (i.e. if + * the provided angle is 0, the arm should be parallel with + * the floor). If your encoder does + * not follow this convention, an offset should be added. + * @param velocity The velocity of the arm. + * @return The maximum possible acceleration at the given velocity. + */ + public double maxAchievableAcceleration(double maxVoltage, double extension, double angle, double velocity) { + return this.armFF.maxAchievableAcceleration(maxVoltage, angle, velocity) - Math.cos(angle) * lerpedCg(extension) / ka; + } + /** + * Calculates the minimum achievable acceleration given a maximum voltage + * supply, a position, and a velocity. Useful for ensuring that velocity and + * acceleration constraints for a trapezoidal profile are simultaneously + * achievable - enter the velocity constraint, and this will give you a + * simultaneously-achievable acceleration constraint. + * + * @param maxVoltage The maximum voltage that can be supplied to the arm. + * @param extension The extension of the arm, relative to the full + * extension. Should be between 0 and 1. + * @param angle The angle of the arm. This angle should be measured from + * the horizontal (i.e. if + * the provided angle is 0, the arm should be parallel with + * the floor). If your encoder does + * not follow this convention, an offset should be added. + * @param velocity The velocity of the arm. + * @return The minimum possible acceleration at the given velocity. + */ + public double minAchievableAcceleration(double maxVoltage, double extension, double angle, double velocity) { + return maxAchievableAcceleration(-maxVoltage, extension, angle, velocity); // this negative voltage trick is the same as used in WPILib ArmFeedForward + } +} \ No newline at end of file From daa15104821e84f7115b2bb6006d019f26bc3397 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sat, 11 Mar 2023 20:25:31 -0800 Subject: [PATCH 063/134] deprecate old code; rm sensor .get() --- commands/chassis/ChassisConstant.java | 84 ------------ commands/chassis/ChassisIdle.java | 37 ------ commands/chassis/ChassisMinimumDistance.java | 128 ------------------- commands/motor/MotorSet.java | 1 + commands/solenoid/SolenoidExtend.java | 2 + commands/solenoid/SolenoidOff.java | 2 + commands/solenoid/SolenoidRetract.java | 2 + commands/solenoid/SolenoidSet.java | 2 + custom/sensors/CustomEncoder.java | 2 +- 9 files changed, 10 insertions(+), 250 deletions(-) delete mode 100644 commands/chassis/ChassisConstant.java delete mode 100644 commands/chassis/ChassisIdle.java delete mode 100644 commands/chassis/ChassisMinimumDistance.java diff --git a/commands/chassis/ChassisConstant.java b/commands/chassis/ChassisConstant.java deleted file mode 100644 index bb8ddb00..00000000 --- a/commands/chassis/ChassisConstant.java +++ /dev/null @@ -1,84 +0,0 @@ -// package org.usfirst.frc4904.standard.commands.chassis; - -// import org.usfirst.frc4904.standard.custom.ChassisController; -// import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; -// import edu.wpi.first.wpilibj2.command.CommandScheduler; -// import edu.wpi.first.wpilibj2.command.CommandBase; -// import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; - -// public class ChassisConstant extends CommandBase implements ChassisController { -// protected final ParallelRaceGroup move; -// protected final double x; -// protected final double y; -// protected double turn; -// protected double timeout; - -// /** -// * -// * @param name -// * @param chassis -// * @param x -// * @param y -// * @param turn -// * @param timeout -// */ -// public ChassisConstant(String name, Chassis chassis, double x, double y, double turn, double timeout) { -// move = new ChassisMove(chassis, this).withTimeout(timeout); -// this.timeout = timeout; -// this.x = x; -// this.y = y; -// this.turn = turn; -// setName(name); -// addRequirements(chassis); -// } - -// /** -// * -// * @param chassis -// * @param x -// * @param y -// * @param turn -// * @param timeout -// */ -// public ChassisConstant(Chassis chassis, double x, double y, double turn, double timeout) { -// this("Chassis Constant", chassis, x, y, turn, timeout); -// } - -// @Override -// public double getX() { -// return x; -// } - -// @Override -// public double getY() { -// return y; -// } - -// @Override -// public double getTurnSpeed() { -// return turn; -// } - -// @Override -// public void initialize() { -// move.schedule(); -// } - -// /** -// * -// * The command has timed out if the time since scheduled is greater than the -// * timeout -// * -// * @return finished -// * -// */ -// @Override -// public boolean isFinished() { -// return move.isFinished() || CommandScheduler.getInstance().timeSinceScheduled(this) >= this.timeout; -// } - -// @Override -// public void end(boolean interrupted) { -// move.cancel(); -// } -// } diff --git a/commands/chassis/ChassisIdle.java b/commands/chassis/ChassisIdle.java deleted file mode 100644 index 142d97ef..00000000 --- a/commands/chassis/ChassisIdle.java +++ /dev/null @@ -1,37 +0,0 @@ -// package org.usfirst.frc4904.standard.commands.chassis; - -// import org.usfirst.frc4904.standard.commands.motor.MotorIdle; -// import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; -// import org.usfirst.frc4904.standard.subsystems.motor.Motor; -// import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -// import edu.wpi.first.wpilibj2.command.Command; - -// /** -// * -// * This command causes the Chassis to idle by spawning a MotorIdle for every -// * Motor in the Chassis. -// * -// */ -// public class ChassisIdle extends ParallelCommandGroup { -// /** -// * -// * @param name -// * @param chassis The robot Chassis to idle. -// */ -// public ChassisIdle(String name, Chassis chassis) { -// for (Motor motor : chassis.getMotors()) { -// addCommands((Command) new MotorIdle(motor)); -// } - -// setName(name); -// addRequirements(chassis); -// } - -// /** -// * -// * @param chassis The robot Chassis to idle. -// */ -// public ChassisIdle(Chassis chassis) { -// this("ChassisIdle", chassis); -// } -// } diff --git a/commands/chassis/ChassisMinimumDistance.java b/commands/chassis/ChassisMinimumDistance.java deleted file mode 100644 index 4ea67b4f..00000000 --- a/commands/chassis/ChassisMinimumDistance.java +++ /dev/null @@ -1,128 +0,0 @@ -// package org.usfirst.frc4904.standard.commands.chassis; - -// import org.usfirst.frc4904.standard.LogKitten; -// import org.usfirst.frc4904.standard.custom.sensors.CustomEncoder; -// import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; -// import org.usfirst.frc4904.standard.subsystems.chassis.Chassis; - -// public class ChassisMinimumDistance extends ChassisConstant { -// protected CustomEncoder[] encoders; -// protected final ChassisConstant fallbackCommand; -// protected double distance; -// protected double[] initialDistances; - -// /** -// * Constructor. This command moves the chassis forward a known distance via a -// * set of encoders. The distance is calculated as the average of the provided -// * encoders. Control is purely bang (as opposed to bang-bang/pid). The chassis -// * will move at the speed until it passes the point, then will stop. -// * -// * @param name -// * @param chassis -// * @param distance distance to move in encoder ticks -// * @param speed the speed to move at -// * @param fallbackCommand If the sensor fails for some reason, this command will -// * be cancelled, then the fallbackCommand will start -// * @param encoders -// */ -// public ChassisMinimumDistance(String name, Chassis chassis, double distance, double speed, -// ChassisConstant fallbackCommand, CustomEncoder... encoders) { -// super(name, chassis, 0.0, speed, 0.0, Double.MAX_VALUE); -// this.encoders = encoders; -// this.distance = distance; -// this.fallbackCommand = fallbackCommand; -// initialDistances = new double[encoders.length]; -// } - -// /** -// * Constructor. This command moves the chassis forward a known distance via a -// * set of encoders. The distance is calculated as the average of the provided -// * encoders. Control is purely bang (as opposed to bang-bang/pid). The chassis -// * will move at the speed until it passes the point, then will stop. -// * -// * @param chassis -// * @param distance distance to move in encoder ticks -// * @param speed the speed to move at -// * @param fallbackCommand If the sensor fails for some reason, this command will -// * be cancelled, then the fallbackCommand will start -// * @param encoders -// */ -// public ChassisMinimumDistance(Chassis chassis, double distance, double speed, ChassisConstant fallbackCommand, -// CustomEncoder... encoders) { -// this("Chassis Minimum Distance", chassis, distance, speed, fallbackCommand, encoders); -// } - -// /** -// * Constructor. This command moves the chassis forward a known distance via a -// * set of encoders. The distance is calculated as the average of the provided -// * encoders. Control is purely bang (as opposed to bang-bang/pid). The chassis -// * will move at the speed until it passes the point, then will stop. -// * -// * @param name -// * @param chassis -// * @param distance distance to move in encoder ticks -// * @param speed the speed to move at -// * @param encoders -// */ -// public ChassisMinimumDistance(String name, Chassis chassis, double distance, double speed, -// CustomEncoder... encoders) { -// this(name, chassis, distance, speed, null, encoders); -// } - -// /** -// * Constructor. This command moves the chassis forward a known distance via a -// * set of encoders. The distance is calculated as the average of the provided -// * encoders. Control is purely bang (as opposed to bang-bang/pid). The chassis -// * will move at the speed until it passes the point, then will stop. -// * -// * @param chassis -// * @param distance distance to move in encoder ticks -// * @param speed the speed to move at -// * @param encoders -// */ -// public ChassisMinimumDistance(Chassis chassis, double distance, double speed, CustomEncoder... encoders) { -// this(chassis, distance, speed, null, encoders); -// } - -// @Override -// public void initialize() { -// super.initialize(); -// for (int i = 0; i < encoders.length; i++) { -// try { -// initialDistances[i] = encoders[i].getDistanceSafely(); -// } catch (InvalidSensorException e) { -// cancel(); -// return; -// } -// } -// } - -// @Override -// public boolean isFinished() { -// double distanceSum = 0; -// for (int i = 0; i < encoders.length; i++) { -// try { -// distanceSum += encoders[i].getDistanceSafely() - initialDistances[i]; -// LogKitten.d("Encoder " + i + " reads " + encoders[i].getDistanceSafely() + " out of " + distance); -// } catch (InvalidSensorException e) { -// cancel(); -// if (fallbackCommand != null) { -// fallbackCommand.schedule(); -// } -// return true; -// } -// } - -// return distanceSum / (double) encoders.length >= distance; -// } - -// @Override -// public void end(boolean interrupted) { -// super.end(interrupted); -// if (interrupted) { -// LogKitten.w("Interrupted! " + getName() + " is in undefined location."); -// } else { -// LogKitten.v("Finished traveling " + distance + " units (as set by setDistancePerPulse)"); -// } -// } -// } \ No newline at end of file diff --git a/commands/motor/MotorSet.java b/commands/motor/MotorSet.java index be382506..3a2c7597 100644 --- a/commands/motor/MotorSet.java +++ b/commands/motor/MotorSet.java @@ -12,6 +12,7 @@ * having multiple attempts to set a motor simultaneously. * */ +@Deprecated public class MotorSet extends CommandBase { protected final MotorController motor; protected double speed; diff --git a/commands/solenoid/SolenoidExtend.java b/commands/solenoid/SolenoidExtend.java index c9d1ce76..d41c7da8 100644 --- a/commands/solenoid/SolenoidExtend.java +++ b/commands/solenoid/SolenoidExtend.java @@ -8,7 +8,9 @@ /** * Command to set the state of a SolenoidSubsystem to * EXTEND(DoubleSolenoid.Value.kFORWARD) + * TODO: rewrite using inline commands on SolenoidSubsystem */ +@Deprecated public class SolenoidExtend extends SolenoidSet { /** * Command to set the state of a SolenoidSubsystem to diff --git a/commands/solenoid/SolenoidOff.java b/commands/solenoid/SolenoidOff.java index f6fa95e9..09ad5a7f 100644 --- a/commands/solenoid/SolenoidOff.java +++ b/commands/solenoid/SolenoidOff.java @@ -8,7 +8,9 @@ /** * Command to set the state of a SolenoidSubsystem to * OFF(DoubleSolenoid.Value.kOff) + * TODO: rewrite using inline commands on SolenoidSubsystem */ +@Deprecated public class SolenoidOff extends SolenoidSet { /** * Command to set the state of a SolenoidSubsystem to diff --git a/commands/solenoid/SolenoidRetract.java b/commands/solenoid/SolenoidRetract.java index de0f2fdd..11eb6817 100644 --- a/commands/solenoid/SolenoidRetract.java +++ b/commands/solenoid/SolenoidRetract.java @@ -8,7 +8,9 @@ /** * Command to set the state of a SolenoidSubsystem to * RETRACT(DoubleSolenoid.Value.kReverse) + * TODO: rewrite using inline commands on SolenoidSubsystem */ +@Deprecated public class SolenoidRetract extends SolenoidSet { /** * Command to set the state of a SolenoidSubsystem to diff --git a/commands/solenoid/SolenoidSet.java b/commands/solenoid/SolenoidSet.java index 4b4a4934..1dc760c3 100644 --- a/commands/solenoid/SolenoidSet.java +++ b/commands/solenoid/SolenoidSet.java @@ -8,7 +8,9 @@ /** * Command to set the state of a SolenoidSubsystem + * TODO: rewrite using inline commands on SolenoidSubsystem */ +@Deprecated public class SolenoidSet extends CommandBase { protected final SolenoidSubsystem system; protected final SolenoidState state; diff --git a/custom/sensors/CustomEncoder.java b/custom/sensors/CustomEncoder.java index fc0b9c64..14cb2982 100644 --- a/custom/sensors/CustomEncoder.java +++ b/custom/sensors/CustomEncoder.java @@ -13,7 +13,7 @@ public interface CustomEncoder extends NativeDerivativeSensor { * * @warning does not indicate sensor errors */ - int get(); + // int get(); /** * Gets current distance From b48cf3a036a4b3b2610415c4f536d6087d06e884 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sun, 12 Mar 2023 00:37:14 -0800 Subject: [PATCH 064/134] pathplanner autonomous westcoastdrive --- subsystems/chassis/WestCoastDrive.java | 134 +++++++++++++++++-- subsystems/motor/SmartMotorSubsystem.java | 3 +- subsystems/motor/SparkMaxMotorSubsystem.java | 4 + subsystems/motor/TalonMotorSubsystem.java | 3 + 4 files changed, 135 insertions(+), 9 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index a8b67433..26162211 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -1,14 +1,28 @@ package org.usfirst.frc4904.standard.subsystems.chassis; +import java.util.HashMap; +import java.util.List; import java.util.function.Supplier; import org.usfirst.frc4904.standard.custom.motorcontrollers.SmartMotorController; +import org.usfirst.frc4904.standard.custom.sensors.NavX; import org.usfirst.frc4904.standard.subsystems.motor.SmartMotorSubsystem; +import com.pathplanner.lib.PathConstraints; +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; +import com.pathplanner.lib.auto.PIDConstants; +import com.pathplanner.lib.auto.RamseteAutoBuilder; + import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages; +import edu.wpi.first.math.controller.RamseteController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -16,7 +30,10 @@ public class WestCoastDrive extends SubsystemBase { private final SmartMotorSubsystem leftMotors; private final SmartMotorSubsystem rightMotors; + private final PIDConstants pidConsts; private final DifferentialDriveKinematics kinematics; + private final DifferentialDriveOdometry odometry; // OPTIM this can be replaced with a kalman filter? + private final NavX gyro; // OPTIM this can be replaced by something more general private final double mps_to_rpm; private final double m_to_motorrots; @@ -29,16 +46,50 @@ public class WestCoastDrive ex * @param leftMotorSubsystem SmartMotorSubsystem for the left wheels. Usually a TalonMotorSubsystem with two talons. * @param rightMotorSubsystem SmartMotorSubsystem for the right wheels. Usually a TalonMotorSubsystem with two talons. */ - public WestCoastDrive(double trackWidthMeters, double motorToWheelGearRatio, double wheelDiameterMeters, SmartMotorSubsystem leftMotorSubsystem, SmartMotorSubsystem rightMotorSubsystem) { + public WestCoastDrive( + double trackWidthMeters, double motorToWheelGearRatio, double wheelDiameterMeters, + double drive_kP, double drive_kI, double drive_kD, + NavX navx, SmartMotorSubsystem leftMotorSubsystem, SmartMotorSubsystem rightMotorSubsystem) { leftMotors = leftMotorSubsystem; rightMotors = rightMotorSubsystem; - kinematics = new DifferentialDriveKinematics(trackWidthMeters); // 2023 robot has track width ~19.5 inches, 5 in wheel diameter + gyro = navx; mps_to_rpm = (Math.PI * wheelDiameterMeters) * motorToWheelGearRatio * 60; m_to_motorrots = 1/wheelDiameterMeters*motorToWheelGearRatio; + pidConsts = new PIDConstants(drive_kP, drive_kI, drive_kD); + kinematics = new DifferentialDriveKinematics(trackWidthMeters); // 2023 robot has track width ~19.5 inches, 5 in wheel diameter + odometry = new DifferentialDriveOdometry(gyro.getRotation2d(), getLeftDistance(), getRightDistance()); + + // OPTIM should probably allow specification of f, max_accumulation, and peakOutput in constructor + leftMotors.configPIDF(drive_kP, drive_kI, drive_kD, 0, 100, 1, null); + rightMotors.configPIDF(drive_kP, drive_kI, drive_kD, 0, 100, 1, null); + zeroEncoders(); // TODO: add requirements? } + + // odometry methods + public double getLeftDistance() { return leftMotors.getSensorPositionRotations()/m_to_motorrots; } + public double getRightDistance() { return rightMotors.getSensorPositionRotations()/m_to_motorrots; } + private void zeroEncoders() { leftMotors.zeroSensors(); rightMotors.zeroSensors(); } + private void resetPoseMeters(Pose2d metersPose) { + zeroEncoders(); + // doesn't matter what the encoders start at, odometry will use delta of odometry.update() from odometry.reset() + odometry.resetPosition(gyro.getRotation2d(), getLeftDistance(), getRightDistance(), metersPose); + } + public DifferentialDriveWheelSpeeds getWheelSpeeds() { + return new DifferentialDriveWheelSpeeds( + leftMotors.getSensorVelocityRPM()/m_to_motorrots/60, + rightMotors.getSensorVelocityRPM()/m_to_motorrots/60 + ); + } + public Pose2d getPoseMeters() { + return odometry.getPoseMeters(); + } + @Override + public void periodic() { + odometry.update(gyro.getRotation2d(), getLeftDistance(), getRightDistance()); + } - /// methods + /// drive methods /** * Convention: +x forwards, +y right, +z down @@ -57,7 +108,6 @@ public void movePolar(double speed, double heading, double turnSpeed) { if (heading != 0) throw new IllegalArgumentException("West Coast Drive cannot move at a non-zero heading!"); setChassisVelocity(new ChassisSpeeds(speed, 0, turnSpeed)); } - // TODO: error if PIDF has not been configured in the underlying motorsubsystems public void setWheelVelocities(DifferentialDriveWheelSpeeds wheelSpeeds) { this.leftMotors .setRPM(wheelSpeeds.leftMetersPerSecond * mps_to_rpm); this.rightMotors.setRPM(wheelSpeeds.rightMetersPerSecond * mps_to_rpm); @@ -71,6 +121,77 @@ public void setWheelVoltages(DifferentialDriveWheelVoltages wheelVoltages) { this.rightMotors.setVoltage(wheelVoltages.right); } + + /// command factories + @Deprecated + public Command c_simpleWPILibSpline(Trajectory trajectory) { + // TODO: blocked by code not being pushed from driver station + throw new UnsupportedOperationException("need to implement ramsete controller"); + } + + /** + * Load a PathPlanner .path file, generate it with maxVel and maxAccl, and + * run the commands specified by eventMap at the stopping points. + * + * Usage example: + * 
+     * // in RobotMap
+     * 
+     * public static HashMap eventMap;
+     * public static IntakeSubsytem intakeSubsystem;
+     * public static WestCoastDrive drivetrain;
+     * public static Command autoCommand;
+     * // ...
+     * RobotMap.eventMap = Map.ofEntries(
+     *   entry("logEvent", Commands.runOnce(() -> LogKitten.wtf("auton log event hit!"))),
+     *   entry("extendIntake", RobotMap.Component.intakeSubsystem.c_extend())
+     * );
+     * RobotMap.autoCommand = RobotMap.Component.drivetrain.c_buildPathPlannerAuto(0, 0, 0, 2, 0.1, "center_auton", 6, 3, eventMap);
+     * // call the auto factory during startup because it can take a second to generate the trajectory.
+     * // make sure to replace 0s with constants from drivetrain characterization!
+     * 
+     * // in autonomousInitialize
+     * RobotMap.autoCommand.schedule();
+     *
+ * + * @param ffks Motor feedforward static gain. From sysid. If unknown, + * 0 is an okay default. + * @param ffkv Motor feedforward velocity gain. From sysid. If + * unknown, 0 is an okay default. + * @param ffka Motor feedforward acceleration gain. 0 is usually okay. + * @param ramsete_b Ramsete controller b value. 2 is a good default. + * @param ramsete_zeta Ramsete controller zeta value. 0.1 is a good default. + * Reduce this value if the drivetrain speed is + * oscillating (speeds up and slows down, moves jerkily, + * sounds like a train). + * @param autonGroupName The name of the pathplanner path. NAME => search for + * src/main/deploy/pathplanner/NAME.path + * @param maxVel Max velocity constraint used in path generation. + * @param maxAccl Max acceleration constraint used in path generation. + * @param eventMap HashMap containing commands for each "event" in the + * path. PathPlanner will run the commands at those event + * markers. + * @return the command to schedule + */ + public Command c_buildPathPlannerAuto( + double ffks, double ffkv, double ffka, + double ramsete_b, double ramsete_zeta, + String autonGroupName, double maxVel, double maxAccl, HashMap eventMap) { + List pathGroup = PathPlanner.loadPathGroup(autonGroupName, new PathConstraints(maxVel, maxAccl)); + RamseteAutoBuilder autoBuilder = new RamseteAutoBuilder( + () -> this.getPoseMeters(), + (pose) -> this.resetPoseMeters(pose), + new RamseteController(ramsete_b, ramsete_zeta), + kinematics, + new SimpleMotorFeedforward(ffks, ffkv, ffka), + () -> this.getWheelSpeeds(), + pidConsts, + (leftVolts, rightVolts) -> this.setWheelVoltages(new DifferentialDriveWheelVoltages(leftVolts, rightVolts)), + eventMap, + this + ); + return autoBuilder.fullAuto(pathGroup); + } /** * A forever command that pulls drive velocities from a function and sends * them to the motor's closed-loop control. @@ -159,7 +280,4 @@ public Command c_chassisTurn(double angle_degrees, double max_turnspeed) { )) // when we turn, the wheels trace out a circle with trackwidth as a diameter. this checks that the wheels have traveled the right distance aroun the circle for our angle .andThen(() -> this.c_idle()); } - - - } - // TODO: write the others. goodfirstissue +} diff --git a/subsystems/motor/SmartMotorSubsystem.java b/subsystems/motor/SmartMotorSubsystem.java index a03602ea..a21c5edd 100644 --- a/subsystems/motor/SmartMotorSubsystem.java +++ b/subsystems/motor/SmartMotorSubsystem.java @@ -264,7 +264,7 @@ public final void coast() { * @param f units = voltage * @param pid_slot */ - public abstract void configPIDF(double p, double i, double d, double f, double accumulator, double peakOutput, Integer pid_slot); + public abstract void configPIDF(double p, double i, double d, double f, double max_accumulation, double peakOutput, Integer pid_slot); public abstract void configDMP(double minRPM, double maxRPM, double maxAccl_RPMps, double maxError_encoderTicks, Integer dmp_slot); // you must configure dynamic motion profiles (motionmagic or smartmotion) before using setPosition public abstract void configSoftwareLimits(double fwdBoundRotations, double revBoundRotations); public abstract Command c_controlRPM(DoubleSupplier setpointSupplier); @@ -290,6 +290,7 @@ public final void coast() { * motor shaft rotations. */ public abstract double getSensorPositionRotations(); + public abstract double getSensorVelocityRPM(); /** * A command that uses dynamic motion profiling (motionmagic/smartmotion) to diff --git a/subsystems/motor/SparkMaxMotorSubsystem.java b/subsystems/motor/SparkMaxMotorSubsystem.java index 9e0a5ad3..74587901 100644 --- a/subsystems/motor/SparkMaxMotorSubsystem.java +++ b/subsystems/motor/SparkMaxMotorSubsystem.java @@ -245,6 +245,10 @@ public void setDynamicMotionProfileTargetRotations(double targetRotations) { } @Override public double getSensorPositionRotations() { + return encoder.getPosition(); + } + @Override + public double getSensorVelocityRPM() { return encoder.getVelocity(); } /** diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index bf949487..d4b01d0f 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -334,6 +334,9 @@ protected void setDynamicMotionProfileTargetRotations(double rotations) { public double getSensorPositionRotations() { return this.leadMotor.getSelectedSensorPosition(DEFAULT_DMP_SLOT) / ENCODER_COUNTS_PER_REV; } + public double getSensorVelocityRPM() { + return this.leadMotor.getSelectedSensorVelocity(DEFAULT_DMP_SLOT) / ENCODER_COUNTS_PER_REV * 10 * 60; + } @Override public void configSoftwareLimits(double fwdBoundRotations, double revBoundRotations) { // this.leadMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, DEFAULT_PID_SLOT, configTimeoutMs); // select which sensor to use for soft limits From 96ab2c7884224a1488b097448092f484f73a37c5 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sun, 12 Mar 2023 00:54:37 -0800 Subject: [PATCH 065/134] fix talon setVoltage w/ voltagecomp; simplify talonMC --- .../TalonMotorController.java | 25 ++++--------------- subsystems/motor/TalonMotorSubsystem.java | 9 +++++-- 2 files changed, 12 insertions(+), 22 deletions(-) diff --git a/custom/motorcontrollers/TalonMotorController.java b/custom/motorcontrollers/TalonMotorController.java index 2712b3a5..996fecad 100644 --- a/custom/motorcontrollers/TalonMotorController.java +++ b/custom/motorcontrollers/TalonMotorController.java @@ -4,24 +4,9 @@ import com.ctre.phoenix.motorcontrol.IMotorControllerEnhanced; /** - * A base class for CANTalonFX and CANTalonSRX that extends 4904 MotorController - * but separates Talons from eg. SparkMaxes. - * - * May be converted to a general interface for motor controllers in the future, - * given that SparkMaxes can also do brake mode, follow mode, etc. + * A base class for CANTalonFX and CANTalonSRX that extends 4904 + * SmartMotorController, and differentiates Talons from eg. SparkMaxes. + * + * See CANTalonFX or CANTalonSRX for implementation details. */ -public interface TalonMotorController extends SmartMotorController, IMotorControllerEnhanced { - //TODO: add all the things - - // TODO: implement setVoltage with native APIs? or just use voltageComp? - - /** - * Follow `leader`'s percent output. - * - * TODO: also add an auxoutput version if SparkMax supports it - * TODO: use deviceNumber to make this spark/talon agnostic? - * - * @param leader the motor to follow - */ - public void follow(IMotorController leader); // are we able to return self to allow builder pattern? prob have to change the method name, else polymorphism breaks bc name collision with the one in base that takes IMotorController -} +public interface TalonMotorController extends SmartMotorController, IMotorControllerEnhanced {} diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index d4b01d0f..005ffb36 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -14,6 +14,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -269,8 +270,12 @@ public void setRPM(double rpm) { */ @Override public void setVoltage(double voltage) { - setFollowMode(); - leadMotor.setVoltage(voltage); + setFollowMode(); // TODO: is doing this every time too slow? + if (voltageComp > 0) { + leadMotor.setPower(voltage / Math.min(RobotController.getBatteryVoltage(), voltageComp)); + } else { + leadMotor.setVoltage(voltage); // CTRE internal code just divides by RobotController.getBatteryVoltage + } } /** * You must call .configPIDF before using this method. From 053aa18c76f22ed2ceb1229930232a253847b048 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sun, 12 Mar 2023 01:13:51 -0800 Subject: [PATCH 066/134] westcoastdrive pathplannercommand use Map for ergonomics :sunglasses: --- subsystems/chassis/WestCoastDrive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 26162211..242aaed3 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -1,7 +1,7 @@ package org.usfirst.frc4904.standard.subsystems.chassis; -import java.util.HashMap; import java.util.List; +import java.util.Map; import java.util.function.Supplier; import org.usfirst.frc4904.standard.custom.motorcontrollers.SmartMotorController; @@ -176,7 +176,7 @@ public Command c_simpleWPILibSpline(Trajectory trajectory) { public Command c_buildPathPlannerAuto( double ffks, double ffkv, double ffka, double ramsete_b, double ramsete_zeta, - String autonGroupName, double maxVel, double maxAccl, HashMap eventMap) { + String autonGroupName, double maxVel, double maxAccl, Map eventMap) { List pathGroup = PathPlanner.loadPathGroup(autonGroupName, new PathConstraints(maxVel, maxAccl)); RamseteAutoBuilder autoBuilder = new RamseteAutoBuilder( () -> this.getPoseMeters(), From d78f8669740ab5d7efa537102c9f65edee421dd8 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sun, 12 Mar 2023 01:46:51 -0800 Subject: [PATCH 067/134] CommandSendableChooser use command over commandBase --- CommandRobotBase.java | 6 +++--- custom/CommandSendableChooser.java | 9 +++++---- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/CommandRobotBase.java b/CommandRobotBase.java index 6d365ccf..dfcb579b 100644 --- a/CommandRobotBase.java +++ b/CommandRobotBase.java @@ -5,7 +5,7 @@ import org.usfirst.frc4904.standard.humaninput.Driver; import org.usfirst.frc4904.standard.humaninput.Operator; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -15,8 +15,8 @@ * CommandRobotBase class. Robot should extend this instead of iterative robot. */ public abstract class CommandRobotBase extends TimedRobot { - private CommandBase autonomousCommand; - protected CommandBase teleopCommand; + private Command autonomousCommand; + protected Command teleopCommand; protected CommandSendableChooser autoChooser; protected TypedNamedSendableChooser driverChooser; protected TypedNamedSendableChooser operatorChooser; diff --git a/custom/CommandSendableChooser.java b/custom/CommandSendableChooser.java index 839cc899..4dd94725 100644 --- a/custom/CommandSendableChooser.java +++ b/custom/CommandSendableChooser.java @@ -1,6 +1,6 @@ package org.usfirst.frc4904.standard.custom; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; /** @@ -8,13 +8,14 @@ * smart dashboard. * */ -public class CommandSendableChooser extends SendableChooser { +// public class CommandSendableChooser extends SendableChooser { // OPTIM: A LOT of stuff in standard takes in CommandBase when it should probably take in Command (unintuitively, CommandBase is actually less basic than Command) +public class CommandSendableChooser extends SendableChooser { /** * Adds the command object to the smart dashboard. * * @param object */ - public void addOption(CommandBase object) { + public void addOption(Command object) { super.addOption(object.getName(), object); } @@ -23,7 +24,7 @@ public void addOption(CommandBase object) { * * @param object */ - public void setDefaultOption(CommandBase object) { + public void setDefaultOption(Command object) { super.setDefaultOption(object.getName() + " (default)", object); } } From 3cd05009846892b079be825789d18e63437b3f73 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sun, 12 Mar 2023 12:12:10 -0700 Subject: [PATCH 068/134] whitespace --- subsystems/chassis/WestCoastDrive.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 242aaed3..d7ce2738 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -174,9 +174,9 @@ public Command c_simpleWPILibSpline(Trajectory trajectory) { * @return the command to schedule */ public Command c_buildPathPlannerAuto( - double ffks, double ffkv, double ffka, - double ramsete_b, double ramsete_zeta, - String autonGroupName, double maxVel, double maxAccl, Map eventMap) { + double ffks, double ffkv, double ffka, + double ramsete_b, double ramsete_zeta, + String autonGroupName, double maxVel, double maxAccl, Map eventMap) { List pathGroup = PathPlanner.loadPathGroup(autonGroupName, new PathConstraints(maxVel, maxAccl)); RamseteAutoBuilder autoBuilder = new RamseteAutoBuilder( () -> this.getPoseMeters(), From 01ece4ea835d58b9b8d0e7637bd6c3292b1094f6 Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Sun, 12 Mar 2023 15:26:49 -0500 Subject: [PATCH 069/134] ez motion --- custom/MCChassisController.java | 1 + .../motioncontrollers/MotionController.java | 1 + custom/motioncontrollers/ezControl.java | 69 ++++++ custom/motioncontrollers/ezFeedForward.java | 6 + custom/motioncontrollers/ezMotion.java | 67 ++++++ .../motorcontrollers/CustomCANSparkMax.java | 2 + custom/sensors/CustomEncoder.java | 7 - subsystems/chassis/SensorDrive.java | 196 +++++++++--------- .../motor/BrakeableMotorController.java | 18 ++ 9 files changed, 262 insertions(+), 105 deletions(-) create mode 100644 custom/motioncontrollers/ezControl.java create mode 100644 custom/motioncontrollers/ezFeedForward.java create mode 100644 custom/motioncontrollers/ezMotion.java create mode 100644 subsystems/motor/BrakeableMotorController.java diff --git a/custom/MCChassisController.java b/custom/MCChassisController.java index 249e1cb2..0eeef9c0 100644 --- a/custom/MCChassisController.java +++ b/custom/MCChassisController.java @@ -6,6 +6,7 @@ import org.usfirst.frc4904.standard.custom.sensors.IMU; import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; +@Deprecated public class MCChassisController implements ChassisController { protected ChassisController controller; protected double maxDegreesPerSecond; diff --git a/custom/motioncontrollers/MotionController.java b/custom/motioncontrollers/MotionController.java index 7f065a27..5276e1bd 100644 --- a/custom/motioncontrollers/MotionController.java +++ b/custom/motioncontrollers/MotionController.java @@ -13,6 +13,7 @@ * certain input. * */ +@Deprecated public abstract class MotionController implements Subsystem { protected DoubleConsumer output; protected double setpoint; diff --git a/custom/motioncontrollers/ezControl.java b/custom/motioncontrollers/ezControl.java new file mode 100644 index 00000000..0adb8b30 --- /dev/null +++ b/custom/motioncontrollers/ezControl.java @@ -0,0 +1,69 @@ +package org.usfirst.frc4904.standard.custom.motioncontrollers; + +import java.util.function.BiFunction; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; + +public class ezControl implements BiFunction { + private final ezControlMethod controller; + private double setpoint; + private double setpoint_dt; + + public ezControl(double kP, double kI, double kD, ezFeedForward F) { + this.controller = new ezControlMethod(new PIDController(kP, kI, kD), F); + } + + + public boolean atSetpoint() { + return this.controller.pid.atSetpoint(); + } + + public void setSetpoint(double setpoint) { + this.setpoint = setpoint; + this.setpoint_dt = 0; + this.controller.pid.setSetpoint(setpoint); + } + + public void updateSetpoint(double setpoint, double setpoint_dt) { + this.setpoint = setpoint; + this.setpoint_dt = setpoint_dt; + if (setpoint != this.setpoint) { + // slightly expensive call that clears integral + this.controller.pid.setSetpoint(this.setpoint); + } + } + + public void setIntegratorRange(double kIMin, double kIMax) { + this.controller.pid.setIntegratorRange(kIMin, kIMin); + } + + public double calculate(double measurement, double elapsed) { + return this.controller.pid.calculate(measurement) + this.controller.F.calculate(this.setpoint, this.setpoint_dt); + } + + // very similar to TrapezoidProfile.State + @Override + public Double apply(Double measurement, Double elapsed_period) { + return calculate(measurement, elapsed_period); + } + + // VERY TEMPTED + public class ezControlState extends TrapezoidProfile.State {} + + // OK maybe a bit bad but techinically good practice and I'm putting it in here anyways + public class ezControlMethod { + public final PIDController pid; + public final ezFeedForward F; + + public ezControlMethod(PIDController pid, ezFeedForward F) { + this.pid = pid; + this.F = F; + } + } + + public ezControlMethod getControl() { + return this.controller; + } + +} diff --git a/custom/motioncontrollers/ezFeedForward.java b/custom/motioncontrollers/ezFeedForward.java new file mode 100644 index 00000000..81712101 --- /dev/null +++ b/custom/motioncontrollers/ezFeedForward.java @@ -0,0 +1,6 @@ +package org.usfirst.frc4904.standard.custom.motioncontrollers; + +@FunctionalInterface +public interface ezFeedForward { + public double calculate(double setpoint, double setpoint_dt); +} diff --git a/custom/motioncontrollers/ezMotion.java b/custom/motioncontrollers/ezMotion.java new file mode 100644 index 00000000..d32f157a --- /dev/null +++ b/custom/motioncontrollers/ezMotion.java @@ -0,0 +1,67 @@ +package org.usfirst.frc4904.standard.custom.motioncontrollers; + +import java.util.function.DoubleConsumer; +import java.util.function.DoubleFunction; +import java.util.function.DoubleSupplier; + +import javax.naming.InitialContext; + +import org.opencv.core.Mat.Tuple2; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; + +import edu.wpi.first.math.Pair; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Subsystem; + +public class ezMotion extends CommandBase { + public ezControl control; + public DoubleConsumer processVariable; + public Double initialTimestamp; + public DoubleSupplier feedback; + public DoubleFunction> setpointProvider; + + public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, DoubleFunction> setpoint, Subsystem... requirements) { + addRequirements(requirements); + this.control = control; + this.processVariable = processVariable; + this.feedback = feedback; + this.setpointProvider = setpoint; + } + + public double getElapsedTime() { + return Timer.getFPGATimestamp() - initialTimestamp; + } + + public boolean atLatestSetpoint() { + return this.control.atSetpoint(); + } + + @Override + public void initialize() { + this.initialTimestamp = Timer.getFPGATimestamp(); + } + + @Override + public void execute() { + Tuple2 setpoints = this.setpointProvider.apply(getElapsedTime()); + double setpoint = setpoints.get_0(); + double setpoint_dt = setpoints.get_1(); + + control.updateSetpoint(setpoint, setpoint_dt); + double controlEffort = control.calculate(feedback.getAsDouble(), getElapsedTime()); + processVariable.accept(controlEffort); + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + // TODO Auto-generated method stub + super.end(interrupted); + } +} diff --git a/custom/motorcontrollers/CustomCANSparkMax.java b/custom/motorcontrollers/CustomCANSparkMax.java index 54a2fe01..8f6c22fb 100644 --- a/custom/motorcontrollers/CustomCANSparkMax.java +++ b/custom/motorcontrollers/CustomCANSparkMax.java @@ -4,6 +4,8 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj.drive.RobotDriveBase.MotorType; + public class CustomCANSparkMax extends CANSparkMax implements BrakeableMotorController { protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; protected static final InvertType DEFAULT_INVERT_TYPE = InvertType.FollowMaster; diff --git a/custom/sensors/CustomEncoder.java b/custom/sensors/CustomEncoder.java index fc0b9c64..3045e35f 100644 --- a/custom/sensors/CustomEncoder.java +++ b/custom/sensors/CustomEncoder.java @@ -8,13 +8,6 @@ * */ public interface CustomEncoder extends NativeDerivativeSensor { - /** - * Gets current count - * - * @warning does not indicate sensor errors - */ - int get(); - /** * Gets current distance * diff --git a/subsystems/chassis/SensorDrive.java b/subsystems/chassis/SensorDrive.java index 5421114a..42f92c52 100644 --- a/subsystems/chassis/SensorDrive.java +++ b/subsystems/chassis/SensorDrive.java @@ -1,110 +1,110 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ +// /*----------------------------------------------------------------------------*/ +// /* Copyright (c) 2019 FIRST. All Rights Reserved. */ +// /* Open Source Software - may be modified and shared by FRC teams. The code */ +// /* must be accompanied by the FIRST BSD license file in the root directory of */ +// /* the project. */ +// /*----------------------------------------------------------------------------*/ -package org.usfirst.frc4904.standard.subsystems.chassis; -// WAS PID SOURCE -import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; -import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder; -import org.usfirst.frc4904.standard.custom.sensors.NavX; +// package org.usfirst.frc4904.standard.subsystems.chassis; +// // WAS PID SOURCE +// import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; +// import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder; +// import org.usfirst.frc4904.standard.custom.sensors.NavX; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; -import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Subsystem; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; +// import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +// import edu.wpi.first.wpilibj2.command.CommandScheduler; +// import edu.wpi.first.wpilibj2.command.Subsystem; -public class SensorDrive implements Subsystem { // Based largely on - // https://github.com/wpilibsuite/allwpilib/blob/master/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java - private final TankDrive driveBase; - private final CANTalonEncoder leftEncoder; - private final CANTalonEncoder rightEncoder; - private final NavX gyro; - private final DifferentialDriveOdometry odometry; +// public class SensorDrive implements Subsystem { // Based largely on +// // https://github.com/wpilibsuite/allwpilib/blob/master/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java +// private final TankDrive driveBase; +// private final CANTalonEncoder leftEncoder; +// private final CANTalonEncoder rightEncoder; +// private final NavX gyro; +// private final DifferentialDriveOdometry odometry; - /** - * Creates a new DriveSubsystem. - */ - public SensorDrive(TankDrive driveBase, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, NavX gyro, Pose2d initialPose) { - this.driveBase = driveBase; - this.leftEncoder = leftEncoder; - this.rightEncoder = rightEncoder; - this.gyro = gyro; +// /** +// * Creates a new DriveSubsystem. +// */ +// public SensorDrive(TankDrive driveBase, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, NavX gyro, Pose2d initialPose) { +// this.driveBase = driveBase; +// this.leftEncoder = leftEncoder; +// this.rightEncoder = rightEncoder; +// this.gyro = gyro; - resetEncoders(); - odometry = new DifferentialDriveOdometry(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), initialPose); - CommandScheduler.getInstance().registerSubsystem(this); - } +// resetEncoders(); +// odometry = new DifferentialDriveOdometry(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), initialPose); +// CommandScheduler.getInstance().registerSubsystem(this); +// } - @Override - public void periodic() { - odometry.update(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance()); - } +// @Override +// public void periodic() { +// odometry.update(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance()); +// } - /** - * Returns the currently-estimated pose of the robot. - * - * @return The pose. - */ - public Pose2d getPose() { - return odometry.getPoseMeters(); - } +// /** +// * Returns the currently-estimated pose of the robot. +// * +// * @return The pose. +// */ +// public Pose2d getPose() { +// return odometry.getPoseMeters(); +// } - /** - * Returns the current wheel speeds of the robot. - * - * @return The current wheel speeds. - */ - public DifferentialDriveWheelSpeeds getWheelSpeeds() { - return new DifferentialDriveWheelSpeeds(leftEncoder.getRate(), rightEncoder.getRate()); - } +// /** +// * Returns the current wheel speeds of the robot. +// * +// * @return The current wheel speeds. +// */ +// public DifferentialDriveWheelSpeeds getWheelSpeeds() { +// return new DifferentialDriveWheelSpeeds(leftEncoder.getRate(), rightEncoder.getRate()); +// } - /** - * Resets the odometry to the specified pose. - * - * @param pose The pose to which to set the odometry. - */ - public void resetOdometry(Pose2d pose) { - resetEncoders(); - odometry.resetPosition(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), pose); - } +// /** +// * Resets the odometry to the specified pose. +// * +// * @param pose The pose to which to set the odometry. +// */ +// public void resetOdometry(Pose2d pose) { +// resetEncoders(); +// odometry.resetPosition(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), pose); +// } - // /** - // * Controls the left and right sides of the drive directly with voltages. - // * - // * @param leftVolts the commanded left output - // * @param rightVolts the commanded right output - // */ - // public void tankDriveVolts(double leftVolts, double rightVolts) { - // MotorSubsystem[] motors = driveBase.getMotors(); - // if (motors.length == 2) { - // driveBase.getMotors()[0].setVoltage(leftVolts); - // driveBase.getMotors()[1].setVoltage(rightVolts); - // } else { - // driveBase.getMotors()[0].setVoltage(leftVolts); - // driveBase.getMotors()[1].setVoltage(leftVolts); - // driveBase.getMotors()[2].setVoltage(rightVolts); - // driveBase.getMotors()[3].setVoltage(rightVolts); - // } - // } +// // /** +// // * Controls the left and right sides of the drive directly with voltages. +// // * +// // * @param leftVolts the commanded left output +// // * @param rightVolts the commanded right output +// // */ +// // public void tankDriveVolts(double leftVolts, double rightVolts) { +// // MotorSubsystem[] motors = driveBase.getMotors(); +// // if (motors.length == 2) { +// // driveBase.getMotors()[0].setVoltage(leftVolts); +// // driveBase.getMotors()[1].setVoltage(rightVolts); +// // } else { +// // driveBase.getMotors()[0].setVoltage(leftVolts); +// // driveBase.getMotors()[1].setVoltage(leftVolts); +// // driveBase.getMotors()[2].setVoltage(rightVolts); +// // driveBase.getMotors()[3].setVoltage(rightVolts); +// // } +// // } - /** - * Resets the drive encoders to currently read a position of 0. - */ - public void resetEncoders() { - leftEncoder.reset(); - rightEncoder.reset(); - } +// /** +// * Resets the drive encoders to currently read a position of 0. +// */ +// public void resetEncoders() { +// leftEncoder.reset(); +// rightEncoder.reset(); +// } - /** - * Returns the heading of the robot. - * - * @return the robot's heading in degrees, from 180 to 180 - */ - public double getHeading() { - return gyro.getYaw() * -1; - } -} +// /** +// * Returns the heading of the robot. +// * +// * @return the robot's heading in degrees, from 180 to 180 +// */ +// public double getHeading() { +// return gyro.getYaw() * -1; +// } +// } diff --git a/subsystems/motor/BrakeableMotorController.java b/subsystems/motor/BrakeableMotorController.java new file mode 100644 index 00000000..dc671788 --- /dev/null +++ b/subsystems/motor/BrakeableMotorController.java @@ -0,0 +1,18 @@ + +package org.usfirst.frc4904.standard.subsystems.motor; + +public interface BrakeableMotorController extends MotorController { + // TODO: should we have these? brake() = setBrakeOnNeutral(); neutral() + // public abstract void setBrakeOnNeutral(); + // public abstract void setCoastOnNeutral(); + // public abstract void setDisableOnNeutral(); + // public abstract void brake(); + // public abstract void coast(); + // public abstract void neutral(); + + public abstract void setBrakeMode(); + + public abstract void setCoastMode(); + + public abstract void neutralOutput(); +} From 28f88ad82d7d206c306e62881dd713b36e51c3ce Mon Sep 17 00:00:00 2001 From: jac0rr Date: Sun, 12 Mar 2023 14:19:51 -0700 Subject: [PATCH 070/134] made stuff protected and added setvoltage polymorphism --- subsystems/chassis/WestCoastDrive.java | 27 ++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 242aaed3..37f6dec1 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -28,14 +28,14 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class WestCoastDrive extends SubsystemBase { - private final SmartMotorSubsystem leftMotors; - private final SmartMotorSubsystem rightMotors; - private final PIDConstants pidConsts; - private final DifferentialDriveKinematics kinematics; - private final DifferentialDriveOdometry odometry; // OPTIM this can be replaced with a kalman filter? - private final NavX gyro; // OPTIM this can be replaced by something more general - private final double mps_to_rpm; - private final double m_to_motorrots; + protected final SmartMotorSubsystem leftMotors; + protected final SmartMotorSubsystem rightMotors; + protected final PIDConstants pidConsts; + protected final DifferentialDriveKinematics kinematics; + protected final DifferentialDriveOdometry odometry; // OPTIM this can be replaced with a kalman filter? + protected final NavX gyro; // OPTIM this can be replaced by something more general + protected final double mps_to_rpm; + protected final double m_to_motorrots; /** * Represents a west coast drive chassis as a subsystem @@ -70,7 +70,7 @@ public WestCoastDrive( public double getLeftDistance() { return leftMotors.getSensorPositionRotations()/m_to_motorrots; } public double getRightDistance() { return rightMotors.getSensorPositionRotations()/m_to_motorrots; } private void zeroEncoders() { leftMotors.zeroSensors(); rightMotors.zeroSensors(); } - private void resetPoseMeters(Pose2d metersPose) { + protected void resetPoseMeters(Pose2d metersPose) { zeroEncoders(); // doesn't matter what the encoders start at, odometry will use delta of odometry.update() from odometry.reset() odometry.resetPosition(gyro.getRotation2d(), getLeftDistance(), getRightDistance(), metersPose); @@ -117,8 +117,11 @@ public void setChassisVelocity(ChassisSpeeds chassisSpeeds) { setWheelVelocities(wheelSpeeds); } public void setWheelVoltages(DifferentialDriveWheelVoltages wheelVoltages) { - this.leftMotors.setVoltage(wheelVoltages.left); - this.rightMotors.setVoltage(wheelVoltages.right); + this.setWheelVoltages(wheelVoltages.left, wheelVoltages.right); + } + public void setWheelVoltages(double leftV, double rightV) { + this.leftMotors.setVoltage(leftV); + this.rightMotors.setVoltage(rightV); } @@ -191,7 +194,7 @@ public Command c_buildPathPlannerAuto( this ); return autoBuilder.fullAuto(pathGroup); - } + } /** * A forever command that pulls drive velocities from a function and sends * them to the motor's closed-loop control. From de513fedd68842acf7795f1ca910ab094f7ac3ea Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Sun, 12 Mar 2023 14:56:54 -0700 Subject: [PATCH 071/134] remove brakeablemotorcontroller --- custom/motorcontrollers/CustomCANSparkMax.java | 8 -------- subsystems/motor/BrakeableMotorController.java | 18 ------------------ 2 files changed, 26 deletions(-) delete mode 100644 subsystems/motor/BrakeableMotorController.java diff --git a/custom/motorcontrollers/CustomCANSparkMax.java b/custom/motorcontrollers/CustomCANSparkMax.java index 2baccfdf..6445e7cf 100644 --- a/custom/motorcontrollers/CustomCANSparkMax.java +++ b/custom/motorcontrollers/CustomCANSparkMax.java @@ -2,17 +2,9 @@ import com.revrobotics.CANSparkMax; -<<<<<<< HEAD -import edu.wpi.first.wpilibj.drive.RobotDriveBase.MotorType; -public class CustomCANSparkMax extends CANSparkMax implements BrakeableMotorController { - protected static final NeutralMode DEFAULT_NEUTRAL_MODE = NeutralMode.Coast; - protected static final InvertType DEFAULT_INVERT_TYPE = InvertType.FollowMaster; - protected Double saturation_voltage = null; // remember the configured saturation voltage to conform to the talon api of having separate config() and enable() methods; error if we try to enable without configuring it. -======= public class CustomCANSparkMax extends CANSparkMax implements SmartMotorController { // protected Double voltage_compensation_max = null; // remember the configured saturation voltage to conform to the talon api of having separate config() and enable() methods; error if we try to enable without configuring it. ->>>>>>> 3cd05009846892b079be825789d18e63437b3f73 public CustomCANSparkMax(int deviceNumber, MotorType motorType, boolean inverted) { super(deviceNumber, motorType); diff --git a/subsystems/motor/BrakeableMotorController.java b/subsystems/motor/BrakeableMotorController.java deleted file mode 100644 index dc671788..00000000 --- a/subsystems/motor/BrakeableMotorController.java +++ /dev/null @@ -1,18 +0,0 @@ - -package org.usfirst.frc4904.standard.subsystems.motor; - -public interface BrakeableMotorController extends MotorController { - // TODO: should we have these? brake() = setBrakeOnNeutral(); neutral() - // public abstract void setBrakeOnNeutral(); - // public abstract void setCoastOnNeutral(); - // public abstract void setDisableOnNeutral(); - // public abstract void brake(); - // public abstract void coast(); - // public abstract void neutral(); - - public abstract void setBrakeMode(); - - public abstract void setCoastMode(); - - public abstract void neutralOutput(); -} From 709b0fda622d19b4cb1ba6b3d70b10ea76eee465 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Sun, 12 Mar 2023 15:20:01 -0700 Subject: [PATCH 072/134] make thing public --- subsystems/chassis/WestCoastDrive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 5949c15b..3f7f013b 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -28,8 +28,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class WestCoastDrive extends SubsystemBase { - protected final SmartMotorSubsystem leftMotors; - protected final SmartMotorSubsystem rightMotors; + public final SmartMotorSubsystem leftMotors; + public final SmartMotorSubsystem rightMotors; protected final PIDConstants pidConsts; protected final DifferentialDriveKinematics kinematics; protected final DifferentialDriveOdometry odometry; // OPTIM this can be replaced with a kalman filter? From 795f61174791433201c2a08c0ce288f5d0caba52 Mon Sep 17 00:00:00 2001 From: jac0rr Date: Sun, 12 Mar 2023 16:29:37 -0700 Subject: [PATCH 073/134] change navx to ahrs and change smartmotorsubsystem to generic --- subsystems/chassis/WestCoastDrive.java | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 5949c15b..904e7fff 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -8,6 +8,7 @@ import org.usfirst.frc4904.standard.custom.sensors.NavX; import org.usfirst.frc4904.standard.subsystems.motor.SmartMotorSubsystem; +import com.kauailabs.navx.frc.AHRS; import com.pathplanner.lib.PathConstraints; import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; @@ -27,13 +28,13 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class WestCoastDrive extends SubsystemBase { - protected final SmartMotorSubsystem leftMotors; - protected final SmartMotorSubsystem rightMotors; +public class WestCoastDrive extends SubsystemBase { + public final MotorSubsystem leftMotors; + public final MotorSubsystem rightMotors; protected final PIDConstants pidConsts; protected final DifferentialDriveKinematics kinematics; protected final DifferentialDriveOdometry odometry; // OPTIM this can be replaced with a kalman filter? - protected final NavX gyro; // OPTIM this can be replaced by something more general + protected final AHRS gyro; // OPTIM this can be replaced by something more general protected final double mps_to_rpm; protected final double m_to_motorrots; @@ -49,7 +50,7 @@ public class WestCoastDrive ex public WestCoastDrive( double trackWidthMeters, double motorToWheelGearRatio, double wheelDiameterMeters, double drive_kP, double drive_kI, double drive_kD, - NavX navx, SmartMotorSubsystem leftMotorSubsystem, SmartMotorSubsystem rightMotorSubsystem) { + AHRS navx, MotorSubsystem leftMotorSubsystem, MotorSubsystem rightMotorSubsystem) { leftMotors = leftMotorSubsystem; rightMotors = rightMotorSubsystem; gyro = navx; From d407426bb383233d9179814e1a87e2815fc827dd Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 13 Mar 2023 15:36:17 -0700 Subject: [PATCH 074/134] de-generic westcoastdrive --- subsystems/chassis/WestCoastDrive.java | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 904e7fff..5568c9d3 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -4,9 +4,7 @@ import java.util.Map; import java.util.function.Supplier; -import org.usfirst.frc4904.standard.custom.motorcontrollers.SmartMotorController; -import org.usfirst.frc4904.standard.custom.sensors.NavX; -import org.usfirst.frc4904.standard.subsystems.motor.SmartMotorSubsystem; +import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorSubsystem; import com.kauailabs.navx.frc.AHRS; import com.pathplanner.lib.PathConstraints; @@ -28,9 +26,9 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class WestCoastDrive extends SubsystemBase { - public final MotorSubsystem leftMotors; - public final MotorSubsystem rightMotors; +public class WestCoastDrive extends SubsystemBase { + public final TalonMotorSubsystem leftMotors; + public final TalonMotorSubsystem rightMotors; protected final PIDConstants pidConsts; protected final DifferentialDriveKinematics kinematics; protected final DifferentialDriveOdometry odometry; // OPTIM this can be replaced with a kalman filter? @@ -50,7 +48,7 @@ public class WestCoastDrive extends public WestCoastDrive( double trackWidthMeters, double motorToWheelGearRatio, double wheelDiameterMeters, double drive_kP, double drive_kI, double drive_kD, - AHRS navx, MotorSubsystem leftMotorSubsystem, MotorSubsystem rightMotorSubsystem) { + AHRS navx, TalonMotorSubsystem leftMotorSubsystem, TalonMotorSubsystem rightMotorSubsystem) { leftMotors = leftMotorSubsystem; rightMotors = rightMotorSubsystem; gyro = navx; @@ -64,7 +62,6 @@ public WestCoastDrive( leftMotors.configPIDF(drive_kP, drive_kI, drive_kD, 0, 100, 1, null); rightMotors.configPIDF(drive_kP, drive_kI, drive_kD, 0, 100, 1, null); zeroEncoders(); - // TODO: add requirements? } // odometry methods From 4ffe59b9ed39742b56de9d7244881f253ee8e6eb Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 13 Mar 2023 19:38:19 -0700 Subject: [PATCH 075/134] deprecate WCD pathplanner spline factory --- subsystems/chassis/WestCoastDrive.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 5568c9d3..5769ef0e 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -126,7 +126,7 @@ public void setWheelVoltages(double leftV, double rightV) { /// command factories @Deprecated public Command c_simpleWPILibSpline(Trajectory trajectory) { - // TODO: blocked by code not being pushed from driver station + // TODO: doesn't work. see implementation in Chassis2023 throw new UnsupportedOperationException("need to implement ramsete controller"); } @@ -174,6 +174,7 @@ public Command c_simpleWPILibSpline(Trajectory trajectory) { * markers. * @return the command to schedule */ + @Deprecated // don't use this, it doesn't really work public Command c_buildPathPlannerAuto( double ffks, double ffkv, double ffka, double ramsete_b, double ramsete_zeta, From 9ffb6a2ce2c548f6f4d6e4340f75a032677aa65a Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 13 Mar 2023 19:38:30 -0700 Subject: [PATCH 076/134] telescoping arm extension feedforward --- .../TelescopingArmExtensionFeedForward.java | 95 +++++++++++++++++++ ...va => TelescopingArmPivotFeedForward.java} | 4 +- 2 files changed, 97 insertions(+), 2 deletions(-) create mode 100644 subsystems/motor/TelescopingArmExtensionFeedForward.java rename subsystems/motor/{TelescopingArmFeedForward.java => TelescopingArmPivotFeedForward.java} (98%) diff --git a/subsystems/motor/TelescopingArmExtensionFeedForward.java b/subsystems/motor/TelescopingArmExtensionFeedForward.java new file mode 100644 index 00000000..23b7b138 --- /dev/null +++ b/subsystems/motor/TelescopingArmExtensionFeedForward.java @@ -0,0 +1,95 @@ +package org.usfirst.frc4904.standard.subsystems.motor; + +import edu.wpi.first.math.controller.ElevatorFeedforward; + +/** + * Computes feed-forward values for an elevator motor, which is mounted on a varying angle. + * Internally composes a wpilib ElevatorFeedForward. + */ +public class TelescopingArmExtensionFeedForward { + public final double ks; + public final double kg_vertical; + public final double kv; + public final double ka; + private final ElevatorFeedforward elevFF; + + /** + * Computes elevator feed-forward values with a varying cg based on rotation. + * + * @param ks Static gain: volts needed to overcome static friction + * @param vertical_kg Gravity gain: usually found by characterizing the arm as + * an elevator in the vertical orientation, equals volts + * needed to hold the arm up against gravity when orinteted + * vertically (max cg). + * @param kv Velocity gain + * @param ka Acceleration gain. Zero is usually an okay default. + */ + public TelescopingArmExtensionFeedForward(double ks, double vertical_kg, double kv, double ka) { + this.ks = ks; + this.kg_vertical = vertical_kg; + this.kv = kv; + this.ka = ka; + this.elevFF = new ElevatorFeedforward(ks, 0, kv, ka); + } + /** + * @param posRads Position of the arm, in radians from + * horizontal. 0 = horizontal, PI/2 = vertical. + * @param velMetersPerSec Velocity setpoint + * @param acclMetersPerSecSquared Acceleration setpoint + */ + public double calculate(double posRads, double velMetersPerSec, double acclMetersPerSecSquared) { + return elevFF.calculate(velMetersPerSec, acclMetersPerSecSquared) + Math.sin(posRads) * kg_vertical; + } + /** + * @param posRads Position of the arm, in radians from + * horizontal. 0 = horizontal, PI/2 = vertical. + * @param velMetersPerSec Velocity setpoint + */ + public double calculate(double posRads, double velMetersPerSec) { + return calculate(posRads, velMetersPerSec, 0); + } + + // Util methods to calculate max achievable velocity/acceleration from the + // other setpoint. Allegedly obtained from rearranging the above equations; + // see WPILib ElevatorFeedForward for details. + /** + * Calculates the maximum achievable velocity given a maximum voltage supply + * and an acceleration. Useful for ensuring that velocity and acceleration + * constraints for a trapezoidal profile are simultaneously achievable - + * enter the acceleration constraint, and this will give you a + * simultaneously-achievable velocity constraint. + */ + public double maxAchievableVelocity(double maxVoltage, double posRads, double acceleration) { + return elevFF.maxAchievableVelocity(maxVoltage, acceleration) - Math.sin(posRads) * kg_vertical / kv; + } + /** + * Calculates the minimum achievable velocity given a minimum voltage supply + * and an acceleration. Useful for ensuring that velocity and acceleration + * constraints for a trapezoidal profile are simultaneously achievable - + * enter the acceleration constraint, and this will give you a + * simultaneously-achievable velocity constraint. + */ + public double minAchievableVelocity(double maxVoltage, double posRads, double acceleration) { + return elevFF.maxAchievableVelocity(maxVoltage, acceleration) - Math.sin(posRads) * kg_vertical / kv; + } + /** + * Calculates the maximum achievable acceleration given a maximum voltage + * supply and a velocity. Useful for ensuring that velocity and acceleration + * constraints for a trapezoidal profile are simultaneously achievable - + * enter the velocity constraint, and this will give you a + * simultaneously-achievable acceleration constraint. + */ + public double maxAchievableAcceleration(double maxVoltage, double posRads, double velocity) { + return elevFF.maxAchievableAcceleration(maxVoltage, velocity) - Math.sin(posRads) * kg_vertical / ka; + } + /** + * Calculates the minimum achievable acceleration given a minimum voltage + * supply and a velocity. Useful for ensuring that velocity and acceleration + * constraints for a trapezoidal profile are simultaneously achievable - + * enter the velocity constraint, and this will give you a + * simultaneously-achievable acceleration constraint. + */ + public double minAchievableAcceleration(double maxVoltage, double posRads, double velocity) { + return maxAchievableAcceleration(-maxVoltage, posRads, velocity); + } +} \ No newline at end of file diff --git a/subsystems/motor/TelescopingArmFeedForward.java b/subsystems/motor/TelescopingArmPivotFeedForward.java similarity index 98% rename from subsystems/motor/TelescopingArmFeedForward.java rename to subsystems/motor/TelescopingArmPivotFeedForward.java index 422426e8..b688834a 100644 --- a/subsystems/motor/TelescopingArmFeedForward.java +++ b/subsystems/motor/TelescopingArmPivotFeedForward.java @@ -6,7 +6,7 @@ * Computes feed-forward values for an arm rotation motor, with a varying cg. * Internally composes a wpilib ArmFeedForward. */ -public class TelescopingArmFeedForward { +public class TelescopingArmPivotFeedForward { public final double ks; public final double kv; public final double ka; @@ -32,7 +32,7 @@ public class TelescopingArmFeedForward { * @param ka Acceleration gain. Makes little difference, 0 is often an * okay default. */ - public TelescopingArmFeedForward(double retracted_kg, double extended_kg, double ks, double kv, double ka) { + public TelescopingArmPivotFeedForward(double retracted_kg, double extended_kg, double ks, double kv, double ka) { this.ks = ks; this.kv = kv; this.ka = ka; From f64098733c3f07e682372efe4263499732a897e9 Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Tue, 14 Mar 2023 01:11:59 -0500 Subject: [PATCH 077/134] Fixed typo --- custom/motioncontrollers/ezControl.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/custom/motioncontrollers/ezControl.java b/custom/motioncontrollers/ezControl.java index 0adb8b30..6430ba43 100644 --- a/custom/motioncontrollers/ezControl.java +++ b/custom/motioncontrollers/ezControl.java @@ -51,7 +51,7 @@ public Double apply(Double measurement, Double elapsed_period) { // VERY TEMPTED public class ezControlState extends TrapezoidProfile.State {} - // OK maybe a bit bad but techinically good practice and I'm putting it in here anyways + // OK maybe a bit bad but technically good practice and I'm putting it in here anyways public class ezControlMethod { public final PIDController pid; public final ezFeedForward F; From 0550b8b85583632c3c9b83f57015b89d120024bd Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Tue, 14 Mar 2023 01:43:32 -0500 Subject: [PATCH 078/134] important modifications --- custom/MCChassisController.java | 60 +-- .../motioncontrollers/MotionController.java | 349 ------------------ subsystems/motor/SensorMotor.java | 84 +---- 3 files changed, 54 insertions(+), 439 deletions(-) delete mode 100644 custom/motioncontrollers/MotionController.java diff --git a/custom/MCChassisController.java b/custom/MCChassisController.java index 0eeef9c0..ddb6fc1f 100644 --- a/custom/MCChassisController.java +++ b/custom/MCChassisController.java @@ -1,8 +1,9 @@ package org.usfirst.frc4904.standard.custom; +import java.lang.reflect.Array; + import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.Util; -import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; import org.usfirst.frc4904.standard.custom.sensors.IMU; import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; @@ -13,20 +14,16 @@ public class MCChassisController implements ChassisController { protected double targetYaw; protected double lastUpdate; protected IMU imu; - protected MotionController motionController; + protected MineCraft minecraft; - public MCChassisController(ChassisController controller, IMU imu, MotionController motionController, + public MCChassisController(ChassisController controller, IMU imu, MineCraft minecraft, double maxDegreesPerSecond) { this.controller = controller; this.maxDegreesPerSecond = maxDegreesPerSecond; this.imu = imu; this.imu.reset(); - this.motionController = motionController; - motionController.setInputRange(-180.0f, 180.0f); - motionController.setOutputRange(-1.0f, 1.0f); - motionController.setContinuous(true); - motionController.reset(); - motionController.enable(); + this.minecraft = minecraft; + minecraft.setInputRange(-180.0f, 180.0f); targetYaw = imu.getYaw(); lastUpdate = System.currentTimeMillis() / 1000.0; } @@ -34,9 +31,7 @@ public MCChassisController(ChassisController controller, IMU imu, MotionControll public void reset() throws InvalidSensorException { targetYaw = imu.getYaw(); lastUpdate = System.currentTimeMillis() / 1000.0; - motionController.disable(); - motionController.reset(); - motionController.enable(); + } @Override @@ -52,14 +47,11 @@ public double getY() { @Override public double getTurnSpeed() { if (Util.isZero(controller.getY()) && Util.isZero(controller.getX())) { - motionController.setSetpoint(imu.getYaw()); + minecraft.setSetpoint(imu.getYaw()); targetYaw = imu.getYaw(); return controller.getTurnSpeed(); } - try { - LogKitten.v(motionController.getSetpoint() + " " + imu.getYaw() + " " + motionController.getSafely()); - } catch (InvalidSensorException e) { - } + LogKitten.v(minecraft.getSetpoint() + " " + imu.getYaw() + " " + minecraft.getSafely()); targetYaw = targetYaw + ((controller.getTurnSpeed() * maxDegreesPerSecond) * ((System.currentTimeMillis() / 1000.0) - lastUpdate)); lastUpdate = System.currentTimeMillis() / 1000.0; @@ -68,11 +60,35 @@ public double getTurnSpeed() { } else if (targetYaw < -180) { targetYaw = 180 - (Math.abs(targetYaw) - 180); } - motionController.setSetpoint(targetYaw); - try { - return motionController.getSafely(); - } catch (InvalidSensorException e) { - return controller.getTurnSpeed(); + minecraft.setSetpoint(targetYaw); + return minecraft.getSafely(); + } + + public class MineCraft { + public Array entities; + + public MineCraft() { + System.out.println("initializing, in minecraft"); + } + + public void setSetpoint(double sheep) { + System.out.println("setting setpoint, in minecraft"); + } + + public double getSafely() { + return (double) Double.parseDouble("getting safely, in minecraft"); + } + + public double getSetpoint() { + return (double) Double.parseDouble("getting setpoint, in minecraft"); + } + + public void setInputRange(double low, double high) { + System.out.println("setting input range, in minecraft"); + } + + public void reset() { + System.out.println("resetting, in minecraft"); } } } diff --git a/custom/motioncontrollers/MotionController.java b/custom/motioncontrollers/MotionController.java deleted file mode 100644 index 5276e1bd..00000000 --- a/custom/motioncontrollers/MotionController.java +++ /dev/null @@ -1,349 +0,0 @@ -package org.usfirst.frc4904.standard.custom.motioncontrollers; - -import java.util.function.DoubleConsumer; - -import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; - -import edu.wpi.first.hal.util.BoundaryException; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Subsystem; - -/** - * A MotionController modifies an output using a sensor to precisely maintain a - * certain input. - * - */ -@Deprecated -public abstract class MotionController implements Subsystem { - protected DoubleConsumer output; - protected double setpoint; - protected double absoluteTolerance; - protected boolean continuous; - protected double inputMax; - protected double inputMin; - protected boolean capOutput; - protected double outputMax; - protected double outputMin; - protected boolean enable; - protected boolean overridden; - protected Exception sensorException; - private volatile boolean justReset; - private final Object lock = new Object(); - - public MotionController(DoubleConsumer output) { - setOutput(output); - enable = false; - overridden = false; - absoluteTolerance = Double.MIN_VALUE; // Nonzero to avoid floating point errors - capOutput = false; - continuous = false; - inputMin = 0.0; - inputMax = 0.0; - outputMin = 0.0; - outputMax = 0.0; - reset(); - justReset = true; - sensorException = null; - CommandScheduler.getInstance().registerSubsystem(this); - } - - /** - * A MotionController modifies an output using a sensor to precisely maintain a - * certain input. - * - * @param sensor The sensor associated with the output you are trying to control - */ - public MotionController() { - this(null); - } - - @Override - public void periodic() { - try { - double value = getSafely(); // Always calculate MC output - synchronized (lock) { - if (justReset) { - justReset = false; - return; - } - } - if (output != null && isEnabled()) { - output.accept(value); - } - } catch (Exception e) { - sensorException = e; - } - } - - /** - * Sets the output for this MotionController. Once every MotionController tick, - * the output will be set to the results from the motion control calculation via - * the pidWrite function. - * - * @param output The output to control - */ - public void setOutput(DoubleConsumer output) { - this.output = output; - } - - /** - * A MotionController modifies an output using a sensor to precisely maintain a - * certain input. - * - * @param sensor The sensor associated with the output you are trying to control - */ - - /** - * This should return the motion controller to a state such that it returns 0. - * - * @warning this does not indicate sensor errors - */ - public final void reset() { - resetErrorToZero(); - //setpoint = sensor.pidGet(); - justReset = true; - } - - /** - * This should return the motion controller to a state such that it returns 0. - */ - public final void resetSafely() throws InvalidSensorException { - resetErrorToZero(); - //setpoint = sensor.pidGetSafely(); - justReset = true; - } - - /** - * Method-specific method for resetting the motion controller without indicating - * sensor errors. - */ - protected abstract void resetErrorToZero(); - - /** - * The calculated output value to achieve the current setpoint. - * - * @return Output value. If output range is set, this will be restricted to - * within that range. - */ - public abstract double getSafely() throws InvalidSensorException; - - /** - * The calculated output value to achieve the current setpoint. - * - * @return Output value. If output range is set, this will be restricted to - * within that range. - * @warning does not indicate sensor errors - */ - public abstract double get(); - - /** - * A very recent error. - * - * @return The most recent error calculated by the get function. - */ - public abstract double getError(); - - /** - * Get the current input to the PID loop. - * - * @return the current value of the sensor - * - * @warning this does not indicate sensor errors - */ - public double getSensorValue() { - return 0; //return sensor.pidGet(); - } - - /** - * Get the current input to the PID loop. - * - * @return the current value of the sensor - */ - public double getInputSafely() throws InvalidSensorException { - return 0; //sensor.pidGetSafely(); - } - - /** - * The most recent setpoint. - * - * @return The most recent setpoint. - */ - public double getSetpoint() { - return setpoint; - } - - /** - * Sets the setpoint of the motion controller. This is the value that the motion - * controller seeks. - * - * @param setpoint - */ - public void setSetpoint(double setpoint) { - this.setpoint = setpoint; - } - - public boolean didJustReset() { - return justReset; - } - - /** - * Sets the tolerance of the motion controller. When the error is less than the - * tolerance, onTarget returns true. - * - * @param absoluteTolerance - */ - public void setAbsoluteTolerance(double absoluteTolerance) { - if (absoluteTolerance >= 0) { - this.absoluteTolerance = absoluteTolerance; - return; - } - throw new BoundaryException("Absolute tolerance negative"); - } - - /** - * Returns the absolute tolerance set on the motion controller. Will return - * {@link org.usfirst.frc4904.standard.Util#EPSILON Util.EPSILON} - * - * @return absolute tolerance - */ - public double getAbsoluteTolerance() { - return absoluteTolerance; - } - - /** - * Sets the input range of the motion controller. This is only used to work with - * continuous inputs. If minimum is greater than maximum, this will throw an - * exception. - * - * @param minimum - * @param maximum - */ - public void setInputRange(double minimum, double maximum) { - if (minimum > maximum) { - throw new BoundaryException("Minimum is greater than maximum"); - } - inputMin = minimum; - inputMax = maximum; - } - - /** - * Sets the output range of the motion controller. Results from the motion - * control calculation will be capped at these values. The cap is automatically - * enabled by calling this function. - * - * @param minimum - * @param maximum - */ - public void setOutputRange(double minimum, double maximum) { - outputMin = minimum; - outputMax = maximum; - capOutput = true; - } - - /** - * Stops capping the output range. - */ - public void disableOutputRange() { - capOutput = false; - } - - /** - * Sets the input range to continuous. This means that it will treat the maximum - * and minimum sensor and input values as being at the same point, e.g. the - * controller will try to pass through the maximum to get to a point beyond it. - * - * @param continuous - */ - public void setContinuous(boolean continuous) { - this.continuous = continuous; - } - - /** - * Turns on the motion controller. - */ - public void enable() { - if (isOverridden()) { - return; - } - enable = true; - } - - /** - * Bypasses the motion controller. In some cases, this will still scale by a - * feed forward term of the motion controller. - */ - public void disable() { - if (isOverridden()) { - return; - } - enable = false; - setpoint = 0; //sensor.pidGet(); - } - - /** - * Is motion control enabled? - */ - public boolean isEnabled() { - return enable; - } - - /** - * Set whether or not the motion controller is overridden. - * - * @see #startOverriding() - * @see #stopOverriding() - */ - private void setOverride(boolean overridden) { - this.overridden = overridden; - } - - /** - * Starts overriding the controller. The controller will disable and not be - * allowed to enable until the override is turned off. - * - * @see #stopOverriding() - */ - public void startOverriding() { - disable(); - setOverride(true); - } - - /** - * Stops overriding the motion controller. Enabling the controller will now be - * allowed. - * - * @see #startOverriding() - */ - public void stopOverriding() { - setOverride(false); - } - - /** - * Has the controller been overridden? - * - * @see #setOverride(boolean) - */ - public boolean isOverridden() { - return overridden; - } - - /** - * True if the error in the motion controller is less than the tolerance of the - * motion controller. - * - * @return - */ - public boolean onTarget() { - return Math.abs(getError()) <= absoluteTolerance; - } - - /** - * Check if the motion controller has generated an exception within the - * TimerTask. If there is not an exception, the function returns null. - * - * @return the exception (probably null) - */ - public Exception checkException() { - return sensorException; - } -} diff --git a/subsystems/motor/SensorMotor.java b/subsystems/motor/SensorMotor.java index 33d323f4..ca58faf7 100644 --- a/subsystems/motor/SensorMotor.java +++ b/subsystems/motor/SensorMotor.java @@ -3,7 +3,8 @@ import java.util.function.DoubleConsumer; import org.usfirst.frc4904.standard.LogKitten; -import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController; +import org.usfirst.frc4904.standard.custom.MCChassisController; +import org.usfirst.frc4904.standard.custom.MCChassisController.MineCraft; import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; @@ -11,59 +12,50 @@ @Deprecated public abstract class SensorMotor extends Motor implements DoubleConsumer { - protected final MotionController motionController; + protected final MCChassisController.MineCraft minecraft; - public SensorMotor(String name, boolean inverted, SpeedModifier speedModifier, MotionController motionController, + public SensorMotor(String name, boolean inverted, SpeedModifier speedModifier, MCChassisController.MineCraft minecraft, MotorController... motors) { super(name, inverted, speedModifier, motors); - this.motionController = motionController; + this.minecraft = minecraft; } - public SensorMotor(String name, boolean isInverted, MotionController motionController, MotorController... motors) { - this(name, isInverted, new IdentityModifier(), motionController, motors); + public SensorMotor(String name, boolean isInverted, MCChassisController.MineCraft minecraft, MotorController... motors) { + this(name, isInverted, new IdentityModifier(), minecraft, motors); } - public SensorMotor(String name, SpeedModifier speedModifier, MotionController motionController, + public SensorMotor(String name, SpeedModifier speedModifier, MCChassisController.MineCraft motionController, MotorController... motors) { this(name, false, speedModifier, motionController, motors); } - public SensorMotor(String name, MotionController motionController, MotorController... motors) { + public SensorMotor(String name, MCChassisController.MineCraft motionController, MotorController... motors) { this(name, false, new IdentityModifier(), motionController, motors); } - public SensorMotor(boolean isInverted, SpeedModifier speedModifier, MotionController motionController, + public SensorMotor(boolean isInverted, SpeedModifier speedModifier, MCChassisController.MineCraft motionController, MotorController... motors) { this("SensorMotor", isInverted, speedModifier, motionController, motors); } - public SensorMotor(boolean isInverted, MotionController motionController, MotorController... motors) { + public SensorMotor(boolean isInverted, MCChassisController.MineCraft motionController, MotorController... motors) { this("SensorMotor", isInverted, motionController, motors); } - public SensorMotor(SpeedModifier speedModifier, MotionController motionController, MotorController... motors) { + public SensorMotor(SpeedModifier speedModifier, MCChassisController.MineCraft motionController, MotorController... motors) { this("SensorMotor", speedModifier, motionController, motors); } - public SensorMotor(MotionController motionController, MotorController... motors) { + public SensorMotor(MCChassisController.MineCraft motionController, MotorController... motors) { this("SensorMotor", motionController, motors); } public void reset() throws InvalidSensorException { - motionController.reset(); + minecraft.reset(); } public void setInputRange(double minimum, double maximum) { - motionController.setInputRange(minimum, maximum); - } - - public void enableMotionController() { - motionController.setOutput(this); - motionController.enable(); - } - - public void disableMotionController() { - motionController.disable(); + minecraft.setInputRange(minimum, maximum); } @Override @@ -71,44 +63,6 @@ public void accept(double speed) { super.set(speed); } - /** - * Starts overriding the controller. The controller will disable and not be - * allowed to enable until the override is turned off. - * - * @see MotionController#startOverriding() - */ - public void startOverridingMotionController() { - motionController.startOverriding(); - } - - /** - * Stops overriding the motion controller. Enabling the controller will now be - * allowed. - * - * @see MotionController#stopOverriding() - */ - public void stopOverridingMotionController() { - motionController.stopOverriding(); - } - - /** - * Has the motion controller been overridden? - * - * @see MotionController#isOverridden() - */ - public boolean isMotorControllerOverridden() { - return motionController.isOverridden(); - } - - /** - * Returns any exceptions generated by the underlying motion controller. Null - * under normal use. - * - * @return exceptions from the motion controller - */ - public Exception checkSensorException() { - return motionController.checkException(); - } /** * Set the position of a sensor motor @@ -118,7 +72,6 @@ public Exception checkSensorException() { */ public void setPositionSafely(double position) throws InvalidSensorException { motionController.setSetpoint(position); - motionController.enable(); double speed = motionController.getSafely(); LogKitten.v(getName() + " set to position " + position + " at speed " + speed); super.set(speed); // TODO remove and test I don't know why this is here. @@ -132,13 +85,8 @@ public void setPositionSafely(double position) throws InvalidSensorException { */ public void setPosition(double position) { motionController.setSetpoint(position); - motionController.enable(); - double speed = motionController.get(); + double speed = motionController.getSafely(); LogKitten.v(getName() + " set to position " + position + " at speed " + speed); super.set(speed); } - - public boolean onTarget() { - return motionController.onTarget(); - } } From 89c99732ecbfc48e4872dfe2504cb7647e35386a Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Tue, 14 Mar 2023 01:49:42 -0500 Subject: [PATCH 079/134] removed some warnings --- commands/solenoid/SolenoidSet.java | 2 +- custom/motioncontrollers/ezMotion.java | 5 ----- custom/motorcontrollers/TalonMotorController.java | 1 - 3 files changed, 1 insertion(+), 7 deletions(-) diff --git a/commands/solenoid/SolenoidSet.java b/commands/solenoid/SolenoidSet.java index 1dc760c3..41054395 100644 --- a/commands/solenoid/SolenoidSet.java +++ b/commands/solenoid/SolenoidSet.java @@ -10,7 +10,7 @@ * Command to set the state of a SolenoidSubsystem * TODO: rewrite using inline commands on SolenoidSubsystem */ -@Deprecated + public class SolenoidSet extends CommandBase { protected final SolenoidSubsystem system; protected final SolenoidState state; diff --git a/custom/motioncontrollers/ezMotion.java b/custom/motioncontrollers/ezMotion.java index d32f157a..13d9d5a0 100644 --- a/custom/motioncontrollers/ezMotion.java +++ b/custom/motioncontrollers/ezMotion.java @@ -4,13 +4,8 @@ import java.util.function.DoubleFunction; import java.util.function.DoubleSupplier; -import javax.naming.InitialContext; - import org.opencv.core.Mat.Tuple2; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; - -import edu.wpi.first.math.Pair; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.Subsystem; diff --git a/custom/motorcontrollers/TalonMotorController.java b/custom/motorcontrollers/TalonMotorController.java index 996fecad..a53e867a 100644 --- a/custom/motorcontrollers/TalonMotorController.java +++ b/custom/motorcontrollers/TalonMotorController.java @@ -1,6 +1,5 @@ package org.usfirst.frc4904.standard.custom.motorcontrollers; -import com.ctre.phoenix.motorcontrol.IMotorController; import com.ctre.phoenix.motorcontrol.IMotorControllerEnhanced; /** From a1993535da7480d9ed6031f29f050784eb361c41 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Tue, 14 Mar 2023 00:32:08 -0700 Subject: [PATCH 080/134] enable zeroing sensors to non-zero values --- subsystems/motor/SmartMotorSubsystem.java | 1 + subsystems/motor/SparkMaxMotorSubsystem.java | 4 ++++ subsystems/motor/TalonMotorSubsystem.java | 8 ++++++-- 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/subsystems/motor/SmartMotorSubsystem.java b/subsystems/motor/SmartMotorSubsystem.java index a21c5edd..42363b3e 100644 --- a/subsystems/motor/SmartMotorSubsystem.java +++ b/subsystems/motor/SmartMotorSubsystem.java @@ -253,6 +253,7 @@ public final void coast() { * TODO: replace with ezControl */ public abstract void setRPM(double voltage); + public abstract void zeroSensors(double rotations); // you must zero the sensors before using position closed loop public abstract void zeroSensors(); // you must zero the sensors before using position closed loop /** * Configure the PIDF constants, and also configure encoder inversion to be diff --git a/subsystems/motor/SparkMaxMotorSubsystem.java b/subsystems/motor/SparkMaxMotorSubsystem.java index 74587901..6d830f84 100644 --- a/subsystems/motor/SparkMaxMotorSubsystem.java +++ b/subsystems/motor/SparkMaxMotorSubsystem.java @@ -184,6 +184,10 @@ public void setRPM(double rpm) { } // TODO the following methods are not thought out or documented + public void zeroSensors(double rotations) { + encoder = leadMotor.getEncoder(); + encoder.setPosition(rotations); + } /** * Zero the sensors. This should be called once on start up, when the motors * are in a known state. Absolute sensor positioning is used for closed loop diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index 005ffb36..eaa058d8 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -327,11 +327,15 @@ public Command c_holdPosition(double setpoint, int dmp_slot) { @Override - public void zeroSensors() { - this.leadMotor.setSelectedSensorPosition(0, DEFAULT_PID_SLOT, configTimeoutMs); + public void zeroSensors(double rotations) { + this.leadMotor.setSelectedSensorPosition(rotations * ENCODER_COUNTS_PER_REV, DEFAULT_PID_SLOT, configTimeoutMs); // should we zero the sensors on follow motors in case they are being used? } @Override + public void zeroSensors() { + zeroSensors(0); + } + @Override protected void setDynamicMotionProfileTargetRotations(double rotations) { this.leadMotor.set(ControlMode.MotionMagic, rotations*RPM_TO_ENCODERCOUNTSPER100MS); } From fa1bc91c71c66da37a64d5dcb854846f8ae34dd8 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Tue, 14 Mar 2023 01:21:36 -0700 Subject: [PATCH 081/134] delete old methods from sensormotor Co-authored-by: zbuster05 --- subsystems/motor/SensorMotor.java | 26 -------------------------- 1 file changed, 26 deletions(-) diff --git a/subsystems/motor/SensorMotor.java b/subsystems/motor/SensorMotor.java index ca58faf7..81ccb4c2 100644 --- a/subsystems/motor/SensorMotor.java +++ b/subsystems/motor/SensorMotor.java @@ -63,30 +63,4 @@ public void accept(double speed) { super.set(speed); } - - /** - * Set the position of a sensor motor - * - * @param position - * @throws InvalidSensorException - */ - public void setPositionSafely(double position) throws InvalidSensorException { - motionController.setSetpoint(position); - double speed = motionController.getSafely(); - LogKitten.v(getName() + " set to position " + position + " at speed " + speed); - super.set(speed); // TODO remove and test I don't know why this is here. - } - - /** - * Set the position of a sensor motor - * - * @param position - * @warning this does not indicate sensor failure - */ - public void setPosition(double position) { - motionController.setSetpoint(position); - double speed = motionController.getSafely(); - LogKitten.v(getName() + " set to position " + position + " at speed " + speed); - super.set(speed); - } } From 24948f8c96acd5b210b5c3ef9a3e0191507e857a Mon Sep 17 00:00:00 2001 From: Exr0n Date: Tue, 14 Mar 2023 02:25:56 -0700 Subject: [PATCH 082/134] smartmotorsubsystem arbitraryfeedforward for c_control methods --- subsystems/motor/SmartMotorSubsystem.java | 3 +++ subsystems/motor/SparkMaxMotorSubsystem.java | 19 +++++++++++++- subsystems/motor/TalonMotorSubsystem.java | 26 ++++++++++++++++++-- 3 files changed, 45 insertions(+), 3 deletions(-) diff --git a/subsystems/motor/SmartMotorSubsystem.java b/subsystems/motor/SmartMotorSubsystem.java index 42363b3e..e4597a17 100644 --- a/subsystems/motor/SmartMotorSubsystem.java +++ b/subsystems/motor/SmartMotorSubsystem.java @@ -253,6 +253,7 @@ public final void coast() { * TODO: replace with ezControl */ public abstract void setRPM(double voltage); + public abstract void setRPM(double voltage, double feedforwardVolts); public abstract void zeroSensors(double rotations); // you must zero the sensors before using position closed loop public abstract void zeroSensors(); // you must zero the sensors before using position closed loop /** @@ -268,8 +269,10 @@ public final void coast() { public abstract void configPIDF(double p, double i, double d, double f, double max_accumulation, double peakOutput, Integer pid_slot); public abstract void configDMP(double minRPM, double maxRPM, double maxAccl_RPMps, double maxError_encoderTicks, Integer dmp_slot); // you must configure dynamic motion profiles (motionmagic or smartmotion) before using setPosition public abstract void configSoftwareLimits(double fwdBoundRotations, double revBoundRotations); + public abstract Command c_controlRPM(DoubleSupplier setpointSupplier, DoubleSupplier feedforwardSupplierVolts); public abstract Command c_controlRPM(DoubleSupplier setpointSupplier); public abstract Command c_holdRPM(double setpoint); + public abstract Command c_controlPosition(DoubleSupplier setpointSupplier, DoubleSupplier feedforwardSupplierVolts); public abstract Command c_controlPosition(DoubleSupplier setpointSupplier); public abstract Command c_holdPosition(double setpoint); diff --git a/subsystems/motor/SparkMaxMotorSubsystem.java b/subsystems/motor/SparkMaxMotorSubsystem.java index 6d830f84..3f7dc963 100644 --- a/subsystems/motor/SparkMaxMotorSubsystem.java +++ b/subsystems/motor/SparkMaxMotorSubsystem.java @@ -15,6 +15,7 @@ import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMax.SoftLimitDirection; +import com.revrobotics.SparkMaxPIDController.ArbFFUnits; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -182,6 +183,9 @@ private void setFollowMode() { public void setRPM(double rpm) { controller.setReference(rpm, ControlType.kVelocity); } + public void setRPM(double rpm, double feedforwardVolts) { + controller.setReference(rpm, ControlType.kVelocity, DEFAULT_PID_SLOT, feedforwardVolts); + } // TODO the following methods are not thought out or documented public void zeroSensors(double rotations) { @@ -265,6 +269,10 @@ public Command c_controlRPM(DoubleSupplier setpointSupplier) { if (!pid_configured) throw new IllegalArgumentException(name + " tried to use c_controlRPM without first configPIDF()-ing."); return this.run(() -> setRPM(setpointSupplier.getAsDouble())); } + public Command c_controlRPM(DoubleSupplier setpointSupplier, DoubleSupplier feedforwardSupplierVolts) { + if (!pid_configured) throw new IllegalArgumentException(name + " tried to use c_controlRPM without first configPIDF()-ing."); + return this.run(() -> setRPM(setpointSupplier.getAsDouble(), feedforwardSupplierVolts.getAsDouble())); + } @Override @Deprecated public Command c_setRPM(double setpoint) { return this.runOnce(() -> setRPM(setpoint)); @@ -284,7 +292,16 @@ public Command c_setPosition(double setpoint) { @Override public Command c_controlPosition(DoubleSupplier setpointSupplier) { if (!pid_configured) throw new IllegalArgumentException(name + " tried to use c_controlPosition without first configPIDF()-ing"); - return this.run(() -> setDynamicMotionProfileTargetRotations(setpointSupplier.getAsDouble())); + return this.run(() -> controller.setReference(setpointSupplier.getAsDouble(), ControlType.kPosition)); + } + @Override + public Command c_controlPosition(DoubleSupplier setpointSupplier, DoubleSupplier feedforwardSupplierVolts) { + if (!pid_configured) throw new IllegalArgumentException(name + " tried to use c_controlPosition without first configPIDF()-ing"); + return this.run(() -> controller.setReference( + setpointSupplier.getAsDouble(), ControlType.kPosition, + DEFAULT_PID_SLOT, + feedforwardSupplierVolts.getAsDouble() + )); } @Override public Command c_holdPosition(double setpoint) { diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index eaa058d8..c0357fc7 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -8,6 +8,7 @@ import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.LimitSwitchSource; @@ -257,6 +258,15 @@ public void set(double power) { public void setRPM(double rpm) { leadMotor.set(ControlMode.Velocity, rpm / 60 / 10 * ENCODER_COUNTS_PER_REV); // velocity units are in encoder counts per 100ms } + /** + * Consider using .c_controlRPM to stream RPM values to this motor in a command. + * + * You must call .configPIDF before using this method. + * @param rpm + */ + public void setRPM(double rpm, double feedforwardVolts) { + leadMotor.set(ControlMode.Velocity, rpm / 60 / 10 * ENCODER_COUNTS_PER_REV, DemandType.ArbitraryFeedForward, feedforwardVolts/RobotController.getBatteryVoltage()); // velocity units are in encoder counts per 100ms + } /** * if using a Ramsete controller, make sure to disable voltage compensation, @@ -285,6 +295,11 @@ public void setVoltage(double voltage) { */ @Override @Deprecated public Command c_setRPM(double rpm) { return this.runOnce(() -> setRPM(rpm)); } + @Override + public Command c_controlRPM(DoubleSupplier setpointSupplier, DoubleSupplier feedforwardSupplierVolts) { + if (pid_configured == false) throw new IllegalArgumentException(name + " tried to use c_controlRPM without first configPIDF()-ing."); + return this.run(() -> setRPM(setpointSupplier.getAsDouble(), feedforwardSupplierVolts.getAsDouble())); + } /** * * @param setpointSupplier a function that returns a double, rpm @@ -293,12 +308,12 @@ public void setVoltage(double voltage) { */ @Override public Command c_controlRPM(DoubleSupplier setpointSupplier) { - if (pid_configured == false) throw new IllegalArgumentException(name + " tried to use c_controlVelocity without first configPIDF()-ing."); + if (pid_configured == false) throw new IllegalArgumentException(name + " tried to use c_controlRPM without first configPIDF()-ing."); return this.run(() -> setRPM(setpointSupplier.getAsDouble())); } @Override public Command c_holdRPM(double setpoint) { - if (pid_configured == false) throw new IllegalArgumentException(name + " tried to use c_controlVelocity without first configPIDF()-ing."); + if (pid_configured == false) throw new IllegalArgumentException(name + " tried to use c_controlRPM without first configPIDF()-ing."); return this.runOnce(() -> setRPM(setpoint)).andThen(new CommandBase(){}); } /** @@ -312,6 +327,13 @@ public Command c_setPosition(double setpoint, int dmp_slot) { @Override public Command c_setPosition(double setpoint) { return c_setPosition(setpoint, DEFAULT_DMP_SLOT); } + @Override + public Command c_controlPosition(DoubleSupplier setpointSupplier, DoubleSupplier feedforwardSupplierVolts) { + return this.run(() -> this.leadMotor.set( + ControlMode.Position, setpointSupplier.getAsDouble(), + DemandType.ArbitraryFeedForward, feedforwardSupplierVolts.getAsDouble()/RobotController.getBatteryVoltage() + )); + } @Override public Command c_controlPosition(DoubleSupplier setpointSupplier) { return this.run(() -> this.leadMotor.set(ControlMode.Position, setpointSupplier.getAsDouble())); From 551ef8ec66812e47f7c21939cf67d04cbb98579d Mon Sep 17 00:00:00 2001 From: Exr0n Date: Tue, 14 Mar 2023 03:12:54 -0700 Subject: [PATCH 083/134] possible cringe limit switch accessor code --- custom/motorcontrollers/CANTalonFX.java | 11 +++++++++ custom/motorcontrollers/CANTalonSRX.java | 11 +++++++++ .../motorcontrollers/CustomCANSparkMax.java | 23 +++++++++++++++++-- .../SmartMotorController.java | 3 +++ 4 files changed, 46 insertions(+), 2 deletions(-) diff --git a/custom/motorcontrollers/CANTalonFX.java b/custom/motorcontrollers/CANTalonFX.java index 760de5a0..25a111df 100644 --- a/custom/motorcontrollers/CANTalonFX.java +++ b/custom/motorcontrollers/CANTalonFX.java @@ -58,4 +58,15 @@ public TalonMotorController setCoastOnNeutral() { setNeutralMode(NeutralMode.Coast); return this; } + + @Override + public boolean isFwdLimitSwitchPressed() throws IllegalAccessException { + // OPTIM: this should probably support normally closed limit switches too... right now only supports normally open + return getSensorCollection().isFwdLimitSwitchClosed() > 0; + } + @Override + public boolean isRevLimitSwitchPressed() throws IllegalAccessException { + // OPTIM: this should probably support normally closed limit switches too... right now only supports normally open + return getSensorCollection().isRevLimitSwitchClosed() > 0; + } } \ No newline at end of file diff --git a/custom/motorcontrollers/CANTalonSRX.java b/custom/motorcontrollers/CANTalonSRX.java index 700e7b4e..53296fbb 100644 --- a/custom/motorcontrollers/CANTalonSRX.java +++ b/custom/motorcontrollers/CANTalonSRX.java @@ -59,4 +59,15 @@ public TalonMotorController setCoastOnNeutral() { setNeutralMode(NeutralMode.Coast); return this; } + + @Override + public boolean isFwdLimitSwitchPressed() throws IllegalAccessException { + // OPTIM: this should probably support normally closed limit switches too... right now only supports normally open + return getSensorCollection().isFwdLimitSwitchClosed(); + } + @Override + public boolean isRevLimitSwitchPressed() throws IllegalAccessException { + // OPTIM: this should probably support normally closed limit switches too... right now only supports normally open + return getSensorCollection().isRevLimitSwitchClosed(); + } } diff --git a/custom/motorcontrollers/CustomCANSparkMax.java b/custom/motorcontrollers/CustomCANSparkMax.java index 6445e7cf..b7bc1c17 100644 --- a/custom/motorcontrollers/CustomCANSparkMax.java +++ b/custom/motorcontrollers/CustomCANSparkMax.java @@ -1,16 +1,23 @@ package org.usfirst.frc4904.standard.custom.motorcontrollers; import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkMaxLimitSwitch.Type; public class CustomCANSparkMax extends CANSparkMax implements SmartMotorController { // protected Double voltage_compensation_max = null; // remember the configured saturation voltage to conform to the talon api of having separate config() and enable() methods; error if we try to enable without configuring it. - - public CustomCANSparkMax(int deviceNumber, MotorType motorType, boolean inverted) { + protected Type switchType; + + public CustomCANSparkMax(int deviceNumber, MotorType motorType, boolean inverted, Type limitSwitchType) { super(deviceNumber, motorType); setInverted(inverted); + this.switchType = limitSwitchType; // have to store it bc we need to error if we try to read switch pressed state without knowing switch type super.restoreFactoryDefaults(); } + + public CustomCANSparkMax(int deviceNumber, MotorType motorType, boolean inverted) { + this(deviceNumber, motorType, inverted, null); + } /** * Alias for .set() on power @@ -32,4 +39,16 @@ public SmartMotorController setCoastOnNeutral() { public void neutralOutput() { stopMotor(); } + + @Override + public boolean isFwdLimitSwitchPressed() throws IllegalAccessException { + if (switchType == null) throw new IllegalAccessException("Cannot read limit switch state when CustomCANSparkMax was constructed without limit switch type!"); + return getForwardLimitSwitch(switchType).isPressed(); + } + + @Override + public boolean isRevLimitSwitchPressed() throws IllegalAccessException { + if (switchType == null) throw new IllegalAccessException("Cannot read limit switch state when CustomCANSparkMax was constructed without limit switch type!"); + return getReverseLimitSwitch(switchType).isPressed(); + } } diff --git a/custom/motorcontrollers/SmartMotorController.java b/custom/motorcontrollers/SmartMotorController.java index abf70b33..babb7aa0 100644 --- a/custom/motorcontrollers/SmartMotorController.java +++ b/custom/motorcontrollers/SmartMotorController.java @@ -13,6 +13,9 @@ public interface SmartMotorController extends MotorController { public abstract void setPower(double power); + public abstract boolean isFwdLimitSwitchPressed() throws IllegalAccessException; // limit switch methods are here because: talonfx and talonsrx has different types for sensorcollection and I didn't want to make TalonMotorSubsystem generic -> require a type parameter; sparkmax also has it. can't be on subsystem level (because talonMotorSubsystem doesn't differentiate b/w talonfx and talonsrx) so it has to be here. + public abstract boolean isRevLimitSwitchPressed() throws IllegalAccessException; // needs to throw an exception so that the downstream CustomCANSparkMax can throw an exception if we try to read from limit switches before they are declared as normallyOpen or normallyClosed + public abstract SmartMotorController setBrakeOnNeutral(); public abstract SmartMotorController setCoastOnNeutral(); public abstract void neutralOutput(); From c0a618c7162d1e29384b7cec4960e7c195e5d16c Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Tue, 14 Mar 2023 13:11:52 -0500 Subject: [PATCH 084/134] Fixed minecraft --- subsystems/motor/SensorMotor.java | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/subsystems/motor/SensorMotor.java b/subsystems/motor/SensorMotor.java index ca58faf7..083360bb 100644 --- a/subsystems/motor/SensorMotor.java +++ b/subsystems/motor/SensorMotor.java @@ -4,7 +4,6 @@ import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.custom.MCChassisController; -import org.usfirst.frc4904.standard.custom.MCChassisController.MineCraft; import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; @@ -71,8 +70,8 @@ public void accept(double speed) { * @throws InvalidSensorException */ public void setPositionSafely(double position) throws InvalidSensorException { - motionController.setSetpoint(position); - double speed = motionController.getSafely(); + minecraft.setSetpoint(position); + double speed = minecraft.getSafely(); LogKitten.v(getName() + " set to position " + position + " at speed " + speed); super.set(speed); // TODO remove and test I don't know why this is here. } @@ -84,8 +83,8 @@ public void setPositionSafely(double position) throws InvalidSensorException { * @warning this does not indicate sensor failure */ public void setPosition(double position) { - motionController.setSetpoint(position); - double speed = motionController.getSafely(); + minecraft.setSetpoint(position); + double speed = minecraft.getSafely(); LogKitten.v(getName() + " set to position " + position + " at speed " + speed); super.set(speed); } From d873fe5e4b169cfe3b86e4bbf51e693dfc73adc4 Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Tue, 14 Mar 2023 13:21:16 -0500 Subject: [PATCH 085/134] ?? --- subsystems/motor/SensorMotor.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/subsystems/motor/SensorMotor.java b/subsystems/motor/SensorMotor.java index 0b984043..083360bb 100644 --- a/subsystems/motor/SensorMotor.java +++ b/subsystems/motor/SensorMotor.java @@ -62,7 +62,6 @@ public void accept(double speed) { super.set(speed); } -<<<<<<< HEAD /** * Set the position of a sensor motor @@ -89,6 +88,4 @@ public void setPosition(double position) { LogKitten.v(getName() + " set to position " + position + " at speed " + speed); super.set(speed); } -======= ->>>>>>> 551ef8ec66812e47f7c21939cf67d04cbb98579d } From 2a4ee8fc093b9439ec1b2d35accff397c6b55be1 Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Wed, 15 Mar 2023 15:59:33 -0500 Subject: [PATCH 086/134] TEMP: Removed logkitten because of its performance impacts. Hopefully a suitable alternative can be implemented eventually. --- CommandRobotBase.java | 4 +- LogKitten.java | 768 +++++++++--------- commands/Cancel.java | 4 +- commands/Idle.java | 12 +- commands/KittenCommand.java | 44 +- commands/SafetyCommand.java | 5 +- commands/motor/MotorConstant.java | 4 +- commands/motor/MotorIdle.java | 11 +- commands/motor/MotorSet.java | 14 +- custom/CustomCAN.java | 5 +- custom/MCChassisController.java | 4 +- custom/sensors/CANEncoder.java | 15 +- custom/sensors/CANInfraredDistanceSensor.java | 7 +- custom/sensors/CANSensor.java | 4 +- .../sensors/CANUltrasonicDistanceSensor.java | 5 +- custom/sensors/CustomCANCoder.java | 14 +- custom/sensors/EncoderPair.java | 17 +- custom/sensors/PDP.java | 26 +- subsystems/motor/Motor.java | 4 +- subsystems/motor/SensorMotor.java | 5 +- subsystems/motor/SmartMotorSubsystem.java | 5 +- .../motor/speedmodifiers/AccelerationCap.java | 14 +- .../speedmodifiers/EnableableModifier.java | 2 + subsystems/net/UDPSocket.java | 4 +- 24 files changed, 511 insertions(+), 486 deletions(-) diff --git a/CommandRobotBase.java b/CommandRobotBase.java index dfcb579b..75e0bf89 100644 --- a/CommandRobotBase.java +++ b/CommandRobotBase.java @@ -76,11 +76,11 @@ public final void robotInit() { public final void teleopInit() { cleanup(); if (driverChooser.getSelected() != null) { - LogKitten.d("Loading driver " + driverChooser.getSelected().getName()); + // LogKitten.d("Loading driver " + driverChooser.getSelected().getName()); driverChooser.getSelected().bindCommands(); } if (operatorChooser.getSelected() != null) { - LogKitten.d("Loading operator " + operatorChooser.getSelected().getName()); + // LogKitten.d("Loading operator " + operatorChooser.getSelected().getName()); operatorChooser.getSelected().bindCommands(); } teleopInitialize(); diff --git a/LogKitten.java b/LogKitten.java index 4140feeb..b9e08734 100644 --- a/LogKitten.java +++ b/LogKitten.java @@ -1,384 +1,384 @@ -package org.usfirst.frc4904.standard; - -import java.io.BufferedOutputStream; -import java.io.File; -import java.io.FileOutputStream; -import java.io.IOException; -import java.io.PrintWriter; -import java.io.StringWriter; -import java.nio.file.Files; -import java.text.SimpleDateFormat; -import java.util.Date; -import edu.wpi.first.hal.HAL; // DONT REMOVE, see TODO below - -public class LogKitten { - private static BufferedOutputStream fileOutput; - public final static KittenLevel LEVEL_WTF = KittenLevel.WTF; - public final static KittenLevel LEVEL_FATAL = KittenLevel.FATAL; - public final static KittenLevel LEVEL_ERROR = KittenLevel.ERROR; - public final static KittenLevel LEVEL_WARN = KittenLevel.WARN; - public final static KittenLevel LEVEL_VERBOSE = KittenLevel.VERBOSE; - public final static KittenLevel LEVEL_DEBUG = KittenLevel.DEBUG; - public final static KittenLevel DEFAULT_LOG_LEVEL = KittenLevel.DEBUG; - public final static KittenLevel DEFAULT_PRINT_LEVEL = KittenLevel.WARN; - public final static KittenLevel DEFAULT_DS_LEVEL = LogKitten.DEFAULT_PRINT_LEVEL; - private static KittenLevel logLevel = LogKitten.DEFAULT_LOG_LEVEL; - private static KittenLevel printLevel = LogKitten.DEFAULT_PRINT_LEVEL; - private static KittenLevel dsLevel = LogKitten.DEFAULT_DS_LEVEL; - private static String LOG_PATH = "/home/lvuser/logs/"; - private static String LOG_ALIAS_PATH = LogKitten.LOG_PATH + "recent.log"; - private static volatile boolean PRINT_MUTE = false; - private static final SimpleDateFormat TIMESTAMP_FORMAT = new SimpleDateFormat("yyyy-MM-dd_HH:mm:ss"); - static { - File logPathDirectory = new File(LogKitten.LOG_PATH); - try { - if (!logPathDirectory.isDirectory()) { // ensure that the directory /home/lvuser/logs/ exists - logPathDirectory.mkdirs(); // otherwise create all the directories of the path - } - } catch (SecurityException se) { - System.out.println("Could not create log directory"); - se.printStackTrace(); - } - String filePath = LogKitten.LOG_PATH + LogKitten.timestamp() + ".log"; // Set this sessions log to - // /home/lvuser/logs/[current time].log - File file = new File(filePath); - try { - // Create new file if it doesn't exist (this should happen) - file.createNewFile(); // creates if does not exist - // Create FileOutputStream to actually write to the file. - LogKitten.fileOutput = new BufferedOutputStream(new FileOutputStream(file)); - } catch (IOException ioe) { - System.out.println("Could not open logfile"); - ioe.printStackTrace(); - } - File logAlias = new File(LogKitten.LOG_ALIAS_PATH); - try { - if (logAlias.exists()) { - logAlias.delete(); - } - Files.createSymbolicLink(logAlias.toPath(), file.toPath()); - } catch (IOException ioe) { - System.out.println("Could not alias logfile"); - ioe.printStackTrace(); - } - } - - /** - * Get the name of a logger method's caller - * - * @return the caller for the callee `f`, `e`, `w`, `v`, or `d` - */ - private static String getLoggerMethodCallerMethodName() { - return Thread.currentThread().getStackTrace()[4].getMethodName(); // caller of the logger method is fifth in the - // stack trace - } - - /** - * Get the name of a logger method's calling class - * - * @return the caller for the callee `f`, `e`, `w`, `v`, or `d` - */ - private static String getLoggerMethodCallerClassName() { - String[] trace = Thread.currentThread().getStackTrace()[4].getClassName().split("\\."); // caller of the logger - // method is fifth in - // the stack trace - if (trace.length == 0) { - return ""; - } - return trace[trace.length - 1]; // don't include the package name - } - - /** - * Set the default level for which logs will be streamed to a file (for all - * LogKitten instances) - * - * @param DEFAULT_LOG_LEVEL default write-to-file level - */ - public static void setDefaultLogLevel(KittenLevel DEFAULT_LOG_LEVEL) { - LogKitten.logLevel = DEFAULT_LOG_LEVEL; - } - - /** - * Set the default level for which logs will be printed to the console (for all - * LogKitten instances) - * - * @param DEFAULT_PRINT_LEVEL default console log level - */ - public static void setDefaultPrintLevel(KittenLevel DEFAULT_PRINT_LEVEL) { - LogKitten.printLevel = DEFAULT_PRINT_LEVEL; - } - - /** - * Set the default level for which logs will be printed to the driver station - * (for all LogKitten instances) - * - * @param DEFAULT_DS_LEVEL default driver station level - */ - public static void setDefaultDSLevel(KittenLevel DEFAULT_DS_LEVEL) { - LogKitten.dsLevel = DEFAULT_DS_LEVEL; - } - - /** - * Set the logfile path for all LogKitten instances - * - * @param LOG_PATH logfile path as a string - */ - public static void setLogPath(String LOG_PATH) { - LogKitten.LOG_PATH = LOG_PATH; - } - - /** - * Mutes all messages except those overriding (useful for debugging) - * - * @param mute - */ - public static void setPrintMute(boolean mute) { - LogKitten.PRINT_MUTE = mute; - } - - /** - * Like DriverStation.reportError, but without stack trace nor printing to - * System.err (updated for 2017 WPILib release) - * - * @see edu.wpi.first.wpilibj.DriverStation.reportError - * @param errorString - */ - private static void reportErrorToDriverStation(String details, String errorMessage, KittenLevel logLevel) { - // HAL.sendError(true, logLevel.getSeverity(), false, errorMessage, details, "", false); - // TODO: this is now broken. commenting out causes silent failure. how do you fix it - } - - public static synchronized void logMessage(Object message, KittenLevel level, boolean override) { - message = message.toString(); // Not strictly needed, but good practice - if (LogKitten.logLevel.compareTo(level) >= 0) { - String content = LogKitten.timestamp() + " " + level.getName() + ": " - + LogKitten.getLoggerMethodCallerMethodName() + ": " + message + " \n"; - try { - if (LogKitten.fileOutput != null) { - LogKitten.fileOutput.write(content.getBytes()); - } else { - System.out.println("Error logging: logfile not open"); - } - } catch (IOException ioe) { - System.out.println("Error logging " + level.getName() + " message"); - ioe.printStackTrace(); - } - } - if (!LogKitten.PRINT_MUTE || override) { - String printContent = level.getName() + ": " + LogKitten.getLoggerMethodCallerClassName() + "#" - + LogKitten.getLoggerMethodCallerMethodName() + ": " + message + " \n"; - if (LogKitten.printLevel.compareTo(level) >= 0) { - System.out.println(printContent); - } - if (LogKitten.dsLevel.compareTo(level) >= 0) { - LogKitten.reportErrorToDriverStation( - LogKitten.getLoggerMethodCallerClassName() + "#" + LogKitten.getLoggerMethodCallerMethodName(), - level.getName() + ": " + message, level); - } - } - } - - /** - * What a Terrible Failure: Report a condition that should never happen, - * allowing override - * - * @param message - * @param override - */ - public static void wtf(Object message, boolean override) { - LogKitten.logMessage(message, KittenLevel.WTF, override); - } - - /** - * What a Terrible Failure: Report a condition that should never happen - * - * @param message the message to log - */ - public static void wtf(Object message) { // Log WTF message - LogKitten.logMessage(message, KittenLevel.WTF, false); - } - - /** - * Log message at level FATAL allowing override - * - * @param message - * @param override - */ - public static void f(Object message, boolean override) { - LogKitten.logMessage(message, KittenLevel.FATAL, override); - } - - /** - * Log message at level FATAL - * - * @param message the message to log - */ - public static void f(Object message) { // Log fatal message - LogKitten.logMessage(message, KittenLevel.FATAL, false); - } - - /** - * Log message at ERROR allowing override - * - * @param message - * @param override - */ - public static void e(Object message, boolean override) { - LogKitten.logMessage(message, KittenLevel.ERROR, override); - } - - /** - * Log message at level ERROR - * - * @param message the message to log - */ - public static void e(Object message) { // Log error message - LogKitten.logMessage(message, KittenLevel.ERROR, false); - } - - /** - * Log message at WARN allowing override - * - * @param message - * @param override - */ - public static void w(Object message, boolean override) { - LogKitten.logMessage(message, KittenLevel.WARN, override); - } - - /** - * Log message at level WARN - * - * @param message the message to log - */ - public static void w(Object message) { // Log warn message - LogKitten.logMessage(message, KittenLevel.WARN, false); - } - - /** - * Log message at VERBOSE allowing override - * - * @param message - * @param override - */ - public static void v(Object message, boolean override) { - LogKitten.logMessage(message, KittenLevel.VERBOSE, override); - } - - /** - * Log message at level VERBOSE - * - * @param message the message to log - */ - public static void v(Object message) { // Log verbose message - LogKitten.logMessage(message, KittenLevel.VERBOSE, false); - } - - /** - * Log message at VERBOSE (INFO links to verbose) allowing override - * - * @param message - * @param override - */ - public static void i(Object message, boolean override) { - LogKitten.logMessage(message, KittenLevel.VERBOSE, override); - } - - /** - * Log message at VERBOSE (INFO links to verbose) - * - * @param message - */ - public static void i(Object message) { - LogKitten.logMessage(message, KittenLevel.VERBOSE, false); - } - - /** - * Log message at level DEBUG allowing override - * - * @param message - * @param override - */ - public static void d(Object message, boolean override) { - LogKitten.logMessage(message, KittenLevel.DEBUG, override); - } - - /** - * Log message at level DEBUG - * - * @param message the message to log - */ - public static void d(Object message) { // Log debug message - LogKitten.logMessage(message, KittenLevel.DEBUG, false); - } - - /** - * Log exception at level ERROR allowing override - * - * @param ex the exception to log - * @param override whether or not to override - */ - public static void ex(Exception ex, boolean override) { - StringWriter stackTraceString = new StringWriter(); - ex.printStackTrace(new PrintWriter(stackTraceString)); - LogKitten.logMessage(stackTraceString.toString(), KittenLevel.ERROR, override); - } - - /** - * Log exception at level ERROR - * - * @param ex the exception to log - */ - public static void ex(Exception ex) { - LogKitten.ex(ex, false); - } - - /** - * Tries to close the logfile stream - */ - public static synchronized void clean() { - try { - if (LogKitten.fileOutput != null) { - LogKitten.fileOutput.close(); - } - } catch (IOException ioe) { - System.out.println("Could not close logfile output. This should never happen"); - ioe.printStackTrace(); - } - } - - /** - * Get a timestamp for the current datetime - me-wow! - * - * @return timestamp as string in the format "YEAR-MONTH-DAY_HOUR:MIN:SEC" - */ - private static synchronized String timestamp() { - return LogKitten.TIMESTAMP_FORMAT.format(new Date()); - } - - public static enum KittenLevel { - // Defined in decreasing order of severity. Enum.compareTo uses the definition - // order to compare enum values. - WTF, FATAL, ERROR, WARN, VERBOSE, DEBUG; - - /** - * Get the level severity - * - * @return the level severity as an int - */ - public int getSeverity() { - // Severity is the same as the ordinal, which increases with the order of the - // enum values - return ordinal(); - } - - /** - * Get the level name - * - * @return level name as a string - */ - public String getName() { - return name(); // Enum.name() is the Java builtin to get the name of an enum value - } - } -} +// package org.usfirst.frc4904.standard; + +// import java.io.BufferedOutputStream; +// import java.io.File; +// import java.io.FileOutputStream; +// import java.io.IOException; +// import java.io.PrintWriter; +// import java.io.StringWriter; +// import java.nio.file.Files; +// import java.text.SimpleDateFormat; +// import java.util.Date; +// import edu.wpi.first.hal.HAL; // DONT REMOVE, see TODO below + +// public class LogKitten { +// private static BufferedOutputStream fileOutput; +// public final static KittenLevel LEVEL_WTF = KittenLevel.WTF; +// public final static KittenLevel LEVEL_FATAL = KittenLevel.FATAL; +// public final static KittenLevel LEVEL_ERROR = KittenLevel.ERROR; +// public final static KittenLevel LEVEL_WARN = KittenLevel.WARN; +// public final static KittenLevel LEVEL_VERBOSE = KittenLevel.VERBOSE; +// public final static KittenLevel LEVEL_DEBUG = KittenLevel.DEBUG; +// public final static KittenLevel DEFAULT_LOG_LEVEL = KittenLevel.DEBUG; +// public final static KittenLevel DEFAULT_PRINT_LEVEL = KittenLevel.WARN; +// public final static KittenLevel DEFAULT_DS_LEVEL = LogKitten.DEFAULT_PRINT_LEVEL; +// private static KittenLevel logLevel = LogKitten.DEFAULT_LOG_LEVEL; +// private static KittenLevel printLevel = LogKitten.DEFAULT_PRINT_LEVEL; +// private static KittenLevel dsLevel = LogKitten.DEFAULT_DS_LEVEL; +// private static String LOG_PATH = "/home/lvuser/logs/"; +// private static String LOG_ALIAS_PATH = LogKitten.LOG_PATH + "recent.log"; +// private static volatile boolean PRINT_MUTE = false; +// private static final SimpleDateFormat TIMESTAMP_FORMAT = new SimpleDateFormat("yyyy-MM-dd_HH:mm:ss"); +// static { +// File logPathDirectory = new File(LogKitten.LOG_PATH); +// try { +// if (!logPathDirectory.isDirectory()) { // ensure that the directory /home/lvuser/logs/ exists +// logPathDirectory.mkdirs(); // otherwise create all the directories of the path +// } +// } catch (SecurityException se) { +// System.out.println("Could not create log directory"); +// se.printStackTrace(); +// } +// String filePath = LogKitten.LOG_PATH + LogKitten.timestamp() + ".log"; // Set this sessions log to +// // /home/lvuser/logs/[current time].log +// File file = new File(filePath); +// try { +// // Create new file if it doesn't exist (this should happen) +// file.createNewFile(); // creates if does not exist +// // Create FileOutputStream to actually write to the file. +// LogKitten.fileOutput = new BufferedOutputStream(new FileOutputStream(file)); +// } catch (IOException ioe) { +// System.out.println("Could not open logfile"); +// ioe.printStackTrace(); +// } +// File logAlias = new File(LogKitten.LOG_ALIAS_PATH); +// try { +// if (logAlias.exists()) { +// logAlias.delete(); +// } +// Files.createSymbolicLink(logAlias.toPath(), file.toPath()); +// } catch (IOException ioe) { +// System.out.println("Could not alias logfile"); +// ioe.printStackTrace(); +// } +// } + +// /** +// * Get the name of a logger method's caller +// * +// * @return the caller for the callee `f`, `e`, `w`, `v`, or `d` +// */ +// private static String getLoggerMethodCallerMethodName() { +// return Thread.currentThread().getStackTrace()[4].getMethodName(); // caller of the logger method is fifth in the +// // stack trace +// } + +// /** +// * Get the name of a logger method's calling class +// * +// * @return the caller for the callee `f`, `e`, `w`, `v`, or `d` +// */ +// private static String getLoggerMethodCallerClassName() { +// String[] trace = Thread.currentThread().getStackTrace()[4].getClassName().split("\\."); // caller of the logger +// // method is fifth in +// // the stack trace +// if (trace.length == 0) { +// return ""; +// } +// return trace[trace.length - 1]; // don't include the package name +// } + +// /** +// * Set the default level for which logs will be streamed to a file (for all +// * LogKitten instances) +// * +// * @param DEFAULT_LOG_LEVEL default write-to-file level +// */ +// public static void setDefaultLogLevel(KittenLevel DEFAULT_LOG_LEVEL) { +// LogKitten.logLevel = DEFAULT_LOG_LEVEL; +// } + +// /** +// * Set the default level for which logs will be printed to the console (for all +// * LogKitten instances) +// * +// * @param DEFAULT_PRINT_LEVEL default console log level +// */ +// public static void setDefaultPrintLevel(KittenLevel DEFAULT_PRINT_LEVEL) { +// LogKitten.printLevel = DEFAULT_PRINT_LEVEL; +// } + +// /** +// * Set the default level for which logs will be printed to the driver station +// * (for all LogKitten instances) +// * +// * @param DEFAULT_DS_LEVEL default driver station level +// */ +// public static void setDefaultDSLevel(KittenLevel DEFAULT_DS_LEVEL) { +// LogKitten.dsLevel = DEFAULT_DS_LEVEL; +// } + +// /** +// * Set the logfile path for all LogKitten instances +// * +// * @param LOG_PATH logfile path as a string +// */ +// public static void setLogPath(String LOG_PATH) { +// LogKitten.LOG_PATH = LOG_PATH; +// } + +// /** +// * Mutes all messages except those overriding (useful for debugging) +// * +// * @param mute +// */ +// public static void setPrintMute(boolean mute) { +// LogKitten.PRINT_MUTE = mute; +// } + +// /** +// * Like DriverStation.reportError, but without stack trace nor printing to +// * System.err (updated for 2017 WPILib release) +// * +// * @see edu.wpi.first.wpilibj.DriverStation.reportError +// * @param errorString +// */ +// private static void reportErrorToDriverStation(String details, String errorMessage, KittenLevel logLevel) { +// // HAL.sendError(true, logLevel.getSeverity(), false, errorMessage, details, "", false); +// // TODO: this is now broken. commenting out causes silent failure. how do you fix it +// } + +// public static synchronized void logMessage(Object message, KittenLevel level, boolean override) { +// message = message.toString(); // Not strictly needed, but good practice +// if (LogKitten.logLevel.compareTo(level) >= 0) { +// String content = LogKitten.timestamp() + " " + level.getName() + ": " +// + LogKitten.getLoggerMethodCallerMethodName() + ": " + message + " \n"; +// try { +// if (LogKitten.fileOutput != null) { +// LogKitten.fileOutput.write(content.getBytes()); +// } else { +// System.out.println("Error logging: logfile not open"); +// } +// } catch (IOException ioe) { +// System.out.println("Error logging " + level.getName() + " message"); +// ioe.printStackTrace(); +// } +// } +// if (!LogKitten.PRINT_MUTE || override) { +// String printContent = level.getName() + ": " + LogKitten.getLoggerMethodCallerClassName() + "#" +// + LogKitten.getLoggerMethodCallerMethodName() + ": " + message + " \n"; +// if (LogKitten.printLevel.compareTo(level) >= 0) { +// System.out.println(printContent); +// } +// if (LogKitten.dsLevel.compareTo(level) >= 0) { +// LogKitten.reportErrorToDriverStation( +// LogKitten.getLoggerMethodCallerClassName() + "#" + LogKitten.getLoggerMethodCallerMethodName(), +// level.getName() + ": " + message, level); +// } +// } +// } + +// /** +// * What a Terrible Failure: Report a condition that should never happen, +// * allowing override +// * +// * @param message +// * @param override +// */ +// public static void wtf(Object message, boolean override) { +// LogKitten.logMessage(message, KittenLevel.WTF, override); +// } + +// /** +// * What a Terrible Failure: Report a condition that should never happen +// * +// * @param message the message to log +// */ +// public static void wtf(Object message) { // Log WTF message +// LogKitten.logMessage(message, KittenLevel.WTF, false); +// } + +// /** +// * Log message at level FATAL allowing override +// * +// * @param message +// * @param override +// */ +// public static void f(Object message, boolean override) { +// LogKitten.logMessage(message, KittenLevel.FATAL, override); +// } + +// /** +// * Log message at level FATAL +// * +// * @param message the message to log +// */ +// public static void f(Object message) { // Log fatal message +// LogKitten.logMessage(message, KittenLevel.FATAL, false); +// } + +// /** +// * Log message at ERROR allowing override +// * +// * @param message +// * @param override +// */ +// public static void e(Object message, boolean override) { +// LogKitten.logMessage(message, KittenLevel.ERROR, override); +// } + +// /** +// * Log message at level ERROR +// * +// * @param message the message to log +// */ +// public static void e(Object message) { // Log error message +// LogKitten.logMessage(message, KittenLevel.ERROR, false); +// } + +// /** +// * Log message at WARN allowing override +// * +// * @param message +// * @param override +// */ +// public static void w(Object message, boolean override) { +// LogKitten.logMessage(message, KittenLevel.WARN, override); +// } + +// /** +// * Log message at level WARN +// * +// * @param message the message to log +// */ +// public static void w(Object message) { // Log warn message +// LogKitten.logMessage(message, KittenLevel.WARN, false); +// } + +// /** +// * Log message at VERBOSE allowing override +// * +// * @param message +// * @param override +// */ +// public static void v(Object message, boolean override) { +// LogKitten.logMessage(message, KittenLevel.VERBOSE, override); +// } + +// /** +// * Log message at level VERBOSE +// * +// * @param message the message to log +// */ +// public static void v(Object message) { // Log verbose message +// LogKitten.logMessage(message, KittenLevel.VERBOSE, false); +// } + +// /** +// * Log message at VERBOSE (INFO links to verbose) allowing override +// * +// * @param message +// * @param override +// */ +// public static void i(Object message, boolean override) { +// LogKitten.logMessage(message, KittenLevel.VERBOSE, override); +// } + +// /** +// * Log message at VERBOSE (INFO links to verbose) +// * +// * @param message +// */ +// public static void i(Object message) { +// LogKitten.logMessage(message, KittenLevel.VERBOSE, false); +// } + +// /** +// * Log message at level DEBUG allowing override +// * +// * @param message +// * @param override +// */ +// public static void d(Object message, boolean override) { +// LogKitten.logMessage(message, KittenLevel.DEBUG, override); +// } + +// /** +// * Log message at level DEBUG +// * +// * @param message the message to log +// */ +// public static void d(Object message) { // Log debug message +// LogKitten.logMessage(message, KittenLevel.DEBUG, false); +// } + +// /** +// * Log exception at level ERROR allowing override +// * +// * @param ex the exception to log +// * @param override whether or not to override +// */ +// public static void ex(Exception ex, boolean override) { +// StringWriter stackTraceString = new StringWriter(); +// ex.printStackTrace(new PrintWriter(stackTraceString)); +// LogKitten.logMessage(stackTraceString.toString(), KittenLevel.ERROR, override); +// } + +// /** +// * Log exception at level ERROR +// * +// * @param ex the exception to log +// */ +// public static void ex(Exception ex) { +// LogKitten.ex(ex, false); +// } + +// /** +// * Tries to close the logfile stream +// */ +// public static synchronized void clean() { +// try { +// if (LogKitten.fileOutput != null) { +// LogKitten.fileOutput.close(); +// } +// } catch (IOException ioe) { +// System.out.println("Could not close logfile output. This should never happen"); +// ioe.printStackTrace(); +// } +// } + +// /** +// * Get a timestamp for the current datetime - me-wow! +// * +// * @return timestamp as string in the format "YEAR-MONTH-DAY_HOUR:MIN:SEC" +// */ +// private static synchronized String timestamp() { +// return LogKitten.TIMESTAMP_FORMAT.format(new Date()); +// } + +// public static enum KittenLevel { +// // Defined in decreasing order of severity. Enum.compareTo uses the definition +// // order to compare enum values. +// WTF, FATAL, ERROR, WARN, VERBOSE, DEBUG; + +// /** +// * Get the level severity +// * +// * @return the level severity as an int +// */ +// public int getSeverity() { +// // Severity is the same as the ordinal, which increases with the order of the +// // enum values +// return ordinal(); +// } + +// /** +// * Get the level name +// * +// * @return level name as a string +// */ +// public String getName() { +// return name(); // Enum.name() is the Java builtin to get the name of an enum value +// } +// } +// } diff --git a/commands/Cancel.java b/commands/Cancel.java index 62b5854b..efd1e5a0 100644 --- a/commands/Cancel.java +++ b/commands/Cancel.java @@ -1,6 +1,6 @@ package org.usfirst.frc4904.standard.commands; -import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.LogKitten; import edu.wpi.first.wpilibj2.command.CommandBase; /** @@ -16,7 +16,7 @@ public Cancel(CommandBase command) { } public void initialize() { - LogKitten.v("Initializing " + getName()); + // LogKitten.v("Initializing " + getName()); command.cancel(); } diff --git a/commands/Idle.java b/commands/Idle.java index 8c761472..8a3847e3 100644 --- a/commands/Idle.java +++ b/commands/Idle.java @@ -1,6 +1,6 @@ package org.usfirst.frc4904.standard.commands; -import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.LogKitten; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.Subsystem; @@ -29,21 +29,21 @@ public Idle(Subsystem... subsystems) { @Override public void initialize() { if (verbose) { - LogKitten.v("Idle " + getName() + " initialized."); + // LogKitten.v("Idle " + getName() + " initialized."); } } @Override public void execute() { if (verbose) { - LogKitten.v("Idle " + getName() + " executed."); + // LogKitten.v("Idle " + getName() + " executed."); } } @Override public boolean isFinished() { if (verbose) { - LogKitten.v("Idle " + getName() + " isFinished?"); + // LogKitten.v("Idle " + getName() + " isFinished?"); } return false; } @@ -52,9 +52,9 @@ public boolean isFinished() { public void end(boolean interrupted) { if (verbose) { if (interrupted) { - LogKitten.v("Idle " + getName() + "ended with interrupt."); + // LogKitten.v("Idle " + getName() + "ended with interrupt."); } else { - LogKitten.v("Idle " + getName() + " ended."); + // LogKitten.v("Idle " + getName() + " ended."); } } } diff --git a/commands/KittenCommand.java b/commands/KittenCommand.java index 84c18315..6a7f6c51 100644 --- a/commands/KittenCommand.java +++ b/commands/KittenCommand.java @@ -1,27 +1,27 @@ -package org.usfirst.frc4904.standard.commands; +// package org.usfirst.frc4904.standard.commands; -import org.usfirst.frc4904.standard.LogKitten; -import edu.wpi.first.wpilibj2.command.CommandBase; +// import org.usfirst.frc4904.standard.LogKitten; +// import edu.wpi.first.wpilibj2.command.CommandBase; -/** - * - */ -public class KittenCommand extends CommandBase { - protected final String message; - protected final LogKitten.KittenLevel level; +// /** +// * +// */ +// public class KittenCommand extends CommandBase { +// protected final String message; +// protected final LogKitten.KittenLevel level; - public KittenCommand(String message, LogKitten.KittenLevel level) { - this.message = message; - this.level = level; - } +// public KittenCommand(String message, LogKitten.KittenLevel level) { +// this.message = message; +// this.level = level; +// } - @Override - public void initialize() { - LogKitten.logMessage(message, level, false); - } +// @Override +// public void initialize() { +// LogKitten.logMessage(message, level, false); +// } - @Override - public boolean isFinished() { - return true; - } -} +// @Override +// public boolean isFinished() { +// return true; +// } +// } diff --git a/commands/SafetyCommand.java b/commands/SafetyCommand.java index 9fa48b2d..29a0f078 100644 --- a/commands/SafetyCommand.java +++ b/commands/SafetyCommand.java @@ -1,6 +1,6 @@ package org.usfirst.frc4904.standard.commands; -import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.LogKitten; import edu.wpi.first.wpilibj2.command.CommandBase; /** @@ -40,7 +40,8 @@ public final void execute() { if (reasonUnsafe == null) { reasonUnsafe = "the required safety conditions haven't been met"; } - LogKitten.e("SafetyCommand " + getName() + " cannot run because " + reasonUnsafe + ". Cancelling..."); + System.err.println("SafetyCommand " + getName() + " cannot run because " + reasonUnsafe + ". Cancelling..."); + // LogKitten.e("SafetyCommand " + getName() + " cannot run because " + reasonUnsafe + ". Cancelling..."); } protected void setUnsafeReason(String reasonUnsafe) { diff --git a/commands/motor/MotorConstant.java b/commands/motor/MotorConstant.java index c4c09815..d12ce912 100644 --- a/commands/motor/MotorConstant.java +++ b/commands/motor/MotorConstant.java @@ -1,6 +1,6 @@ package org.usfirst.frc4904.standard.commands.motor; -import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.subsystems.motor.Motor; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -39,7 +39,7 @@ public MotorConstant(Motor motor, double motorSpeed) { @Override public void initialize() { motor.set(motorSpeed); - LogKitten.wtf("emacs"); + // LogKitten.wtf("emacs"); } @Override diff --git a/commands/motor/MotorIdle.java b/commands/motor/MotorIdle.java index 84190efb..b46b2e7a 100644 --- a/commands/motor/MotorIdle.java +++ b/commands/motor/MotorIdle.java @@ -1,6 +1,6 @@ package org.usfirst.frc4904.standard.commands.motor; -import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.subsystems.motor.Motor; import edu.wpi.first.wpilibj2.command.CommandBase; @@ -24,7 +24,7 @@ public MotorIdle(String name, Motor motor) { setName(name); addRequirements(motor); this.motor = motor; - LogKitten.d("MotorIdle created"); + // LogKitten.d("MotorIdle created"); } /** @@ -39,13 +39,13 @@ public MotorIdle(Motor motor) { @Override public void initialize() { motor.set(0); - LogKitten.d("MotorIdle initialized"); + // LogKitten.d("MotorIdle initialized"); } @Override public void execute() { motor.set(0); - LogKitten.d("MotorIdle executing"); + // LogKitten.d("MotorIdle executing"); } @Override @@ -56,7 +56,8 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { if (interrupted) { - LogKitten.d("MotorIdle interrupted"); + System.out.println("MotorIdle interrupted"); + // LogKitten.d("MotorIdle interrupted"); } } } diff --git a/commands/motor/MotorSet.java b/commands/motor/MotorSet.java index 3a2c7597..5014155b 100644 --- a/commands/motor/MotorSet.java +++ b/commands/motor/MotorSet.java @@ -1,6 +1,6 @@ package org.usfirst.frc4904.standard.commands.motor; -import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.subsystems.motor.Motor; import edu.wpi.first.wpilibj.motorcontrol.MotorController; @@ -23,12 +23,12 @@ public MotorSet(String name, Motor motor) { addRequirements(motor); this.motor = motor; speed = 0; - LogKitten.d("MotorSet created for " + motor.getName()); + // LogKitten.d("MotorSet created for " + motor.getName()); } @Override public void initialize() { - LogKitten.d("MotorSet initialized"); + // LogKitten.d("MotorSet initialized"); } /** @@ -36,22 +36,22 @@ public void initialize() { */ public void set(double speed) { this.speed = speed; - LogKitten.d("MotorSet writePipe set to " + speed); + // LogKitten.d("MotorSet writePipe set to " + speed); } @Override public void execute() { motor.set(speed); - LogKitten.d("MotorSet executing with speed " + speed); + // LogKitten.d("MotorSet executing with speed " + speed); } @Override public void end(boolean interrupted) { if (!interrupted) { motor.set(0); - LogKitten.d("MotorSet ended (motor speed set to 0)"); + // LogKitten.d("MotorSet ended (motor speed set to 0)"); } else { - LogKitten.d("MotorSet interrupted (motor speed undefined)"); + // LogKitten.d("MotorSet interrupted (motor speed undefined)"); } } diff --git a/custom/CustomCAN.java b/custom/CustomCAN.java index 3e314912..c8b008c1 100644 --- a/custom/CustomCAN.java +++ b/custom/CustomCAN.java @@ -3,7 +3,7 @@ import java.nio.ByteBuffer; import java.nio.IntBuffer; import java.util.Optional; -import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.LogKitten; import edu.wpi.first.hal.can.CANJNI; import edu.wpi.first.hal.can.CANMessageNotFoundException; import edu.wpi.first.hal.util.UncleanStatusException; @@ -45,7 +45,8 @@ public void write(byte[] data) { try { writeSafely(data); } catch (UncleanStatusException e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); } } diff --git a/custom/MCChassisController.java b/custom/MCChassisController.java index ddb6fc1f..4ccba67e 100644 --- a/custom/MCChassisController.java +++ b/custom/MCChassisController.java @@ -2,7 +2,7 @@ import java.lang.reflect.Array; -import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.Util; import org.usfirst.frc4904.standard.custom.sensors.IMU; import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; @@ -51,7 +51,7 @@ public double getTurnSpeed() { targetYaw = imu.getYaw(); return controller.getTurnSpeed(); } - LogKitten.v(minecraft.getSetpoint() + " " + imu.getYaw() + " " + minecraft.getSafely()); + // LogKitten.v(minecraft.getSetpoint() + " " + imu.getYaw() + " " + minecraft.getSafely()); targetYaw = targetYaw + ((controller.getTurnSpeed() * maxDegreesPerSecond) * ((System.currentTimeMillis() / 1000.0) - lastUpdate)); lastUpdate = System.currentTimeMillis() / 1000.0; diff --git a/custom/sensors/CANEncoder.java b/custom/sensors/CANEncoder.java index 09c50255..7f997f06 100644 --- a/custom/sensors/CANEncoder.java +++ b/custom/sensors/CANEncoder.java @@ -1,6 +1,7 @@ package org.usfirst.frc4904.standard.custom.sensors; // WAS PID SOURCE -import org.usfirst.frc4904.standard.LogKitten; +// TODO FIND BETTER LOG KITTEN REPLACEMENT +// import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.Util; /** @@ -140,7 +141,8 @@ public double getDistance() { try { return getDistanceSafely(); } catch (Exception e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return 0; } } @@ -150,7 +152,8 @@ public boolean getDirection() { try { return getDirectionSafely(); } catch (Exception e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return false; } } @@ -160,7 +163,8 @@ public boolean getStopped() { try { return getStoppedSafely(); } catch (Exception e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return false; } } @@ -170,7 +174,8 @@ public double getRate() { try { return getRateSafely(); } catch (Exception e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return 0; } } diff --git a/custom/sensors/CANInfraredDistanceSensor.java b/custom/sensors/CANInfraredDistanceSensor.java index 3116f35d..63db7509 100644 --- a/custom/sensors/CANInfraredDistanceSensor.java +++ b/custom/sensors/CANInfraredDistanceSensor.java @@ -1,6 +1,6 @@ package org.usfirst.frc4904.standard.custom.sensors; -import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.LogKitten; /** * Infrared Distance Sensor connected via CAN assumes mode containing distance @@ -25,7 +25,8 @@ public double getDistance() { try { return getDistanceSafely(); } catch (Exception e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return 0; } } @@ -33,7 +34,7 @@ public double getDistance() { @Override public double getDistanceSafely() throws InvalidSensorException { int value = readSensor()[CANInfraredDistanceSensor.DISTANCE_SENSOR_ARRAY_INDEX]; - LogKitten.d(name + " read value " + value); + // LogKitten.d(name + " read value " + value); return value; } } diff --git a/custom/sensors/CANSensor.java b/custom/sensors/CANSensor.java index 1eff2383..67b6d1be 100644 --- a/custom/sensors/CANSensor.java +++ b/custom/sensors/CANSensor.java @@ -3,7 +3,7 @@ import java.nio.ByteBuffer; import java.util.LinkedHashMap; import java.util.Map.Entry; -import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.custom.CANMessageUnavailableException; import org.usfirst.frc4904.standard.custom.CustomCAN; @@ -74,7 +74,7 @@ public int[] readSensor() throws InvalidSensorException { throw new InvalidSensorException( "CAN data oudated For CAN sensor " + getName() + " with ID 0x" + Integer.toHexString(messageID)); } - LogKitten.v("Cached Sensor Value Used\n"); + // LogKitten.v("Cached Sensor Value Used\n"); return values; } } diff --git a/custom/sensors/CANUltrasonicDistanceSensor.java b/custom/sensors/CANUltrasonicDistanceSensor.java index 6c50b27c..732c5d16 100644 --- a/custom/sensors/CANUltrasonicDistanceSensor.java +++ b/custom/sensors/CANUltrasonicDistanceSensor.java @@ -1,6 +1,6 @@ package org.usfirst.frc4904.standard.custom.sensors; -import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.LogKitten; public class CANUltrasonicDistanceSensor extends CANSensor implements DistanceSensor { public static final int DISTANCE_SENSOR_ARRAY_INDEX = 0; @@ -21,7 +21,8 @@ public double getDistance() { try { return getDistanceSafely(); } catch (Exception e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return 0; } } diff --git a/custom/sensors/CustomCANCoder.java b/custom/sensors/CustomCANCoder.java index b39ba9d3..9367fdcd 100644 --- a/custom/sensors/CustomCANCoder.java +++ b/custom/sensors/CustomCANCoder.java @@ -4,7 +4,7 @@ import com.ctre.phoenix.sensors.CANCoderConfiguration; import com.ctre.phoenix.sensors.SensorTimeBase; -import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.Util; public class CustomCANCoder extends CANCoder implements CustomEncoder { @@ -41,7 +41,8 @@ public double getRate() { try { return getRateSafely(); } catch (Exception e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return 0; } } @@ -57,7 +58,8 @@ public double getDistance() { try { return getDistanceSafely(); } catch (Exception e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return 0; } } @@ -72,7 +74,8 @@ public boolean getDirection() { try { return getDirectionSafely(); } catch (Exception e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return false; } } @@ -87,7 +90,8 @@ public boolean getStopped() { try { return getStoppedSafely(); } catch (Exception e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return false; } } diff --git a/custom/sensors/EncoderPair.java b/custom/sensors/EncoderPair.java index bb19fc37..83d25111 100644 --- a/custom/sensors/EncoderPair.java +++ b/custom/sensors/EncoderPair.java @@ -1,6 +1,6 @@ package org.usfirst.frc4904.standard.custom.sensors; -import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.Util; /** @@ -68,7 +68,8 @@ public double getDistance() { try { return getDistanceSafely(); } catch (InvalidSensorException e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return 0; } } @@ -103,7 +104,8 @@ public double getRate() { try { return getRateSafely(); } catch (InvalidSensorException e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return 0; } } @@ -156,7 +158,8 @@ public double getDifference() { try { return getDifferenceSafely(); } catch (InvalidSensorException e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return 0.0; // TODO: is this a reasonable default } } @@ -170,7 +173,8 @@ public double getRateDifference() { try { return getRateDifferenceSafely(); } catch (InvalidSensorException e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return 0.0; // TODO: is this a reasonable default } } @@ -183,7 +187,8 @@ public boolean isInSync() { try { return isInSyncSafely(); } catch (InvalidSensorException e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return false; // If a sensor is broken, it is not in sync. } } diff --git a/custom/sensors/PDP.java b/custom/sensors/PDP.java index 1f3a3db0..0eb4ba71 100644 --- a/custom/sensors/PDP.java +++ b/custom/sensors/PDP.java @@ -1,6 +1,6 @@ package org.usfirst.frc4904.standard.custom.sensors; -import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.custom.CANMessageUnavailableException; import org.usfirst.frc4904.standard.custom.CustomCAN; import edu.wpi.first.wpilibj.RobotController; @@ -73,7 +73,8 @@ private void readStatus(int status) throws InvalidSensorException { rawArray = status3.readSafely(); numberCurrents = 4; } else { - LogKitten.w("Trying to read PDP status " + status + ", which does not exist!"); + System.err.println("Trying to read PDP status " + status + ", which does not exist!"); + // LogKitten.w("Trying to read PDP status " + status + ", which does not exist!"); return; } } catch (CANMessageUnavailableException e) { @@ -133,7 +134,8 @@ public double getVoltage() { try { return getVoltageSafely(); } catch (InvalidSensorException e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return RobotController.getBatteryVoltage(); } } @@ -154,7 +156,8 @@ public double getBatteryResistance() { try { return getBatteryResistanceSafely(); } catch (InvalidSensorException e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return cachedResistance; } } @@ -168,7 +171,8 @@ public double getTotalCurrent() { try { return getTotalCurrentSafely(); } catch (InvalidSensorException e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return cachedCurrent; } } @@ -194,7 +198,8 @@ public double getTotalPower() { try { return getTotalPowerSafely(); } catch (InvalidSensorException e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return cachedPower; } } @@ -221,7 +226,8 @@ public double getTotalEnergy() { try { return getTotalEnergySafely(); } catch (InvalidSensorException e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return cachedEnergy; } } @@ -256,7 +262,8 @@ public double getCurrentSafely(int channel) throws InvalidSensorException { } else if (channel <= 15) { readStatus(3); } else { - LogKitten.w("Trying to read PDP channel " + channel + ", which does not exist!"); + System.err.println("Trying to read PDP channel " + channel + ", which does not exist!"); + // LogKitten.w("Trying to read PDP channel " + channel + ", which does not exist!"); return 0.0; } return cachedChannelCurrents[channel]; @@ -272,7 +279,8 @@ public double getCurrent(int channel) { try { return getCurrentSafely(channel); } catch (InvalidSensorException e) { - LogKitten.ex(e); + e.printStackTrace(); + // LogKitten.ex(e); return cachedChannelCurrents[channel]; } } diff --git a/subsystems/motor/Motor.java b/subsystems/motor/Motor.java index 6a88690a..00ea51ad 100644 --- a/subsystems/motor/Motor.java +++ b/subsystems/motor/Motor.java @@ -1,6 +1,5 @@ package org.usfirst.frc4904.standard.subsystems.motor; -import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.commands.motor.MotorIdle; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; @@ -207,8 +206,7 @@ public double get() { */ @Override public void set(double speed) { - - LogKitten.v("Motor " + getName() + " @ " + speed); + // LogKitten.v("Motor " + getName() + " @ " + speed); double newSpeed = speedModifier.modify(speed); lastSpeed = newSpeed; for (MotorController motor : motors) { diff --git a/subsystems/motor/SensorMotor.java b/subsystems/motor/SensorMotor.java index 083360bb..5032fb5d 100644 --- a/subsystems/motor/SensorMotor.java +++ b/subsystems/motor/SensorMotor.java @@ -2,7 +2,6 @@ import java.util.function.DoubleConsumer; -import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.custom.MCChassisController; import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; @@ -72,7 +71,7 @@ public void accept(double speed) { public void setPositionSafely(double position) throws InvalidSensorException { minecraft.setSetpoint(position); double speed = minecraft.getSafely(); - LogKitten.v(getName() + " set to position " + position + " at speed " + speed); + // LogKitten.v(getName() + " set to position " + position + " at speed " + speed); super.set(speed); // TODO remove and test I don't know why this is here. } @@ -85,7 +84,7 @@ public void setPositionSafely(double position) throws InvalidSensorException { public void setPosition(double position) { minecraft.setSetpoint(position); double speed = minecraft.getSafely(); - LogKitten.v(getName() + " set to position " + position + " at speed " + speed); + // LogKitten.v(getName() + " set to position " + position + " at speed " + speed); super.set(speed); } } diff --git a/subsystems/motor/SmartMotorSubsystem.java b/subsystems/motor/SmartMotorSubsystem.java index e4597a17..57d3af9e 100644 --- a/subsystems/motor/SmartMotorSubsystem.java +++ b/subsystems/motor/SmartMotorSubsystem.java @@ -2,7 +2,6 @@ import java.util.function.DoubleSupplier; -import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.custom.motorcontrollers.SmartMotorController; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; @@ -111,7 +110,7 @@ public SmartMotorSubsystem(MotorControllerType... motors) { * @param power The power to set. Value should be between -1.0 and 1.0. */ public void set(double power) { - LogKitten.v("Motor " + getName() + " @ " + power); + // LogKitten.v("Motor " + getName() + " @ " + power); double newPower = speedModifier.modify(power); for (var motor : motors) motor.set(newPower); } @@ -136,7 +135,7 @@ public final void setPower(double power) { * function, it is not "set it and forget it." */ public void setVoltage(double voltage) { - LogKitten.v("Motor " + getName() + " @ " + voltage + "v"); + // LogKitten.v("Motor " + getName() + " @ " + voltage + "v"); for (var motor : motors) { motor.setVoltage(voltage); } diff --git a/subsystems/motor/speedmodifiers/AccelerationCap.java b/subsystems/motor/speedmodifiers/AccelerationCap.java index 86ea4123..6c12f42e 100644 --- a/subsystems/motor/speedmodifiers/AccelerationCap.java +++ b/subsystems/motor/speedmodifiers/AccelerationCap.java @@ -1,6 +1,6 @@ package org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers; -import org.usfirst.frc4904.standard.LogKitten; +// import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.Util; import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException; import org.usfirst.frc4904.standard.custom.sensors.PDP; @@ -92,7 +92,7 @@ protected double calculate(double inputSpeed) { // After doing updates, check for low battery voltage first double currentVoltage = newVoltage; if (currentVoltage < hardStopVoltage) { // If we are below hardStopVoltage, stop motors - LogKitten.w("Low voltage, AccelerationCap stopping motors"); + // LogKitten.w("Low voltage, AccelerationCap stopping motors"); return 0; } if (Math.abs(inputSpeed) < Math.abs(currentSpeed) && Math.signum(inputSpeed) == Math.signum(currentSpeed)) { @@ -113,10 +113,10 @@ protected double calculate(double inputSpeed) { // Even if we are still above the hard stop voltage, try to avoid going below // next tick if (currentVoltage < hardStopVoltage + (lastVoltage - voltage) * AccelerationCap.TICKS_PER_PDP_DATA / 2.0) { - LogKitten.w("Preventative capping to " - + (currentSpeed - - AccelerationCap.ANTI_BROWNOUT_BACKOFF_PER_SECOND * Math.signum(currentSpeed) * deltaTime) - + " from " + inputSpeed); + // LogKitten.w("Preventative capping to " + // + (currentSpeed + // - AccelerationCap.ANTI_BROWNOUT_BACKOFF_PER_SECOND * Math.signum(currentSpeed) * deltaTime) + // + " from " + inputSpeed); return currentSpeed - AccelerationCap.ANTI_BROWNOUT_BACKOFF_PER_SECOND * Math.signum(currentSpeed) * deltaTime; } @@ -135,7 +135,7 @@ protected double calculate(double inputSpeed) { @Override public double modify(double inputSpeed) { currentSpeed = calculate(inputSpeed); - LogKitten.d("AccelerationCap outputed: " + currentSpeed); + // LogKitten.d("AccelerationCap outputed: " + currentSpeed); return currentSpeed; } } \ No newline at end of file diff --git a/subsystems/motor/speedmodifiers/EnableableModifier.java b/subsystems/motor/speedmodifiers/EnableableModifier.java index 037a8258..3c344981 100644 --- a/subsystems/motor/speedmodifiers/EnableableModifier.java +++ b/subsystems/motor/speedmodifiers/EnableableModifier.java @@ -2,6 +2,8 @@ import org.usfirst.frc4904.standard.commands.Idle; +// import org.usfirst.frc4904.standard.commands.Idle; + import edu.wpi.first.wpilibj2.command.SubsystemBase; public class EnableableModifier extends SubsystemBase implements SpeedModifier { diff --git a/subsystems/net/UDPSocket.java b/subsystems/net/UDPSocket.java index 725638b5..21659151 100644 --- a/subsystems/net/UDPSocket.java +++ b/subsystems/net/UDPSocket.java @@ -10,7 +10,7 @@ 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.LogKitten; import org.usfirst.frc4904.standard.subsystems.net.message.Packable; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -65,7 +65,7 @@ public void periodic() { try { pollReceive(); } catch (IOException ex) { - LogKitten.ex(ex); + ex.printStackTrace(); } } } From 398ab9d3e721b1730d5edd035cb0980a0cca8982 Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Wed, 15 Mar 2023 17:49:53 -0500 Subject: [PATCH 087/134] Staging` --- subsystems/chassis/WestCoastDrive.java | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 5769ef0e..01512f1d 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -106,6 +106,7 @@ public void movePolar(double speed, double heading, double turnSpeed) { if (heading != 0) throw new IllegalArgumentException("West Coast Drive cannot move at a non-zero heading!"); setChassisVelocity(new ChassisSpeeds(speed, 0, turnSpeed)); } + public void setWheelVelocities(DifferentialDriveWheelSpeeds wheelSpeeds) { this.leftMotors .setRPM(wheelSpeeds.leftMetersPerSecond * mps_to_rpm); this.rightMotors.setRPM(wheelSpeeds.rightMetersPerSecond * mps_to_rpm); @@ -114,6 +115,14 @@ public void setChassisVelocity(ChassisSpeeds chassisSpeeds) { final var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); setWheelVelocities(wheelSpeeds); } + + public void setChassisVoltage(ChassisSpeeds sketchyVoltages) { + final double SKETCHY_CORRECTION = 1; + final var wheelVoltages = kinematics.toWheelSpeeds(sketchyVoltages); + // we do a little trolling + setWheelVoltages(wheelVoltages.leftMetersPerSecond * SKETCHY_CORRECTION, wheelVoltages.rightMetersPerSecond * SKETCHY_CORRECTION); + } + public void setWheelVoltages(DifferentialDriveWheelVoltages wheelVoltages) { this.setWheelVoltages(wheelVoltages.left, wheelVoltages.right); } @@ -211,6 +220,15 @@ public Command c_controlWheelVelocities(Supplier l cmd.addRequirements(rightMotors); return cmd; } + // TODO sketchy as hell + public Command c_controlChassisVoltage(Supplier chassisPowerSupplier) { + new ChassisSpeeds(); + var cmd = this.run(() -> setChassisVelocity(chassisPowerSupplier.get())); // this.run() runs repeatedly + cmd.addRequirements(leftMotors); + cmd.addRequirements(rightMotors); + return cmd; + } + /** * A forever command that pulls chassis movement * (forward speed and turn * radians/sec) from a * function and From 2646cb68076b08ec9d0ef928ecaebdd4ff8c7014 Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Wed, 15 Mar 2023 18:16:39 -0500 Subject: [PATCH 088/134] NEEDS REVIEW: I think this works? --- subsystems/chassis/WestCoastDrive.java | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 01512f1d..587cd560 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -13,6 +13,7 @@ import com.pathplanner.lib.auto.PIDConstants; import com.pathplanner.lib.auto.RamseteAutoBuilder; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.DifferentialDriveWheelVoltages; import edu.wpi.first.math.controller.RamseteController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -22,6 +23,7 @@ import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -222,8 +224,7 @@ public Command c_controlWheelVelocities(Supplier l } // TODO sketchy as hell public Command c_controlChassisVoltage(Supplier chassisPowerSupplier) { - new ChassisSpeeds(); - var cmd = this.run(() -> setChassisVelocity(chassisPowerSupplier.get())); // this.run() runs repeatedly + var cmd = this.run(() -> setChassisVoltage(chassisPowerSupplier.get())); // this.run() runs repeatedly cmd.addRequirements(leftMotors); cmd.addRequirements(rightMotors); return cmd; @@ -240,6 +241,17 @@ public Command c_controlChassisVelocity(Supplier chassisSpeedsSup cmd.addRequirements(rightMotors); return cmd; } + /** + * A forever command that pulls left and right wheel voltages from a + * function. + */ + public Command c_controlWheelPower(Supplier> wheelVoltageSupplier) { + Pair powers = wheelVoltageSupplier.get(); + var cmd = this.run(() -> c_controlChassisVoltage(() -> new ChassisSpeeds(powers.getFirst()/RobotController.getBatteryVoltage(), 0, powers.getSecond()))); // this.run() runs repeatedly + cmd.addRequirements(leftMotors); + cmd.addRequirements(rightMotors); + return cmd; + } /** * A forever command that pulls left and right wheel voltages from a * function. From bb0eaf8f6ac775e5379a4f8fef709f4ab9019e4b Mon Sep 17 00:00:00 2001 From: Exr0n Date: Wed, 15 Mar 2023 16:32:11 -0700 Subject: [PATCH 089/134] fix names on openloop chassis control Co-authored-by: zbuster05 --- subsystems/chassis/WestCoastDrive.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 587cd560..b6437dc1 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -119,7 +119,7 @@ public void setChassisVelocity(ChassisSpeeds chassisSpeeds) { } public void setChassisVoltage(ChassisSpeeds sketchyVoltages) { - final double SKETCHY_CORRECTION = 1; + final double SKETCHY_CORRECTION = 1; // TODO: tune? @zbuster05 final var wheelVoltages = kinematics.toWheelSpeeds(sketchyVoltages); // we do a little trolling setWheelVoltages(wheelVoltages.leftMetersPerSecond * SKETCHY_CORRECTION, wheelVoltages.rightMetersPerSecond * SKETCHY_CORRECTION); @@ -223,8 +223,8 @@ public Command c_controlWheelVelocities(Supplier l return cmd; } // TODO sketchy as hell - public Command c_controlChassisVoltage(Supplier chassisPowerSupplier) { - var cmd = this.run(() -> setChassisVoltage(chassisPowerSupplier.get())); // this.run() runs repeatedly + public Command c_controlChassisWithVoltage(Supplier chassisSpeedVoltsSupplier) { + var cmd = this.run(() -> setChassisVoltage(chassisSpeedVoltsSupplier.get())); // this.run() runs repeatedly cmd.addRequirements(leftMotors); cmd.addRequirements(rightMotors); return cmd; @@ -245,9 +245,9 @@ public Command c_controlChassisVelocity(Supplier chassisSpeedsSup * A forever command that pulls left and right wheel voltages from a * function. */ - public Command c_controlWheelPower(Supplier> wheelVoltageSupplier) { - Pair powers = wheelVoltageSupplier.get(); - var cmd = this.run(() -> c_controlChassisVoltage(() -> new ChassisSpeeds(powers.getFirst()/RobotController.getBatteryVoltage(), 0, powers.getSecond()))); // this.run() runs repeatedly + public Command c_controlChassisSpeedAndTurn(Supplier> chasssisSpeedAndTurnSupplier) { + Pair speedAndTurn = chasssisSpeedAndTurnSupplier.get(); + var cmd = this.run(() -> c_controlChassisWithVoltage(() -> new ChassisSpeeds(speedAndTurn.getFirst()*RobotController.getBatteryVoltage(), 0, speedAndTurn.getSecond()))); // this.run() runs repeatedly cmd.addRequirements(leftMotors); cmd.addRequirements(rightMotors); return cmd; From 34549eb8ca16c1311a274ec084ee6d2ad08e6f52 Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Wed, 15 Mar 2023 18:46:39 -0500 Subject: [PATCH 090/134] Sketchy driver chooser fr --- CommandRobotBase.java | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/CommandRobotBase.java b/CommandRobotBase.java index 75e0bf89..2dc41d9e 100644 --- a/CommandRobotBase.java +++ b/CommandRobotBase.java @@ -1,5 +1,7 @@ package org.usfirst.frc4904.standard; +import java.io.Console; + import org.usfirst.frc4904.standard.custom.CommandSendableChooser; import org.usfirst.frc4904.standard.custom.TypedNamedSendableChooser; import org.usfirst.frc4904.standard.humaninput.Driver; @@ -15,6 +17,7 @@ * CommandRobotBase class. Robot should extend this instead of iterative robot. */ public abstract class CommandRobotBase extends TimedRobot { + private Command autonomousCommand; protected Command teleopCommand; protected CommandSendableChooser autoChooser; @@ -45,6 +48,23 @@ private void cleanup() { } } + // HACK TODO, incredibly cursed and potentially bad + public static Driver drivingConfig = new Driver("uhohhh") { + @Override + public double getX() { + System.err.println("DRIVER NOT CONFIGED"); + return 0;} + + @Override + public double getY() {return 0;} + + @Override + public double getTurnSpeed() {return 0;} + + @Override + public void bindCommands() {} + }; + /** * This initializes the entire robot. It is called by WPILib on robot code * launch. Year-specific code should be written in the initialize function. @@ -77,6 +97,7 @@ public final void teleopInit() { cleanup(); if (driverChooser.getSelected() != null) { // LogKitten.d("Loading driver " + driverChooser.getSelected().getName()); + CommandRobotBase.drivingConfig = driverChooser.getSelected(); driverChooser.getSelected().bindCommands(); } if (operatorChooser.getSelected() != null) { From b833d752f6277296b56b11bdb22519819084edc3 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Wed, 15 Mar 2023 17:06:16 -0700 Subject: [PATCH 091/134] complete merge \/ --- subsystems/chassis/WestCoastDrive.java | 6 ------ 1 file changed, 6 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index cabf9590..b6437dc1 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -28,15 +28,9 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -<<<<<<< HEAD -public class WestCoastDrive extends SubsystemBase { - public final SmartMotorSubsystem leftMotors; - public final SmartMotorSubsystem rightMotors; -======= public class WestCoastDrive extends SubsystemBase { public final TalonMotorSubsystem leftMotors; public final TalonMotorSubsystem rightMotors; ->>>>>>> 841c1910e5ed1200efb50c6b3a041818ea7fe13d protected final PIDConstants pidConsts; protected final DifferentialDriveKinematics kinematics; protected final DifferentialDriveOdometry odometry; // OPTIM this can be replaced with a kalman filter? From a8e5d5985599b3442e23e7f87a91acad584cdd80 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Wed, 15 Mar 2023 18:00:35 -0700 Subject: [PATCH 092/134] no operator fail more gracefully; fix westcoastdrive command factories .run-ing twice --- CommandRobotBase.java | 5 +++-- subsystems/chassis/WestCoastDrive.java | 14 +++++++++----- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/CommandRobotBase.java b/CommandRobotBase.java index 2dc41d9e..be97f539 100644 --- a/CommandRobotBase.java +++ b/CommandRobotBase.java @@ -52,8 +52,9 @@ private void cleanup() { public static Driver drivingConfig = new Driver("uhohhh") { @Override public double getX() { - System.err.println("DRIVER NOT CONFIGED"); - return 0;} + for (int i=0; i<1000000; i++) System.err.println("DRIVER NOT CONFIGED"); + return 0; + } @Override public double getY() {return 0;} diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index b6437dc1..019faec3 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -24,6 +24,7 @@ import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -224,7 +225,10 @@ public Command c_controlWheelVelocities(Supplier l } // TODO sketchy as hell public Command c_controlChassisWithVoltage(Supplier chassisSpeedVoltsSupplier) { - var cmd = this.run(() -> setChassisVoltage(chassisSpeedVoltsSupplier.get())); // this.run() runs repeatedly + var cmd = this.run(() -> { + var volts = chassisSpeedVoltsSupplier.get(); + setChassisVoltage(volts); + }); // this.run() runs repeatedly cmd.addRequirements(leftMotors); cmd.addRequirements(rightMotors); return cmd; @@ -246,10 +250,10 @@ public Command c_controlChassisVelocity(Supplier chassisSpeedsSup * function. */ public Command c_controlChassisSpeedAndTurn(Supplier> chasssisSpeedAndTurnSupplier) { - Pair speedAndTurn = chasssisSpeedAndTurnSupplier.get(); - var cmd = this.run(() -> c_controlChassisWithVoltage(() -> new ChassisSpeeds(speedAndTurn.getFirst()*RobotController.getBatteryVoltage(), 0, speedAndTurn.getSecond()))); // this.run() runs repeatedly - cmd.addRequirements(leftMotors); - cmd.addRequirements(rightMotors); + var cmd = c_controlChassisWithVoltage(() -> { + Pair speedAndTurn = chasssisSpeedAndTurnSupplier.get(); + return new ChassisSpeeds(speedAndTurn.getFirst()*RobotController.getBatteryVoltage(), 0, speedAndTurn.getSecond()); + }); return cmd; } /** From 80a509b863868ea293bb1c346cc853bb9359acd8 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Wed, 15 Mar 2023 21:09:10 -0700 Subject: [PATCH 093/134] remove smartmotorsubsystem defaultcommand --- subsystems/motor/SmartMotorSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/subsystems/motor/SmartMotorSubsystem.java b/subsystems/motor/SmartMotorSubsystem.java index 57d3af9e..d1fbcbd3 100644 --- a/subsystems/motor/SmartMotorSubsystem.java +++ b/subsystems/motor/SmartMotorSubsystem.java @@ -42,7 +42,7 @@ public SmartMotorSubsystem(String name, SpeedModifier speedModifier, MotorContro for (var motor : motors) { motor.set(0); } - setDefaultCommand(this.runOnce(() -> this.neutralOutput())); + // setDefaultCommand(this.runOnce(() -> this.neutralOutput())); } /** From c82d37ce3af872bdc369c6c103d2c1de6e34fa16 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 16 Mar 2023 00:50:35 -0700 Subject: [PATCH 094/134] rip out follow mode --- subsystems/motor/TalonMotorSubsystem.java | 41 ++++++++++------------- 1 file changed, 17 insertions(+), 24 deletions(-) diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index c0357fc7..5ec7de2e 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -43,7 +43,7 @@ public class TalonMotorSubsystem extends SmartMotorSubsystem { private static final int ENCODER_COUNTS_PER_REV = 2048; private static final double RPM_TO_ENCODERCOUNTSPER100MS = ENCODER_COUNTS_PER_REV/60/10; - private final int configTimeoutMs = 50; // milliseconds until the Talon gives up trying to configure + private final int configTimeoutMs = 5; // milliseconds until the Talon gives up trying to configure private static final int DEFAULT_PID_SLOT = 0; // TODO: add support for auxillary pid private final int follow_motors_remote_filter_id = 0; // DONOT REMOVE, USED IN COMMENTED CODE BELOW; which filter (0 or 1) will be used to configure reading from the integrated encoder on the lead motor private final double voltageComp; @@ -130,6 +130,10 @@ public TalonMotorSubsystem(String name, SpeedModifier speedModifier, NeutralMode } } + // feedback sensor configuration (for PID and general encoder stuff) + leadMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, DEFAULT_PID_SLOT, configTimeoutMs); + // make sure to update the static final ENCODER_COUNTS_PER_REV if you are using different encoders. Better yet, add a feedbackSensor argument to this method + // other configuration (neutral mode, neutral deadband, voltagecomp) for (var motor : motors) { motor.setNeutralMode(neutralMode); @@ -141,7 +145,6 @@ public TalonMotorSubsystem(String name, SpeedModifier speedModifier, NeutralMode motor.enableVoltageCompensation(false); } } - setFollowMode(); } /** * Motor Subsystem for a group of Talon motor controllers (Falcons, 775s). @@ -179,14 +182,14 @@ public TalonMotorSubsystem(String name, NeutralMode neutralMode, double voltageC this(name, new IdentityModifier(), neutralMode, 0.001, false, voltageCompensation, leadMotor, followMotors); } - /** - * Make all follow motors follow the lead motor. - */ - private void setFollowMode() { - for (var motor : this.followMotors) { - motor.follow(leadMotor); - } - } + // /** + // * Make all follow motors follow the lead motor. + // */ + // private void setFollowMode() { + // for (var motor : this.followMotors) { + // motor.follow(leadMotor); + // } + // } // TODO the following methods are not thought out or documented /** @@ -206,10 +209,6 @@ private void setFollowMode() { public void configPIDF(double p, double i, double d, double f, double max_integral_accumulation, double peakOutput, Integer pid_slot) { if (pid_slot == null) pid_slot = TalonMotorSubsystem.DEFAULT_PID_SLOT; - // feedback sensor configuration (for PID) - leadMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, pid_slot, configTimeoutMs); - // make sure to update the static final ENCODER_COUNTS_PER_REV if you are using different encoders. Better yet, add a feedbackSensor argument to this method - // should follow motors do their own PID? no, right? // PID constants configuration @@ -244,11 +243,6 @@ public void configDMP(double minRPM, double cruiseRPM, double accl_RPMps, double // don't override disable() or stop() because we *should* indeed use the base implementation of disabling/stopping each motor controller individually. Otherwise the following motors will try to follow a disabled motor, which may cause unexpected behavior (although realistically, it likely just gets set to zero and neutrallized by the deadband). - @Override - public void set(double power) { - setFollowMode(); // make sure all motors are following the lead as we expect. Possible OPTIMIZATION: store we set the output type to something else on the follow motors (eg. Neutral), and only re-set follow mode if we did. - leadMotor.set(power); - } /** * Consider using .c_controlRPM to stream RPM values to this motor in a command. * @@ -256,7 +250,7 @@ public void set(double power) { * @param rpm */ public void setRPM(double rpm) { - leadMotor.set(ControlMode.Velocity, rpm / 60 / 10 * ENCODER_COUNTS_PER_REV); // velocity units are in encoder counts per 100ms + for (var m: motors) m.set(ControlMode.Velocity, rpm / 60 / 10 * ENCODER_COUNTS_PER_REV); // velocity units are in encoder counts per 100ms } /** * Consider using .c_controlRPM to stream RPM values to this motor in a command. @@ -265,7 +259,7 @@ public void setRPM(double rpm) { * @param rpm */ public void setRPM(double rpm, double feedforwardVolts) { - leadMotor.set(ControlMode.Velocity, rpm / 60 / 10 * ENCODER_COUNTS_PER_REV, DemandType.ArbitraryFeedForward, feedforwardVolts/RobotController.getBatteryVoltage()); // velocity units are in encoder counts per 100ms + for (var m : motors) m.set(ControlMode.Velocity, rpm / 60 / 10 * ENCODER_COUNTS_PER_REV, DemandType.ArbitraryFeedForward, feedforwardVolts/RobotController.getBatteryVoltage()); // velocity units are in encoder counts per 100ms } /** @@ -280,11 +274,10 @@ public void setRPM(double rpm, double feedforwardVolts) { */ @Override public void setVoltage(double voltage) { - setFollowMode(); // TODO: is doing this every time too slow? if (voltageComp > 0) { - leadMotor.setPower(voltage / Math.min(RobotController.getBatteryVoltage(), voltageComp)); + for (var m : motors) m.setPower(voltage / Math.min(RobotController.getBatteryVoltage(), voltageComp)); } else { - leadMotor.setVoltage(voltage); // CTRE internal code just divides by RobotController.getBatteryVoltage + for (var m : motors) m.setVoltage(voltage); // CTRE internal code just divides by RobotController.getBatteryVoltage } } /** From a39b007eeecae94e43c91db27f361e06b457da39 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 16 Mar 2023 00:50:51 -0700 Subject: [PATCH 095/134] correct docstring wording --- subsystems/motor/TelescopingArmPivotFeedForward.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/subsystems/motor/TelescopingArmPivotFeedForward.java b/subsystems/motor/TelescopingArmPivotFeedForward.java index b688834a..23149b3b 100644 --- a/subsystems/motor/TelescopingArmPivotFeedForward.java +++ b/subsystems/motor/TelescopingArmPivotFeedForward.java @@ -51,7 +51,7 @@ private double lerpedCg(double param) { } /** * @param armExtensionRatio The extension of the arm, relative to it's full extension. Should be between 0 and 1. Used as cg linear interpolation parameter. - * @param posRads Angle setpoint, in radians, measured from horizontal (0 = parallel with floor). + * @param posRads Current angle, in radians, measured from horizontal (0 = parallel with floor). * @param velRadPerSec Velocity setpoint. * @param accelRadPerSecSquared Acceleration setpoint. * @return The computed feedforward. From bf797fe2f194483fc9f22e7ab588d776917108ae Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 16 Mar 2023 00:53:09 -0700 Subject: [PATCH 096/134] remove defaultCommand on SmartMotorSubsystem; westCoast integrate feedforward --- subsystems/motor/SmartMotorSubsystem.java | 1 - 1 file changed, 1 deletion(-) diff --git a/subsystems/motor/SmartMotorSubsystem.java b/subsystems/motor/SmartMotorSubsystem.java index d1fbcbd3..44c2f235 100644 --- a/subsystems/motor/SmartMotorSubsystem.java +++ b/subsystems/motor/SmartMotorSubsystem.java @@ -42,7 +42,6 @@ public SmartMotorSubsystem(String name, SpeedModifier speedModifier, MotorContro for (var motor : motors) { motor.set(0); } - // setDefaultCommand(this.runOnce(() -> this.neutralOutput())); } /** From dd158a9af83ca898b3029a98d903d708de1733ac Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 16 Mar 2023 00:53:14 -0700 Subject: [PATCH 097/134] \/ --- subsystems/chassis/WestCoastDrive.java | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 019faec3..bb9860dc 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -4,6 +4,7 @@ import java.util.Map; import java.util.function.Supplier; +import org.usfirst.frc4904.robot.RobotMap; import org.usfirst.frc4904.standard.subsystems.motor.TalonMotorSubsystem; import com.kauailabs.navx.frc.AHRS; @@ -34,11 +35,17 @@ public class WestCoastDrive extends SubsystemBase { public final TalonMotorSubsystem rightMotors; protected final PIDConstants pidConsts; protected final DifferentialDriveKinematics kinematics; - protected final DifferentialDriveOdometry odometry; // OPTIM this can be replaced with a kalman filter? + public final DifferentialDriveOdometry odometry; // OPTIM this can be replaced with a kalman filter? protected final AHRS gyro; // OPTIM this can be replaced by something more general protected final double mps_to_rpm; protected final double m_to_motorrots; + private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward( + RobotMap.PID.Drive.kS, + RobotMap.PID.Drive.kV, + RobotMap.PID.Drive.kA + ); + /** * Represents a west coast drive chassis as a subsystem * @@ -110,6 +117,13 @@ public void movePolar(double speed, double heading, double turnSpeed) { setChassisVelocity(new ChassisSpeeds(speed, 0, turnSpeed)); } + public void testFeedForward(double speed) { + var ff = this.feedforward.calculate(speed); + // this.setWheelVoltages(new DifferentialDriveWheelVoltages(ff, ff)); + this.leftMotors.setVoltage(ff); + this.rightMotors.setVoltage(ff); + } + public void setWheelVelocities(DifferentialDriveWheelSpeeds wheelSpeeds) { this.leftMotors .setRPM(wheelSpeeds.leftMetersPerSecond * mps_to_rpm); this.rightMotors.setRPM(wheelSpeeds.rightMetersPerSecond * mps_to_rpm); From 0410b19b9b5f9791a6229111541eb6c63c04b306 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 16 Mar 2023 01:27:17 -0700 Subject: [PATCH 098/134] triage todos into TODO (comp) and TO DO (later) --- LogKitten.java | 4 +- commands/solenoid/SolenoidExtend.java | 2 +- commands/solenoid/SolenoidOff.java | 2 +- commands/solenoid/SolenoidRetract.java | 2 +- commands/solenoid/SolenoidSet.java | 2 +- custom/controllers/CustomCommandJoystick.java | 2 +- custom/sensors/CANEncoder.java | 2 +- .../CustomCommandDigitalLimitSwitch.java | 2 +- custom/sensors/EncoderPair.java | 4 +- subsystems/chassis/SensorDrive.java | 110 ------------------ subsystems/chassis/WestCoastDrive.java | 36 +----- subsystems/motor/SensorMotor.java | 2 +- subsystems/motor/SmartMotorSubsystem.java | 4 +- subsystems/motor/SparkMaxMotorSubsystem.java | 22 ++-- subsystems/motor/TalonMotorSubsystem.java | 20 ++-- 15 files changed, 37 insertions(+), 179 deletions(-) delete mode 100644 subsystems/chassis/SensorDrive.java diff --git a/LogKitten.java b/LogKitten.java index b9e08734..6045d09f 100644 --- a/LogKitten.java +++ b/LogKitten.java @@ -9,7 +9,7 @@ // import java.nio.file.Files; // import java.text.SimpleDateFormat; // import java.util.Date; -// import edu.wpi.first.hal.HAL; // DONT REMOVE, see TODO below +// import edu.wpi.first.hal.HAL; // DONT REMOVE, // public class LogKitten { // private static BufferedOutputStream fileOutput; @@ -145,7 +145,7 @@ // */ // private static void reportErrorToDriverStation(String details, String errorMessage, KittenLevel logLevel) { // // HAL.sendError(true, logLevel.getSeverity(), false, errorMessage, details, "", false); -// // TODO: this is now broken. commenting out causes silent failure. how do you fix it +// // TO DO: this is now broken. commenting out causes silent failure. how do you fix it // } // public static synchronized void logMessage(Object message, KittenLevel level, boolean override) { diff --git a/commands/solenoid/SolenoidExtend.java b/commands/solenoid/SolenoidExtend.java index d41c7da8..4dd6d57b 100644 --- a/commands/solenoid/SolenoidExtend.java +++ b/commands/solenoid/SolenoidExtend.java @@ -8,7 +8,7 @@ /** * Command to set the state of a SolenoidSubsystem to * EXTEND(DoubleSolenoid.Value.kFORWARD) - * TODO: rewrite using inline commands on SolenoidSubsystem + * TO DO: rewrite using inline commands on SolenoidSubsystem */ @Deprecated public class SolenoidExtend extends SolenoidSet { diff --git a/commands/solenoid/SolenoidOff.java b/commands/solenoid/SolenoidOff.java index 09ad5a7f..16f402d0 100644 --- a/commands/solenoid/SolenoidOff.java +++ b/commands/solenoid/SolenoidOff.java @@ -8,7 +8,7 @@ /** * Command to set the state of a SolenoidSubsystem to * OFF(DoubleSolenoid.Value.kOff) - * TODO: rewrite using inline commands on SolenoidSubsystem + * TO DO: rewrite using inline commands on SolenoidSubsystem */ @Deprecated public class SolenoidOff extends SolenoidSet { diff --git a/commands/solenoid/SolenoidRetract.java b/commands/solenoid/SolenoidRetract.java index 11eb6817..3e2a7eba 100644 --- a/commands/solenoid/SolenoidRetract.java +++ b/commands/solenoid/SolenoidRetract.java @@ -8,7 +8,7 @@ /** * Command to set the state of a SolenoidSubsystem to * RETRACT(DoubleSolenoid.Value.kReverse) - * TODO: rewrite using inline commands on SolenoidSubsystem + * TO DO: rewrite using inline commands on SolenoidSubsystem */ @Deprecated public class SolenoidRetract extends SolenoidSet { diff --git a/commands/solenoid/SolenoidSet.java b/commands/solenoid/SolenoidSet.java index 41054395..551db099 100644 --- a/commands/solenoid/SolenoidSet.java +++ b/commands/solenoid/SolenoidSet.java @@ -8,7 +8,7 @@ /** * Command to set the state of a SolenoidSubsystem - * TODO: rewrite using inline commands on SolenoidSubsystem + * TO DO: rewrite using inline commands on SolenoidSubsystem */ public class SolenoidSet extends CommandBase { diff --git a/custom/controllers/CustomCommandJoystick.java b/custom/controllers/CustomCommandJoystick.java index bdbed8c3..de291226 100644 --- a/custom/controllers/CustomCommandJoystick.java +++ b/custom/controllers/CustomCommandJoystick.java @@ -9,7 +9,7 @@ * trigger interface. This allows us to use a joystick as a controller. This * contains 12 buttons to reflect the joysticks we are typically using. * - * TODO: should probably extend or be replaced with https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/wpilibj2/command/button/CommandJoystick.html + * TO DO: should probably extend or be replaced with https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/wpilibj2/command/button/CommandJoystick.html */ public class CustomCommandJoystick extends Joystick { public static final int X_AXIS = 0; diff --git a/custom/sensors/CANEncoder.java b/custom/sensors/CANEncoder.java index 7f997f06..b26f1a6c 100644 --- a/custom/sensors/CANEncoder.java +++ b/custom/sensors/CANEncoder.java @@ -1,6 +1,6 @@ package org.usfirst.frc4904.standard.custom.sensors; // WAS PID SOURCE -// TODO FIND BETTER LOG KITTEN REPLACEMENT +// TO DO FIND BETTER LOG KITTEN REPLACEMENT // import org.usfirst.frc4904.standard.LogKitten; import org.usfirst.frc4904.standard.Util; diff --git a/custom/sensors/CustomCommandDigitalLimitSwitch.java b/custom/sensors/CustomCommandDigitalLimitSwitch.java index 479aecd8..4679bb36 100644 --- a/custom/sensors/CustomCommandDigitalLimitSwitch.java +++ b/custom/sensors/CustomCommandDigitalLimitSwitch.java @@ -3,7 +3,7 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.button.Trigger; -// TODO untested +// TO DO untested /** * Digital limit switch that provides the same Trigger interface as HID diff --git a/custom/sensors/EncoderPair.java b/custom/sensors/EncoderPair.java index 83d25111..da9a7db2 100644 --- a/custom/sensors/EncoderPair.java +++ b/custom/sensors/EncoderPair.java @@ -160,7 +160,7 @@ public double getDifference() { } catch (InvalidSensorException e) { e.printStackTrace(); // LogKitten.ex(e); - return 0.0; // TODO: is this a reasonable default + return 0.0; // TO DO: is this a reasonable default } } @@ -175,7 +175,7 @@ public double getRateDifference() { } catch (InvalidSensorException e) { e.printStackTrace(); // LogKitten.ex(e); - return 0.0; // TODO: is this a reasonable default + return 0.0; // TO DO: is this a reasonable default } } diff --git a/subsystems/chassis/SensorDrive.java b/subsystems/chassis/SensorDrive.java deleted file mode 100644 index 42f92c52..00000000 --- a/subsystems/chassis/SensorDrive.java +++ /dev/null @@ -1,110 +0,0 @@ -// /*----------------------------------------------------------------------------*/ -// /* Copyright (c) 2019 FIRST. All Rights Reserved. */ -// /* Open Source Software - may be modified and shared by FRC teams. The code */ -// /* must be accompanied by the FIRST BSD license file in the root directory of */ -// /* the project. */ -// /*----------------------------------------------------------------------------*/ - -// package org.usfirst.frc4904.standard.subsystems.chassis; -// // WAS PID SOURCE -// import org.usfirst.frc4904.standard.subsystems.motor.MotorSubsystem; -// import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder; -// import org.usfirst.frc4904.standard.custom.sensors.NavX; - -// import edu.wpi.first.math.geometry.Pose2d; -// import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; -// import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; -// import edu.wpi.first.wpilibj2.command.CommandScheduler; -// import edu.wpi.first.wpilibj2.command.Subsystem; - -// public class SensorDrive implements Subsystem { // Based largely on -// // https://github.com/wpilibsuite/allwpilib/blob/master/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java -// private final TankDrive driveBase; -// private final CANTalonEncoder leftEncoder; -// private final CANTalonEncoder rightEncoder; -// private final NavX gyro; -// private final DifferentialDriveOdometry odometry; - -// /** -// * Creates a new DriveSubsystem. -// */ -// public SensorDrive(TankDrive driveBase, CANTalonEncoder leftEncoder, CANTalonEncoder rightEncoder, NavX gyro, Pose2d initialPose) { -// this.driveBase = driveBase; -// this.leftEncoder = leftEncoder; -// this.rightEncoder = rightEncoder; -// this.gyro = gyro; - -// resetEncoders(); -// odometry = new DifferentialDriveOdometry(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), initialPose); -// CommandScheduler.getInstance().registerSubsystem(this); -// } - -// @Override -// public void periodic() { -// odometry.update(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance()); -// } - -// /** -// * Returns the currently-estimated pose of the robot. -// * -// * @return The pose. -// */ -// public Pose2d getPose() { -// return odometry.getPoseMeters(); -// } - -// /** -// * Returns the current wheel speeds of the robot. -// * -// * @return The current wheel speeds. -// */ -// public DifferentialDriveWheelSpeeds getWheelSpeeds() { -// return new DifferentialDriveWheelSpeeds(leftEncoder.getRate(), rightEncoder.getRate()); -// } - -// /** -// * Resets the odometry to the specified pose. -// * -// * @param pose The pose to which to set the odometry. -// */ -// public void resetOdometry(Pose2d pose) { -// resetEncoders(); -// odometry.resetPosition(gyro.getRotation2d(), leftEncoder.getDistance(), rightEncoder.getDistance(), pose); -// } - -// // /** -// // * Controls the left and right sides of the drive directly with voltages. -// // * -// // * @param leftVolts the commanded left output -// // * @param rightVolts the commanded right output -// // */ -// // public void tankDriveVolts(double leftVolts, double rightVolts) { -// // MotorSubsystem[] motors = driveBase.getMotors(); -// // if (motors.length == 2) { -// // driveBase.getMotors()[0].setVoltage(leftVolts); -// // driveBase.getMotors()[1].setVoltage(rightVolts); -// // } else { -// // driveBase.getMotors()[0].setVoltage(leftVolts); -// // driveBase.getMotors()[1].setVoltage(leftVolts); -// // driveBase.getMotors()[2].setVoltage(rightVolts); -// // driveBase.getMotors()[3].setVoltage(rightVolts); -// // } -// // } - -// /** -// * Resets the drive encoders to currently read a position of 0. -// */ -// public void resetEncoders() { -// leftEncoder.reset(); -// rightEncoder.reset(); -// } - -// /** -// * Returns the heading of the robot. -// * -// * @return the robot's heading in degrees, from 180 to 180 -// */ -// public double getHeading() { -// return gyro.getYaw() * -1; -// } -// } diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index bb9860dc..6e2c60b1 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -120,6 +120,7 @@ public void movePolar(double speed, double heading, double turnSpeed) { public void testFeedForward(double speed) { var ff = this.feedforward.calculate(speed); // this.setWheelVoltages(new DifferentialDriveWheelVoltages(ff, ff)); + // TODO COMP: why does this do one side forward, one side backwards? should be inverted correctly?? this.leftMotors.setVoltage(ff); this.rightMotors.setVoltage(ff); } @@ -133,13 +134,6 @@ public void setChassisVelocity(ChassisSpeeds chassisSpeeds) { setWheelVelocities(wheelSpeeds); } - public void setChassisVoltage(ChassisSpeeds sketchyVoltages) { - final double SKETCHY_CORRECTION = 1; // TODO: tune? @zbuster05 - final var wheelVoltages = kinematics.toWheelSpeeds(sketchyVoltages); - // we do a little trolling - setWheelVoltages(wheelVoltages.leftMetersPerSecond * SKETCHY_CORRECTION, wheelVoltages.rightMetersPerSecond * SKETCHY_CORRECTION); - } - public void setWheelVoltages(DifferentialDriveWheelVoltages wheelVoltages) { this.setWheelVoltages(wheelVoltages.left, wheelVoltages.right); } @@ -150,11 +144,6 @@ public void setWheelVoltages(double leftV, double rightV) { /// command factories - @Deprecated - public Command c_simpleWPILibSpline(Trajectory trajectory) { - // TODO: doesn't work. see implementation in Chassis2023 - throw new UnsupportedOperationException("need to implement ramsete controller"); - } /** * Load a PathPlanner .path file, generate it with maxVel and maxAccl, and @@ -237,16 +226,6 @@ public Command c_controlWheelVelocities(Supplier l cmd.addRequirements(rightMotors); return cmd; } - // TODO sketchy as hell - public Command c_controlChassisWithVoltage(Supplier chassisSpeedVoltsSupplier) { - var cmd = this.run(() -> { - var volts = chassisSpeedVoltsSupplier.get(); - setChassisVoltage(volts); - }); // this.run() runs repeatedly - cmd.addRequirements(leftMotors); - cmd.addRequirements(rightMotors); - return cmd; - } /** * A forever command that pulls chassis movement @@ -259,17 +238,6 @@ public Command c_controlChassisVelocity(Supplier chassisSpeedsSup cmd.addRequirements(rightMotors); return cmd; } - /** - * A forever command that pulls left and right wheel voltages from a - * function. - */ - public Command c_controlChassisSpeedAndTurn(Supplier> chasssisSpeedAndTurnSupplier) { - var cmd = c_controlChassisWithVoltage(() -> { - Pair speedAndTurn = chasssisSpeedAndTurnSupplier.get(); - return new ChassisSpeeds(speedAndTurn.getFirst()*RobotController.getBatteryVoltage(), 0, speedAndTurn.getSecond()); - }); - return cmd; - } /** * A forever command that pulls left and right wheel voltages from a * function. @@ -298,7 +266,7 @@ public Command c_chassisConstant(double x, double y, double turn, double timeout * Replaces ChassisIdle in pre-2023 standard. */ public Command c_idle() { - return Commands.parallel(leftMotors.c_idle(), rightMotors.c_idle()); // TODO do we need to require the chassis subsystem for this? else chassis will read as unrequired, but all of it's subcomponents will be required. + return Commands.parallel(leftMotors.c_idle(), rightMotors.c_idle()); } /** diff --git a/subsystems/motor/SensorMotor.java b/subsystems/motor/SensorMotor.java index 5032fb5d..2d32d2b4 100644 --- a/subsystems/motor/SensorMotor.java +++ b/subsystems/motor/SensorMotor.java @@ -72,7 +72,7 @@ public void setPositionSafely(double position) throws InvalidSensorException { minecraft.setSetpoint(position); double speed = minecraft.getSafely(); // LogKitten.v(getName() + " set to position " + position + " at speed " + speed); - super.set(speed); // TODO remove and test I don't know why this is here. + super.set(speed); // TO DO remove and test I don't know why this is here. } /** diff --git a/subsystems/motor/SmartMotorSubsystem.java b/subsystems/motor/SmartMotorSubsystem.java index 44c2f235..fe800aba 100644 --- a/subsystems/motor/SmartMotorSubsystem.java +++ b/subsystems/motor/SmartMotorSubsystem.java @@ -176,7 +176,7 @@ public final void coast() { } /// COMMANDS - // TODO: remove the "Replaces *.java in pre-2023 standard" and "eg. button1.onTrue(motor.c_idle())" when they become unnecessary. + // TO DO: remove the "Replaces *.java in pre-2023 standard" and "eg. button1.onTrue(motor.c_idle())" when they become unnecessary. /** * Disable motor using the underlying .disable(); command version of .disable(). @@ -248,7 +248,7 @@ public final void coast() { /** * Write PIDF values to hardware, which will be used with the closed loop control commands * - * TODO: replace with ezControl + * TO DO: replace with ezControl */ public abstract void setRPM(double voltage); public abstract void setRPM(double voltage, double feedforwardVolts); diff --git a/subsystems/motor/SparkMaxMotorSubsystem.java b/subsystems/motor/SparkMaxMotorSubsystem.java index 3f7dc963..a9af1b57 100644 --- a/subsystems/motor/SparkMaxMotorSubsystem.java +++ b/subsystems/motor/SparkMaxMotorSubsystem.java @@ -50,10 +50,10 @@ public class SparkMaxMotorSubsystem extends SmartMotorSubsystem setRPM(setpoint)); } - // TODO: these should probably use a diff pid slot from RPM + // TO DO: these should probably use a diff pid slot from RPM @Override public Command c_setPosition(double setpoint) { if (!pid_configured) throw new IllegalArgumentException(name + " tried to use c_setPosition without first configPIDF()-ing"); @@ -335,7 +335,7 @@ public void setVoltage(double voltage) { // no need to override setPower because the base class just uses set // don't override setBrakeOnNeutral, setCoastOnNeutral, neutralOutput because we indeed want to set it individually on each motor. Otherwise, the followers might try to follow a disabled/neutral motor which might cause unexpected behavior. - // TODO: except do we actually yes need to? since it seems to not persist, see the warning about auto-disable here: https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces + // TO DO: except do we actually yes need to? since it seems to not persist, see the warning about auto-disable here: https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces // ERROR HANDLING static class REVLibThrowable extends IllegalStateException { diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index 5ec7de2e..d976e69c 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -44,17 +44,17 @@ public class TalonMotorSubsystem extends SmartMotorSubsystem Date: Thu, 16 Mar 2023 09:53:21 -0700 Subject: [PATCH 099/134] add zach's goofy driver code back Co-authored-by: zbuster05 --- subsystems/chassis/WestCoastDrive.java | 37 ++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 6e2c60b1..10286f4c 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -143,6 +143,43 @@ public void setWheelVoltages(double leftV, double rightV) { } + + + + + public void setChassisVoltage(ChassisSpeeds sketchyVoltages) { + final double SKETCHY_CORRECTION = 1; // TODO: tune? @zbuster05 + final var wheelVoltages = kinematics.toWheelSpeeds(sketchyVoltages); + // we do a little trolling + setWheelVoltages(wheelVoltages.leftMetersPerSecond * SKETCHY_CORRECTION, wheelVoltages.rightMetersPerSecond * SKETCHY_CORRECTION); + } + public Command c_controlChassisWithVoltage(Supplier chassisSpeedVoltsSupplier) { + var cmd = this.run(() -> { + var volts = chassisSpeedVoltsSupplier.get(); + setChassisVoltage(volts); + }); // this.run() runs repeatedly + cmd.addRequirements(leftMotors); + cmd.addRequirements(rightMotors); + return cmd; + } + /** + * A forever command that pulls left and right wheel voltages from a + * function. + */ + public Command c_controlChassisSpeedAndTurn(Supplier> chasssisSpeedAndTurnSupplier) { + var cmd = c_controlChassisWithVoltage(() -> { + Pair speedAndTurn = chasssisSpeedAndTurnSupplier.get(); + return new ChassisSpeeds(speedAndTurn.getFirst()*RobotController.getBatteryVoltage(), 0, speedAndTurn.getSecond()); + }); + return cmd; + } + + + + + + + /// command factories /** From 7e8bb0703c6ecc9f0e28248ebdf0620d5fd5d4f6 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Thu, 16 Mar 2023 17:11:19 -0700 Subject: [PATCH 100/134] ezcontrol logging --- custom/motioncontrollers/ezControl.java | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/custom/motioncontrollers/ezControl.java b/custom/motioncontrollers/ezControl.java index 6430ba43..8003a8bb 100644 --- a/custom/motioncontrollers/ezControl.java +++ b/custom/motioncontrollers/ezControl.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class ezControl implements BiFunction { private final ezControlMethod controller; @@ -26,12 +27,12 @@ public void setSetpoint(double setpoint) { } public void updateSetpoint(double setpoint, double setpoint_dt) { - this.setpoint = setpoint; - this.setpoint_dt = setpoint_dt; if (setpoint != this.setpoint) { // slightly expensive call that clears integral this.controller.pid.setSetpoint(this.setpoint); } + this.setpoint = setpoint; + this.setpoint_dt = setpoint_dt; } public void setIntegratorRange(double kIMin, double kIMax) { @@ -39,7 +40,15 @@ public void setIntegratorRange(double kIMin, double kIMax) { } public double calculate(double measurement, double elapsed) { - return this.controller.pid.calculate(measurement) + this.controller.F.calculate(this.setpoint, this.setpoint_dt); + // FIXME, revert logging + SmartDashboard.putNumber("setpoint", this.setpoint); + SmartDashboard.putNumber("setpoint_dt", this.setpoint_dt); + + double pidout = this.controller.pid.calculate(measurement); + System.out.println(pidout); + SmartDashboard.putNumber("Feedback", measurement); + SmartDashboard.putNumber("PID out", pidout); + return pidout + this.controller.F.calculate(this.setpoint, this.setpoint_dt); } // very similar to TrapezoidProfile.State From 5119f82c0fed742916edf85cb50b0324364d5e7b Mon Sep 17 00:00:00 2001 From: jac0rr Date: Thu, 16 Mar 2023 18:48:57 -0700 Subject: [PATCH 101/134] added custom xbox --- custom/controllers/CustomCommandXbox.java | 59 +++++++++++++++++++++++ 1 file changed, 59 insertions(+) create mode 100644 custom/controllers/CustomCommandXbox.java diff --git a/custom/controllers/CustomCommandXbox.java b/custom/controllers/CustomCommandXbox.java new file mode 100644 index 00000000..6a400bc3 --- /dev/null +++ b/custom/controllers/CustomCommandXbox.java @@ -0,0 +1,59 @@ +package org.usfirst.frc4904.standard.custom.controllers; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; + +public class CustomCommandXbox extends CommandXboxController{ + private final double deadZoneSize; + private final XboxController m_hid; + public CustomCommandXbox(int port, double deadZoneSize){ + super(port); + m_hid = new XboxController(port); + this.deadZoneSize = deadZoneSize; + } + + public double getLeftX() { + return applyDeadzone(m_hid.getLeftX(),deadZoneSize); + } + + /** + * Get the X axis value of right side of the controller. + * + * @return The axis value. + */ + public double getRightX() { + return applyDeadzone(m_hid.getRightX(),deadZoneSize); + } + + /** + * Get the Y axis value of left side of the controller. + * + * @return The axis value. + */ + public double getLeftY() { + return applyDeadzone(m_hid.getLeftY(),deadZoneSize); + } + + /** + * Get the Y axis value of right side of the controller. + * + * @return The axis value. + */ + public double getRightY() { + return applyDeadzone(m_hid.getRightY(),deadZoneSize); + } + + public double applyDeadzone(double input, double deadZoneSize){ + final double negative; + double deadZoneSizeClamp = deadZoneSize; + double adjusted; + if (deadZoneSizeClamp < 0 || deadZoneSizeClamp >= 1) { + deadZoneSizeClamp = 0; // Prevent any weird errors + } + negative = input < 0 ? -1 : 1; + adjusted = Math.abs(input) - deadZoneSizeClamp; // Subtract the deadzone from the magnitude + adjusted = adjusted < 0 ? 0 : adjusted; // if the new input is negative, make it zero + adjusted = adjusted / (1 - deadZoneSizeClamp); // Adjust the adjustment so it can max at 1 + return negative * adjusted; + } +} From 3109be53ae6f53c516a70485d694e8ffe00070db Mon Sep 17 00:00:00 2001 From: Exr0n Date: Thu, 16 Mar 2023 18:54:17 -0700 Subject: [PATCH 102/134] further remove follow mode (read remote encoders) --- subsystems/motor/TalonMotorSubsystem.java | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index d976e69c..026ff253 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -3,6 +3,7 @@ import java.util.function.DoubleSupplier; import java.util.stream.Stream; +import org.usfirst.frc4904.standard.custom.motorcontrollers.CANTalonFX; import org.usfirst.frc4904.standard.custom.motorcontrollers.TalonMotorController; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier; import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier; @@ -104,7 +105,7 @@ public class TalonMotorSubsystem extends SmartMotorSubsystem Date: Thu, 16 Mar 2023 19:40:34 -0700 Subject: [PATCH 103/134] applied config software limits to every motor --- subsystems/motor/TalonMotorSubsystem.java | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index 026ff253..5e395c57 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -367,17 +367,25 @@ public double getSensorVelocityRPM() { return this.leadMotor.getSelectedSensorVelocity(DEFAULT_DMP_SLOT) / ENCODER_COUNTS_PER_REV * 10 * 60; } @Override - public void configSoftwareLimits(double fwdBoundRotations, double revBoundRotations) { + public void configSoftwareLimits(double fwdBoundRotations, double revBoundRotations) { + //set each software limits for each follow motor + leadMotor.configForwardSoftLimitThreshold((fwdBoundRotations*ENCODER_COUNTS_PER_REV), configTimeoutMs); + leadMotor.configReverseSoftLimitThreshold((revBoundRotations*ENCODER_COUNTS_PER_REV), configTimeoutMs); + leadMotor.configForwardSoftLimitEnable(true, configTimeoutMs); + leadMotor.configReverseSoftLimitEnable(true, configTimeoutMs); + leadMotor.overrideSoftLimitsEnable(true); + for (var motor : followMotors) { + motor.configForwardSoftLimitThreshold((fwdBoundRotations*ENCODER_COUNTS_PER_REV), configTimeoutMs); + motor.configReverseSoftLimitThreshold((revBoundRotations*ENCODER_COUNTS_PER_REV), configTimeoutMs); + motor.configForwardSoftLimitEnable(true, configTimeoutMs); + motor.configReverseSoftLimitEnable(true, configTimeoutMs); + motor.overrideSoftLimitsEnable(true); // this.leadMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, DEFAULT_PID_SLOT, configTimeoutMs); // select which sensor to use for soft limits // this.leadMotor.setSensorPhase(true); - this.leadMotor.configForwardSoftLimitThreshold((fwdBoundRotations*ENCODER_COUNTS_PER_REV), configTimeoutMs); - this.leadMotor.configReverseSoftLimitThreshold((revBoundRotations*ENCODER_COUNTS_PER_REV), configTimeoutMs); - this.leadMotor.configForwardSoftLimitEnable(true, configTimeoutMs); - this.leadMotor.configReverseSoftLimitEnable(true, configTimeoutMs); - this.leadMotor.overrideSoftLimitsEnable(true); - } + } // no need to override setPower because the base class just uses set // don't override setBrakeOnNeutral, setCoastOnNeutral, neutralOutput because we indeed want to set it individually on each motor. Otherwise, the followers might try to follow a disabled/neutral motor which might cause unexpected behavior. + } } From 0db177487cb1ff74bad37b2d3b22977c2d17ed30 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Thu, 16 Mar 2023 19:45:41 -0700 Subject: [PATCH 104/134] run command scheduler in test --- CommandRobotBase.java | 1 + 1 file changed, 1 insertion(+) diff --git a/CommandRobotBase.java b/CommandRobotBase.java index be97f539..6b7ba440 100644 --- a/CommandRobotBase.java +++ b/CommandRobotBase.java @@ -221,6 +221,7 @@ public final void testInit() { */ @Override public void testPeriodic() { + CommandScheduler.getInstance().run(); testExecute(); alwaysExecute(); } From 7f9cebc7dd6bfbd5ce60f8ff9dbb71430d329da1 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Thu, 16 Mar 2023 23:25:26 -0700 Subject: [PATCH 105/134] joystick deadzone: linear interpolate --- custom/controllers/CustomCommandJoystick.java | 23 +++++++++++-------- custom/motioncontrollers/ezControl.java | 10 ++++---- .../speedmodifiers/EnableableModifier.java | 1 + 3 files changed, 19 insertions(+), 15 deletions(-) diff --git a/custom/controllers/CustomCommandJoystick.java b/custom/controllers/CustomCommandJoystick.java index de291226..fb4d96c5 100644 --- a/custom/controllers/CustomCommandJoystick.java +++ b/custom/controllers/CustomCommandJoystick.java @@ -53,16 +53,17 @@ public CustomCommandJoystick(int port) { * Returns true if a given axis is above the move threshold. * * @param axis - * @return + * @return whether the current value of that axis is outside of the deadzone */ public boolean active(int axis) { - if (axis == CustomCommandJoystick.X_AXIS) { - return Math.abs(super.getX()) > deadzone; - } else if (axis == CustomCommandJoystick.Y_AXIS) { - return Math.abs(super.getY()) > deadzone; - } else { - return false; - } + return Math.abs(getAxis(axis)) > deadzone; + // if (axis == CustomCommandJoystick.X_AXIS) { + // return Math.abs(super.getX()) > deadzone; + // } else if (axis == CustomCommandJoystick.Y_AXIS) { + // return Math.abs(super.getY()) > deadzone; + // } else { + // return false; + // } } /** @@ -79,10 +80,12 @@ public boolean connected() { * Returns the value of the given axis. */ public double getAxis(int axis) { - if (Math.abs(super.getRawAxis(axis)) < deadzone) { + double val = super.getRawAxis(axis); + if (Math.abs(val) < deadzone) { return 0.0; } - return super.getRawAxis(axis); + return (val - Math.signum(val)*deadzone)/(1-deadzone); // linear between 0 and 1 in the remaining range + // return super.getRawAxis(axis); } public void setDeadzone(double deadzone) { diff --git a/custom/motioncontrollers/ezControl.java b/custom/motioncontrollers/ezControl.java index 8003a8bb..20a97195 100644 --- a/custom/motioncontrollers/ezControl.java +++ b/custom/motioncontrollers/ezControl.java @@ -41,13 +41,13 @@ public void setIntegratorRange(double kIMin, double kIMax) { public double calculate(double measurement, double elapsed) { // FIXME, revert logging - SmartDashboard.putNumber("setpoint", this.setpoint); - SmartDashboard.putNumber("setpoint_dt", this.setpoint_dt); + // SmartDashboard.putNumber("setpoint", this.setpoint); + // SmartDashboard.putNumber("setpoint_dt", this.setpoint_dt); double pidout = this.controller.pid.calculate(measurement); - System.out.println(pidout); - SmartDashboard.putNumber("Feedback", measurement); - SmartDashboard.putNumber("PID out", pidout); + // System.out.println(pidout); + // SmartDashboard.putNumber("Feedback", measurement); + // SmartDashboard.putNumber("PID out", pidout); return pidout + this.controller.F.calculate(this.setpoint, this.setpoint_dt); } diff --git a/subsystems/motor/speedmodifiers/EnableableModifier.java b/subsystems/motor/speedmodifiers/EnableableModifier.java index 3c344981..813a6f8b 100644 --- a/subsystems/motor/speedmodifiers/EnableableModifier.java +++ b/subsystems/motor/speedmodifiers/EnableableModifier.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; +@Deprecated public class EnableableModifier extends SubsystemBase implements SpeedModifier { protected boolean enabled; protected final SpeedModifier modifier; From 3f912561c4a2b4e68ee6c301a3d093cd775cf6d7 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Fri, 17 Mar 2023 01:09:16 -0700 Subject: [PATCH 106/134] better controller deadzone implementations --- custom/controllers/CustomCommandJoystick.java | 12 +++---- custom/controllers/CustomCommandXbox.java | 31 +++++++++++-------- 2 files changed, 24 insertions(+), 19 deletions(-) diff --git a/custom/controllers/CustomCommandJoystick.java b/custom/controllers/CustomCommandJoystick.java index fb4d96c5..1a8967a8 100644 --- a/custom/controllers/CustomCommandJoystick.java +++ b/custom/controllers/CustomCommandJoystick.java @@ -15,7 +15,7 @@ public class CustomCommandJoystick extends Joystick { public static final int X_AXIS = 0; public static final int Y_AXIS = 1; public static final int SLIDER_AXIS = 3; - protected double deadzone; + protected final double deadzone; protected final int port; // Buttons public final JoystickButton button1; @@ -31,8 +31,12 @@ public class CustomCommandJoystick extends Joystick { public final JoystickButton button11; public final JoystickButton button12; - public CustomCommandJoystick(int port) { + public CustomCommandJoystick(int port, double deadzone) { super(port); + if (deadzone < 0 || deadzone > 1) { + throw new IllegalArgumentException("Joystick deadzone must be in [0, 1]"); + } + this.deadzone = deadzone; this.port = port; deadzone = 0; button1 = new JoystickButton(this, 1); @@ -87,8 +91,4 @@ public double getAxis(int axis) { return (val - Math.signum(val)*deadzone)/(1-deadzone); // linear between 0 and 1 in the remaining range // return super.getRawAxis(axis); } - - public void setDeadzone(double deadzone) { - this.deadzone = deadzone; - } } diff --git a/custom/controllers/CustomCommandXbox.java b/custom/controllers/CustomCommandXbox.java index 6a400bc3..48d3bb96 100644 --- a/custom/controllers/CustomCommandXbox.java +++ b/custom/controllers/CustomCommandXbox.java @@ -3,12 +3,15 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -public class CustomCommandXbox extends CommandXboxController{ +public class CustomCommandXbox extends CommandXboxController { private final double deadZoneSize; private final XboxController m_hid; public CustomCommandXbox(int port, double deadZoneSize){ super(port); m_hid = new XboxController(port); + if (deadZoneSize < 0 || deadZoneSize > 1) { + throw new IllegalArgumentException("CustomCommandXBox deadzone must be in [0, 1]"); + } this.deadZoneSize = deadZoneSize; } @@ -42,18 +45,20 @@ public double getLeftY() { public double getRightY() { return applyDeadzone(m_hid.getRightY(),deadZoneSize); } + + @Override + public double getRightTriggerAxis() { + return applyDeadzone(m_hid.getRightTriggerAxis(), deadZoneSize); + } + @Override + public double getLeftTriggerAxis() { + return applyDeadzone(m_hid.getLeftTriggerAxis(), deadZoneSize); + } - public double applyDeadzone(double input, double deadZoneSize){ - final double negative; - double deadZoneSizeClamp = deadZoneSize; - double adjusted; - if (deadZoneSizeClamp < 0 || deadZoneSizeClamp >= 1) { - deadZoneSizeClamp = 0; // Prevent any weird errors - } - negative = input < 0 ? -1 : 1; - adjusted = Math.abs(input) - deadZoneSizeClamp; // Subtract the deadzone from the magnitude - adjusted = adjusted < 0 ? 0 : adjusted; // if the new input is negative, make it zero - adjusted = adjusted / (1 - deadZoneSizeClamp); // Adjust the adjustment so it can max at 1 - return negative * adjusted; + public static double applyDeadzone(double input, double deadZoneSize){ + if (Math.abs(input) < deadZoneSize) { // return 0 if within the deadzone + return 0.0; + } + return (input - Math.signum(input) * deadZoneSize) / (1 - input); // linear between 0 and 1 in the remaining range } } From 51b3e88ae4be66c9f46b89119325e47ad78578c8 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Fri, 17 Mar 2023 01:10:36 -0700 Subject: [PATCH 107/134] format customCommandXbox --- custom/controllers/CustomCommandXbox.java | 110 +++++++++++----------- 1 file changed, 56 insertions(+), 54 deletions(-) diff --git a/custom/controllers/CustomCommandXbox.java b/custom/controllers/CustomCommandXbox.java index 48d3bb96..52379644 100644 --- a/custom/controllers/CustomCommandXbox.java +++ b/custom/controllers/CustomCommandXbox.java @@ -4,61 +4,63 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class CustomCommandXbox extends CommandXboxController { - private final double deadZoneSize; - private final XboxController m_hid; - public CustomCommandXbox(int port, double deadZoneSize){ - super(port); - m_hid = new XboxController(port); - if (deadZoneSize < 0 || deadZoneSize > 1) { - throw new IllegalArgumentException("CustomCommandXBox deadzone must be in [0, 1]"); - } - this.deadZoneSize = deadZoneSize; + private final double deadZoneSize; + private final XboxController m_hid; + + public CustomCommandXbox(int port, double deadZoneSize) { + super(port); + m_hid = new XboxController(port); + if (deadZoneSize < 0 || deadZoneSize > 1) { + throw new IllegalArgumentException("CustomCommandXBox deadzone must be in [0, 1]"); } + this.deadZoneSize = deadZoneSize; + } + + public double getLeftX() { + return applyDeadzone(m_hid.getLeftX(), deadZoneSize); + } + + /** + * Get the X axis value of right side of the controller. + * + * @return The axis value. + */ + public double getRightX() { + return applyDeadzone(m_hid.getRightX(), deadZoneSize); + } + + /** + * Get the Y axis value of left side of the controller. + * + * @return The axis value. + */ + public double getLeftY() { + return applyDeadzone(m_hid.getLeftY(), deadZoneSize); + } - public double getLeftX() { - return applyDeadzone(m_hid.getLeftX(),deadZoneSize); - } - - /** - * Get the X axis value of right side of the controller. - * - * @return The axis value. - */ - public double getRightX() { - return applyDeadzone(m_hid.getRightX(),deadZoneSize); - } - - /** - * Get the Y axis value of left side of the controller. - * - * @return The axis value. - */ - public double getLeftY() { - return applyDeadzone(m_hid.getLeftY(),deadZoneSize); - } - - /** - * Get the Y axis value of right side of the controller. - * - * @return The axis value. - */ - public double getRightY() { - return applyDeadzone(m_hid.getRightY(),deadZoneSize); - } + /** + * Get the Y axis value of right side of the controller. + * + * @return The axis value. + */ + public double getRightY() { + return applyDeadzone(m_hid.getRightY(), deadZoneSize); + } - @Override - public double getRightTriggerAxis() { - return applyDeadzone(m_hid.getRightTriggerAxis(), deadZoneSize); - } - @Override - public double getLeftTriggerAxis() { - return applyDeadzone(m_hid.getLeftTriggerAxis(), deadZoneSize); - } - - public static double applyDeadzone(double input, double deadZoneSize){ - if (Math.abs(input) < deadZoneSize) { // return 0 if within the deadzone - return 0.0; - } - return (input - Math.signum(input) * deadZoneSize) / (1 - input); // linear between 0 and 1 in the remaining range - } + @Override + public double getRightTriggerAxis() { + return applyDeadzone(m_hid.getRightTriggerAxis(), deadZoneSize); + } + + @Override + public double getLeftTriggerAxis() { + return applyDeadzone(m_hid.getLeftTriggerAxis(), deadZoneSize); + } + + public static double applyDeadzone(double input, double deadZoneSize) { + if (Math.abs(input) < deadZoneSize) { // return 0 if within the deadzone + return 0.0; + } + return (input - Math.signum(input) * deadZoneSize) / (1 - input); // linear between 0 and 1 in the remaining range + } } From 8cf0f58268c4fedadc1f1301acfc6fc3af1d3901 Mon Sep 17 00:00:00 2001 From: jac0rr Date: Fri, 17 Mar 2023 10:59:23 -0700 Subject: [PATCH 108/134] clean up todos --- custom/motioncontrollers/ezMotion.java | 2 +- subsystems/chassis/WestCoastDrive.java | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/custom/motioncontrollers/ezMotion.java b/custom/motioncontrollers/ezMotion.java index 13d9d5a0..2308a35b 100644 --- a/custom/motioncontrollers/ezMotion.java +++ b/custom/motioncontrollers/ezMotion.java @@ -56,7 +56,7 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - // TODO Auto-generated method stub + // FIXME Auto-generated method stub super.end(interrupted); } } diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 10286f4c..5aa6bf71 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -117,10 +117,11 @@ public void movePolar(double speed, double heading, double turnSpeed) { setChassisVelocity(new ChassisSpeeds(speed, 0, turnSpeed)); } + @Deprecated public void testFeedForward(double speed) { var ff = this.feedforward.calculate(speed); // this.setWheelVoltages(new DifferentialDriveWheelVoltages(ff, ff)); - // TODO COMP: why does this do one side forward, one side backwards? should be inverted correctly?? + // COMP: why does this do one side forward, one side backwards? should be inverted correctly?? this.leftMotors.setVoltage(ff); this.rightMotors.setVoltage(ff); } From f19dc8e6b5dc4b763514a7cc0091a54ce19654be Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Fri, 17 Mar 2023 15:58:48 -0700 Subject: [PATCH 109/134] clamp the arm lerp to avoid accidentally using unreasonable values and blowing something up --- subsystems/motor/TelescopingArmPivotFeedForward.java | 1 + 1 file changed, 1 insertion(+) diff --git a/subsystems/motor/TelescopingArmPivotFeedForward.java b/subsystems/motor/TelescopingArmPivotFeedForward.java index 23149b3b..7f501cab 100644 --- a/subsystems/motor/TelescopingArmPivotFeedForward.java +++ b/subsystems/motor/TelescopingArmPivotFeedForward.java @@ -47,6 +47,7 @@ private double lerpedCg(double param) { // linear relationship between extension and cg remains the same, // because of lever arm mechanics. Prefer measuring/calculating cg for // the full range of motion to reduce relative error. + param = Math.min(Math.max(param, -0.5), 1.5); return (this.retracted_kg * (1-param)) + (this.extended_kg * param); } /** From 49422c8a5ab4b2af1823156693937d846fcf71c6 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Sat, 18 Mar 2023 00:01:54 -0700 Subject: [PATCH 110/134] manually invert left motors in WestCoastDrive (bodge) --- subsystems/chassis/WestCoastDrive.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 5aa6bf71..6ac3b855 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -139,7 +139,7 @@ public void setWheelVoltages(DifferentialDriveWheelVoltages wheelVoltages) { this.setWheelVoltages(wheelVoltages.left, wheelVoltages.right); } public void setWheelVoltages(double leftV, double rightV) { - this.leftMotors.setVoltage(leftV); + this.leftMotors.setVoltage(-leftV); this.rightMotors.setVoltage(rightV); } @@ -281,7 +281,11 @@ public Command c_controlChassisVelocity(Supplier chassisSpeedsSup * function. */ public Command c_controlWheelVoltages(Supplier wheelVoltageSupplier) { - var cmd = this.run(() -> setWheelVoltages(wheelVoltageSupplier.get())); // this.run() runs repeatedly + var cmd = this.run(() -> { + setWheelVoltages(wheelVoltageSupplier.get()); + System.out.println(wheelVoltageSupplier.get()); + } + ); // this.run() runs repeatedly cmd.addRequirements(leftMotors); cmd.addRequirements(rightMotors); return cmd; From 81bd420a9524d1411aae01c2e97a19c4ac7e6226 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sat, 18 Mar 2023 00:12:45 -0700 Subject: [PATCH 111/134] clean up todos --- CommandRobotBase.java | 2 +- custom/motioncontrollers/ezMotion.java | 2 +- subsystems/chassis/WestCoastDrive.java | 53 +++++++++++++------------- 3 files changed, 29 insertions(+), 28 deletions(-) diff --git a/CommandRobotBase.java b/CommandRobotBase.java index 6b7ba440..c25bc48c 100644 --- a/CommandRobotBase.java +++ b/CommandRobotBase.java @@ -48,7 +48,7 @@ private void cleanup() { } } - // HACK TODO, incredibly cursed and potentially bad + // HACK FIXME, incredibly cursed and potentially bad public static Driver drivingConfig = new Driver("uhohhh") { @Override public double getX() { diff --git a/custom/motioncontrollers/ezMotion.java b/custom/motioncontrollers/ezMotion.java index 13d9d5a0..2308a35b 100644 --- a/custom/motioncontrollers/ezMotion.java +++ b/custom/motioncontrollers/ezMotion.java @@ -56,7 +56,7 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - // TODO Auto-generated method stub + // FIXME Auto-generated method stub super.end(interrupted); } } diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index 10286f4c..ce54d213 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -147,32 +147,33 @@ public void setWheelVoltages(double leftV, double rightV) { - public void setChassisVoltage(ChassisSpeeds sketchyVoltages) { - final double SKETCHY_CORRECTION = 1; // TODO: tune? @zbuster05 - final var wheelVoltages = kinematics.toWheelSpeeds(sketchyVoltages); - // we do a little trolling - setWheelVoltages(wheelVoltages.leftMetersPerSecond * SKETCHY_CORRECTION, wheelVoltages.rightMetersPerSecond * SKETCHY_CORRECTION); - } - public Command c_controlChassisWithVoltage(Supplier chassisSpeedVoltsSupplier) { - var cmd = this.run(() -> { - var volts = chassisSpeedVoltsSupplier.get(); - setChassisVoltage(volts); - }); // this.run() runs repeatedly - cmd.addRequirements(leftMotors); - cmd.addRequirements(rightMotors); - return cmd; - } - /** - * A forever command that pulls left and right wheel voltages from a - * function. - */ - public Command c_controlChassisSpeedAndTurn(Supplier> chasssisSpeedAndTurnSupplier) { - var cmd = c_controlChassisWithVoltage(() -> { - Pair speedAndTurn = chasssisSpeedAndTurnSupplier.get(); - return new ChassisSpeeds(speedAndTurn.getFirst()*RobotController.getBatteryVoltage(), 0, speedAndTurn.getSecond()); - }); - return cmd; - } + // FIXME: use reasonable driver controls + // public void setChassisVoltage(ChassisSpeeds sketchyVoltages) { + // final double SKETCHY_CORRECTION = 1; + // final var wheelVoltages = kinematics.toWheelSpeeds(sketchyVoltages); + // // we do a little trolling + // setWheelVoltages(wheelVoltages.leftMetersPerSecond * SKETCHY_CORRECTION, wheelVoltages.rightMetersPerSecond * SKETCHY_CORRECTION); + // } + // public Command c_controlChassisWithVoltage(Supplier chassisSpeedVoltsSupplier) { + // var cmd = this.run(() -> { + // var volts = chassisSpeedVoltsSupplier.get(); + // setChassisVoltage(volts); + // }); // this.run() runs repeatedly + // cmd.addRequirements(leftMotors); + // cmd.addRequirements(rightMotors); + // return cmd; + // } + // /** + // * A forever command that pulls left and right wheel voltages from a + // * function. + // */ + // public Command c_controlChassisSpeedAndTurn(Supplier> chasssisSpeedAndTurnSupplier) { + // var cmd = c_controlChassisWithVoltage(() -> { + // Pair speedAndTurn = chasssisSpeedAndTurnSupplier.get(); + // return new ChassisSpeeds(speedAndTurn.getFirst()*RobotController.getBatteryVoltage(), 0, speedAndTurn.getSecond()); + // }); + // return cmd; + // } From d921939710bf9bacde5e87d32f4f62756fcc1a2a Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Sat, 18 Mar 2023 01:08:40 -0700 Subject: [PATCH 112/134] fix xboxcontroller deadzone implementation --- custom/controllers/CustomCommandXbox.java | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/custom/controllers/CustomCommandXbox.java b/custom/controllers/CustomCommandXbox.java index 52379644..da51e462 100644 --- a/custom/controllers/CustomCommandXbox.java +++ b/custom/controllers/CustomCommandXbox.java @@ -1,16 +1,15 @@ package org.usfirst.frc4904.standard.custom.controllers; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; public class CustomCommandXbox extends CommandXboxController { private final double deadZoneSize; - private final XboxController m_hid; + private final CommandXboxController m_hid; public CustomCommandXbox(int port, double deadZoneSize) { super(port); - m_hid = new XboxController(port); - if (deadZoneSize < 0 || deadZoneSize > 1) { + m_hid = new CommandXboxController(port); + if (deadZoneSize < 0 || deadZoneSize >= 1) { throw new IllegalArgumentException("CustomCommandXBox deadzone must be in [0, 1]"); } this.deadZoneSize = deadZoneSize; @@ -61,6 +60,6 @@ public static double applyDeadzone(double input, double deadZoneSize) { if (Math.abs(input) < deadZoneSize) { // return 0 if within the deadzone return 0.0; } - return (input - Math.signum(input) * deadZoneSize) / (1 - input); // linear between 0 and 1 in the remaining range + return (input - Math.signum(input) * deadZoneSize) / (1 - deadZoneSize); // linear between 0 and 1 in the remaining range } } From fcffe9d3190b72b105e49f3c6055363795cc8f70 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Wed, 22 Mar 2023 19:47:38 -0700 Subject: [PATCH 113/134] Removed unecessary logging Co-authored-by: zbuster05 --- custom/sensors/NavX.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/custom/sensors/NavX.java b/custom/sensors/NavX.java index 33b99631..a925c3fc 100644 --- a/custom/sensors/NavX.java +++ b/custom/sensors/NavX.java @@ -52,8 +52,8 @@ public double getRate() { */ public float getSafeYaw() { float yaw = super.getYaw(); - SmartDashboard.putNumber("navx_yaw", yaw); - SmartDashboard.putNumber("navx_last_yaw", lastYaw); + // SmartDashboard.putNumber("navx_yaw", yaw); + // SmartDashboard.putNumber("navx_last_yaw", lastYaw); if ((Math.abs(yaw - lastYaw) > NavX.MAX_DEGREES_PER_TICK) && (Math.abs(Math.abs(yaw - lastYaw) - 360) > NavX.MAX_DEGREES_PER_TICK)) { // Smoothing return lastYaw; From d467086d5c305107e04bc2a311034eee5192131e Mon Sep 17 00:00:00 2001 From: jac0rr Date: Fri, 24 Mar 2023 18:15:40 -0700 Subject: [PATCH 114/134] make resetpose public --- subsystems/chassis/WestCoastDrive.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index a919c6e0..f0d728a8 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -78,7 +78,7 @@ public WestCoastDrive( public double getLeftDistance() { return leftMotors.getSensorPositionRotations()/m_to_motorrots; } public double getRightDistance() { return rightMotors.getSensorPositionRotations()/m_to_motorrots; } private void zeroEncoders() { leftMotors.zeroSensors(); rightMotors.zeroSensors(); } - protected void resetPoseMeters(Pose2d metersPose) { + public void resetPoseMeters(Pose2d metersPose) { zeroEncoders(); // doesn't matter what the encoders start at, odometry will use delta of odometry.update() from odometry.reset() odometry.resetPosition(gyro.getRotation2d(), getLeftDistance(), getRightDistance(), metersPose); @@ -284,7 +284,7 @@ public Command c_controlChassisVelocity(Supplier chassisSpeedsSup public Command c_controlWheelVoltages(Supplier wheelVoltageSupplier) { var cmd = this.run(() -> { setWheelVoltages(wheelVoltageSupplier.get()); - System.out.println(wheelVoltageSupplier.get()); + //System.out.println(wheelVoltageSupplier.get()); } ); // this.run() runs repeatedly cmd.addRequirements(leftMotors); From e21cc0c707b295ae6df66b87abfbfc1bbbd3a3a7 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Mon, 27 Mar 2023 16:58:41 -0700 Subject: [PATCH 115/134] add extended kg for arm pivot --- .../motor/TelescopingArmPivotFeedForward.java | 71 ++++++++++--------- 1 file changed, 36 insertions(+), 35 deletions(-) diff --git a/subsystems/motor/TelescopingArmPivotFeedForward.java b/subsystems/motor/TelescopingArmPivotFeedForward.java index 7f501cab..4bd08bc9 100644 --- a/subsystems/motor/TelescopingArmPivotFeedForward.java +++ b/subsystems/motor/TelescopingArmPivotFeedForward.java @@ -7,12 +7,17 @@ * Internally composes a wpilib ArmFeedForward. */ public class TelescopingArmPivotFeedForward { - public final double ks; - public final double kv; - public final double ka; + public final double retracted_ks; + public final double extended_ks; + public final double retracted_kv; + public final double extended_kv; + public final double retracted_ka; + public final double extended_ka; public final double retracted_kg; public final double extended_kg; - private final ArmFeedforward armFF; + private final ArmFeedforward armFFextended; + private final ArmFeedforward armFFretracted; + /** * Computes feed-forward values for an arm rotation motor, with a varying * cg. cg is calculated by linearly interpolating between retracted and @@ -32,23 +37,19 @@ public class TelescopingArmPivotFeedForward { * @param ka Acceleration gain. Makes little difference, 0 is often an * okay default. */ - public TelescopingArmPivotFeedForward(double retracted_kg, double extended_kg, double ks, double kv, double ka) { - this.ks = ks; - this.kv = kv; - this.ka = ka; + public TelescopingArmPivotFeedForward(double retracted_kg, double extended_kg, double retracted_ks, double extended_ks, double retracted_kv, double extended_kv, double retracted_ka, double extended_ka) { + this.retracted_ks = retracted_ks; + this.extended_ks = extended_ks; + + this.extended_kv = extended_kv; + this.retracted_kv = extended_kv; + this.retracted_ka = retracted_ka; + this.extended_ka = extended_ka; + this.retracted_kg = retracted_kg; this.extended_kg = extended_kg; - this.armFF = new ArmFeedforward(ks, 0, kv, ka); // passes zero for kg because we will do our own kg calculations and add the result externally - } - private double lerpedCg(double param) { - // You could pass something outside of [0, 1] here, it would just assume - // the arm can extend in the other direction (and the cg still moves - // linearly) or more than originally specified. The assumption of a - // linear relationship between extension and cg remains the same, - // because of lever arm mechanics. Prefer measuring/calculating cg for - // the full range of motion to reduce relative error. - param = Math.min(Math.max(param, -0.5), 1.5); - return (this.retracted_kg * (1-param)) + (this.extended_kg * param); + this.armFFextended = new ArmFeedforward(extended_ks, extended_kg, extended_kv, extended_ka); // passes zero for kg because we will do our own kg calculations and add the result externally + this.armFFretracted = new ArmFeedforward(retracted_ks, retracted_kg, retracted_kv, retracted_ka); } /** * @param armExtensionRatio The extension of the arm, relative to it's full extension. Should be between 0 and 1. Used as cg linear interpolation parameter. @@ -58,7 +59,8 @@ private double lerpedCg(double param) { * @return The computed feedforward. */ public double calculate(double armExtensionRatio, double posRads, double velRadPerSec, double accelRadPerSecSquared) { - return this.armFF.calculate(posRads, velRadPerSec, accelRadPerSecSquared) + lerpedCg(armExtensionRatio) * Math.cos(posRads); + double param = Math.min(Math.max(armExtensionRatio, -0.5), 1.5); + return (this.armFFextended.calculate(posRads, velRadPerSec, accelRadPerSecSquared) * (1-param)) + (this.armFFretracted.calculate(posRads, velRadPerSec, accelRadPerSecSquared) * param); } } // Util methods to calculate max achievable velocity/acceleration from the @@ -83,10 +85,10 @@ public double calculate(double armExtensionRatio, double posRads, double velRadP * @return The maximum possible velocity at the given acceleration and * angle. */ - public double maxAchievableVelocity(double maxVoltage, double extension, double angle, double acceleration) { - // Assume max velocity is positive - return this.armFF.maxAchievableVelocity(maxVoltage, angle, acceleration) - Math.cos(angle) * lerpedCg(extension) / kv; - } + // public double maxAchievableVelocity(double maxVoltage, double extension, double angle, double acceleration) { + // // Assume max velocity is positive + // return this.armFF.maxAchievableVelocity(maxVoltage, angle, acceleration) - Math.cos(angle) * lerpedCg(extension) / kv; + // } /** * Calculates the minimum achievable velocity given a maximum voltage * supply, a position, and an acceleration. Useful for ensuring that @@ -105,10 +107,10 @@ public double maxAchievableVelocity(double maxVoltage, double extension, double * @param acceleration The acceleration of the arm. * @return The minimum possible velocity at the given acceleration and angle. */ - public double minAchievableVelocity(double maxVoltage, double extension, double angle, double acceleration) { - // Assume min velocity is negative, ks flips sign - return this.armFF.minAchievableVelocity(maxVoltage, angle, acceleration) - Math.cos(angle) * lerpedCg(extension) / kv; - } + // public double minAchievableVelocity(double maxVoltage, double extension, double angle, double acceleration) { + // // Assume min velocity is negative, ks flips sign + // return this.armFF.minAchievableVelocity(maxVoltage, angle, acceleration) - Math.cos(angle) * lerpedCg(extension) / kv; + // } /** * Calculates the maximum achievable acceleration given a maximum voltage @@ -128,9 +130,9 @@ public double minAchievableVelocity(double maxVoltage, double extension, double * @param velocity The velocity of the arm. * @return The maximum possible acceleration at the given velocity. */ - public double maxAchievableAcceleration(double maxVoltage, double extension, double angle, double velocity) { - return this.armFF.maxAchievableAcceleration(maxVoltage, angle, velocity) - Math.cos(angle) * lerpedCg(extension) / ka; - } + // public double maxAchievableAcceleration(double maxVoltage, double extension, double angle, double velocity) { + // return this.armFF.maxAchievableAcceleration(maxVoltage, angle, velocity) - Math.cos(angle) * lerpedCg(extension) / ka; + // } /** * Calculates the minimum achievable acceleration given a maximum voltage * supply, a position, and a velocity. Useful for ensuring that velocity and @@ -149,7 +151,6 @@ public double maxAchievableAcceleration(double maxVoltage, double extension, dou * @param velocity The velocity of the arm. * @return The minimum possible acceleration at the given velocity. */ - public double minAchievableAcceleration(double maxVoltage, double extension, double angle, double velocity) { - return maxAchievableAcceleration(-maxVoltage, extension, angle, velocity); // this negative voltage trick is the same as used in WPILib ArmFeedForward - } -} \ No newline at end of file + // public double minAchievableAcceleration(double maxVoltage, double extension, double angle, double velocity) { + // return maxAchievableAcceleration(-maxVoltage, extension, angle, velocity); // this negative voltage trick is the same as used in WPILib ArmFeedForward + // } \ No newline at end of file From 51735784ae2cc3ee6f420282d516eea222492983 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Tue, 28 Mar 2023 17:49:58 -0700 Subject: [PATCH 116/134] Revert "add extended kg for arm pivot", because we don't want to lerp on ff output This reverts commit e21cc0c707b295ae6df66b87abfbfc1bbbd3a3a7. --- .../motor/TelescopingArmPivotFeedForward.java | 71 +++++++++---------- 1 file changed, 35 insertions(+), 36 deletions(-) diff --git a/subsystems/motor/TelescopingArmPivotFeedForward.java b/subsystems/motor/TelescopingArmPivotFeedForward.java index 4bd08bc9..7f501cab 100644 --- a/subsystems/motor/TelescopingArmPivotFeedForward.java +++ b/subsystems/motor/TelescopingArmPivotFeedForward.java @@ -7,17 +7,12 @@ * Internally composes a wpilib ArmFeedForward. */ public class TelescopingArmPivotFeedForward { - public final double retracted_ks; - public final double extended_ks; - public final double retracted_kv; - public final double extended_kv; - public final double retracted_ka; - public final double extended_ka; + public final double ks; + public final double kv; + public final double ka; public final double retracted_kg; public final double extended_kg; - private final ArmFeedforward armFFextended; - private final ArmFeedforward armFFretracted; - + private final ArmFeedforward armFF; /** * Computes feed-forward values for an arm rotation motor, with a varying * cg. cg is calculated by linearly interpolating between retracted and @@ -37,19 +32,23 @@ public class TelescopingArmPivotFeedForward { * @param ka Acceleration gain. Makes little difference, 0 is often an * okay default. */ - public TelescopingArmPivotFeedForward(double retracted_kg, double extended_kg, double retracted_ks, double extended_ks, double retracted_kv, double extended_kv, double retracted_ka, double extended_ka) { - this.retracted_ks = retracted_ks; - this.extended_ks = extended_ks; - - this.extended_kv = extended_kv; - this.retracted_kv = extended_kv; - this.retracted_ka = retracted_ka; - this.extended_ka = extended_ka; - + public TelescopingArmPivotFeedForward(double retracted_kg, double extended_kg, double ks, double kv, double ka) { + this.ks = ks; + this.kv = kv; + this.ka = ka; this.retracted_kg = retracted_kg; this.extended_kg = extended_kg; - this.armFFextended = new ArmFeedforward(extended_ks, extended_kg, extended_kv, extended_ka); // passes zero for kg because we will do our own kg calculations and add the result externally - this.armFFretracted = new ArmFeedforward(retracted_ks, retracted_kg, retracted_kv, retracted_ka); + this.armFF = new ArmFeedforward(ks, 0, kv, ka); // passes zero for kg because we will do our own kg calculations and add the result externally + } + private double lerpedCg(double param) { + // You could pass something outside of [0, 1] here, it would just assume + // the arm can extend in the other direction (and the cg still moves + // linearly) or more than originally specified. The assumption of a + // linear relationship between extension and cg remains the same, + // because of lever arm mechanics. Prefer measuring/calculating cg for + // the full range of motion to reduce relative error. + param = Math.min(Math.max(param, -0.5), 1.5); + return (this.retracted_kg * (1-param)) + (this.extended_kg * param); } /** * @param armExtensionRatio The extension of the arm, relative to it's full extension. Should be between 0 and 1. Used as cg linear interpolation parameter. @@ -59,8 +58,7 @@ public TelescopingArmPivotFeedForward(double retracted_kg, double extended_kg, d * @return The computed feedforward. */ public double calculate(double armExtensionRatio, double posRads, double velRadPerSec, double accelRadPerSecSquared) { - double param = Math.min(Math.max(armExtensionRatio, -0.5), 1.5); - return (this.armFFextended.calculate(posRads, velRadPerSec, accelRadPerSecSquared) * (1-param)) + (this.armFFretracted.calculate(posRads, velRadPerSec, accelRadPerSecSquared) * param); } + return this.armFF.calculate(posRads, velRadPerSec, accelRadPerSecSquared) + lerpedCg(armExtensionRatio) * Math.cos(posRads); } // Util methods to calculate max achievable velocity/acceleration from the @@ -85,10 +83,10 @@ public double calculate(double armExtensionRatio, double posRads, double velRadP * @return The maximum possible velocity at the given acceleration and * angle. */ - // public double maxAchievableVelocity(double maxVoltage, double extension, double angle, double acceleration) { - // // Assume max velocity is positive - // return this.armFF.maxAchievableVelocity(maxVoltage, angle, acceleration) - Math.cos(angle) * lerpedCg(extension) / kv; - // } + public double maxAchievableVelocity(double maxVoltage, double extension, double angle, double acceleration) { + // Assume max velocity is positive + return this.armFF.maxAchievableVelocity(maxVoltage, angle, acceleration) - Math.cos(angle) * lerpedCg(extension) / kv; + } /** * Calculates the minimum achievable velocity given a maximum voltage * supply, a position, and an acceleration. Useful for ensuring that @@ -107,10 +105,10 @@ public double calculate(double armExtensionRatio, double posRads, double velRadP * @param acceleration The acceleration of the arm. * @return The minimum possible velocity at the given acceleration and angle. */ - // public double minAchievableVelocity(double maxVoltage, double extension, double angle, double acceleration) { - // // Assume min velocity is negative, ks flips sign - // return this.armFF.minAchievableVelocity(maxVoltage, angle, acceleration) - Math.cos(angle) * lerpedCg(extension) / kv; - // } + public double minAchievableVelocity(double maxVoltage, double extension, double angle, double acceleration) { + // Assume min velocity is negative, ks flips sign + return this.armFF.minAchievableVelocity(maxVoltage, angle, acceleration) - Math.cos(angle) * lerpedCg(extension) / kv; + } /** * Calculates the maximum achievable acceleration given a maximum voltage @@ -130,9 +128,9 @@ public double calculate(double armExtensionRatio, double posRads, double velRadP * @param velocity The velocity of the arm. * @return The maximum possible acceleration at the given velocity. */ - // public double maxAchievableAcceleration(double maxVoltage, double extension, double angle, double velocity) { - // return this.armFF.maxAchievableAcceleration(maxVoltage, angle, velocity) - Math.cos(angle) * lerpedCg(extension) / ka; - // } + public double maxAchievableAcceleration(double maxVoltage, double extension, double angle, double velocity) { + return this.armFF.maxAchievableAcceleration(maxVoltage, angle, velocity) - Math.cos(angle) * lerpedCg(extension) / ka; + } /** * Calculates the minimum achievable acceleration given a maximum voltage * supply, a position, and a velocity. Useful for ensuring that velocity and @@ -151,6 +149,7 @@ public double calculate(double armExtensionRatio, double posRads, double velRadP * @param velocity The velocity of the arm. * @return The minimum possible acceleration at the given velocity. */ - // public double minAchievableAcceleration(double maxVoltage, double extension, double angle, double velocity) { - // return maxAchievableAcceleration(-maxVoltage, extension, angle, velocity); // this negative voltage trick is the same as used in WPILib ArmFeedForward - // } \ No newline at end of file + public double minAchievableAcceleration(double maxVoltage, double extension, double angle, double velocity) { + return maxAchievableAcceleration(-maxVoltage, extension, angle, velocity); // this negative voltage trick is the same as used in WPILib ArmFeedForward + } +} \ No newline at end of file From f8afa8106e399f23d923d7278cc2b4ab7c740c5d Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Sat, 1 Apr 2023 19:04:40 -0700 Subject: [PATCH 117/134] Added triplet type --- custom/Triple.java | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 custom/Triple.java diff --git a/custom/Triple.java b/custom/Triple.java new file mode 100644 index 00000000..c017b0c8 --- /dev/null +++ b/custom/Triple.java @@ -0,0 +1,29 @@ +package org.usfirst.frc4904.standard.custom; + +public class Triple { + private final A m_first; + private final B m_second; + private final C m_third; + + public Triple(A first, B second, C third) { + m_first = first; + m_second = second; + m_third = third; + } + + public A getFirst() { + return m_first; + } + + public B getSecond() { + return m_second; + } + + public C getThird() { + return m_third; + } + + public static Triple of(A a, B b, C c) { + return new Triple<>(a, b, c); + } +} From d7c1a03313e82cc40cd10590afdce47af056697c Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sun, 2 Apr 2023 20:22:36 -0700 Subject: [PATCH 118/134] easymotion takes setpointDealerDealer --- custom/motioncontrollers/ezMotion.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/custom/motioncontrollers/ezMotion.java b/custom/motioncontrollers/ezMotion.java index 2308a35b..1881b702 100644 --- a/custom/motioncontrollers/ezMotion.java +++ b/custom/motioncontrollers/ezMotion.java @@ -3,6 +3,7 @@ import java.util.function.DoubleConsumer; import java.util.function.DoubleFunction; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; import org.opencv.core.Mat.Tuple2; @@ -15,14 +16,15 @@ public class ezMotion extends CommandBase { public DoubleConsumer processVariable; public Double initialTimestamp; public DoubleSupplier feedback; - public DoubleFunction> setpointProvider; + public Supplier>> setpointDealerDealer; + public DoubleFunction> setpointDealer = null; - public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, DoubleFunction> setpoint, Subsystem... requirements) { + public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, Supplier>> setpointDealerDealer, Subsystem... requirements) { // FIXME: use Pair addRequirements(requirements); this.control = control; this.processVariable = processVariable; this.feedback = feedback; - this.setpointProvider = setpoint; + this.setpointDealerDealer = setpointDealerDealer; } public double getElapsedTime() { @@ -35,12 +37,13 @@ public boolean atLatestSetpoint() { @Override public void initialize() { + this.setpointDealer = setpointDealerDealer.get(); this.initialTimestamp = Timer.getFPGATimestamp(); } @Override public void execute() { - Tuple2 setpoints = this.setpointProvider.apply(getElapsedTime()); + Tuple2 setpoints = this.setpointDealer.apply(getElapsedTime()); double setpoint = setpoints.get_0(); double setpoint_dt = setpoints.get_1(); From 1af05e38d023a6e056a9047ed1970b75a815d790 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sun, 2 Apr 2023 20:54:44 -0700 Subject: [PATCH 119/134] UT: trigger command factory wrapper --- commands/TriggerCommandFactory.java | 43 +++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) create mode 100644 commands/TriggerCommandFactory.java diff --git a/commands/TriggerCommandFactory.java b/commands/TriggerCommandFactory.java new file mode 100644 index 00000000..64819f12 --- /dev/null +++ b/commands/TriggerCommandFactory.java @@ -0,0 +1,43 @@ +package org.usfirst.frc4904.standard.commands; + +import java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class TriggerCommandFactory extends CommandBase { + private final Supplier commandDealer; + private Command currentActiveCommand = null; + + /** + * Command that takes in a command factory and runs a new command from that + * factory each time it is scheduled. This avoids the problems of command + * factory code running on "construct" rather than on "initialize." + * + * @param commandDealer The Command factory + */ + public TriggerCommandFactory(Supplier commandDealer) { + this.commandDealer = commandDealer; + } + @Override + public void initialize() { + currentActiveCommand = commandDealer.get(); + currentActiveCommand.schedule(); + } + @Override + public void execute() { + // no need to do anything here, as the scheduler should execute as needed. + } + @Override + public boolean isFinished() { + if (currentActiveCommand != null) return currentActiveCommand.isFinished(); + return true; + } + public void end(boolean wasInturrupted) { + if (currentActiveCommand != null && wasInturrupted) { + currentActiveCommand.cancel(); + // no need to worry about calling .end() on currentActiveCommand, as if the ending was caused by isFinished() -> true, then the scheduler will deal with calling .end() on the active command. + } + currentActiveCommand = null; + } +} From 3ce009290aeac7107fef9206a7e5c90d2f355fb3 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sun, 2 Apr 2023 21:53:52 -0700 Subject: [PATCH 120/134] should we add requirements to triggercommandfactory --- commands/TriggerCommandFactory.java | 3 +++ custom/motioncontrollers/ezMotion.java | 4 ++++ 2 files changed, 7 insertions(+) diff --git a/commands/TriggerCommandFactory.java b/commands/TriggerCommandFactory.java index 64819f12..aa8725f3 100644 --- a/commands/TriggerCommandFactory.java +++ b/commands/TriggerCommandFactory.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Subsystem; public class TriggerCommandFactory extends CommandBase { private final Supplier commandDealer; @@ -18,6 +19,8 @@ public class TriggerCommandFactory extends CommandBase { */ public TriggerCommandFactory(Supplier commandDealer) { this.commandDealer = commandDealer; + // TODO: do we need to add requirements? + // var reqs = this.commandDealer.get().getRequirements(); addRequirements(reqs.toArray(new Subsystem[reqs.size()])); } @Override public void initialize() { diff --git a/custom/motioncontrollers/ezMotion.java b/custom/motioncontrollers/ezMotion.java index 1881b702..e7a2f3a4 100644 --- a/custom/motioncontrollers/ezMotion.java +++ b/custom/motioncontrollers/ezMotion.java @@ -27,6 +27,10 @@ public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer proce this.setpointDealerDealer = setpointDealerDealer; } + public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, DoubleFunction> setpointDealer, Subsystem... requirements) { + this(control, feedback, processVariable, () -> setpointDealer, requirements); + } + public double getElapsedTime() { return Timer.getFPGATimestamp() - initialTimestamp; } From d2415f03fcdc8e09e11719fc5cfef17850fc6de0 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 3 Apr 2023 14:40:14 -0700 Subject: [PATCH 121/134] named triggerCommandFactory --- commands/TriggerCommandFactory.java | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/commands/TriggerCommandFactory.java b/commands/TriggerCommandFactory.java index aa8725f3..c64cf254 100644 --- a/commands/TriggerCommandFactory.java +++ b/commands/TriggerCommandFactory.java @@ -9,7 +9,15 @@ public class TriggerCommandFactory extends CommandBase { private final Supplier commandDealer; private Command currentActiveCommand = null; + private final String name; + public TriggerCommandFactory(String name, Supplier commandDealer) { + this.commandDealer = commandDealer; + this.name = name; + setName(name); + // TODO: do we need to add requirements? + // var reqs = this.commandDealer.get().getRequirements(); addRequirements(reqs.toArray(new Subsystem[reqs.size()])); + } /** * Command that takes in a command factory and runs a new command from that * factory each time it is scheduled. This avoids the problems of command @@ -18,9 +26,7 @@ public class TriggerCommandFactory extends CommandBase { * @param commandDealer The Command factory */ public TriggerCommandFactory(Supplier commandDealer) { - this.commandDealer = commandDealer; - // TODO: do we need to add requirements? - // var reqs = this.commandDealer.get().getRequirements(); addRequirements(reqs.toArray(new Subsystem[reqs.size()])); + this("Unnamed TriggerCommandFactory", commandDealer); } @Override public void initialize() { From 5217d908faa8f888b1b3fad9e5fc56372ab8abb2 Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Mon, 3 Apr 2023 15:26:22 -0700 Subject: [PATCH 122/134] actually fix triggercommandfactory to instantly end --- commands/TriggerCommandFactory.java | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/commands/TriggerCommandFactory.java b/commands/TriggerCommandFactory.java index c64cf254..e53d0f13 100644 --- a/commands/TriggerCommandFactory.java +++ b/commands/TriggerCommandFactory.java @@ -11,6 +11,7 @@ public class TriggerCommandFactory extends CommandBase { private Command currentActiveCommand = null; private final String name; + // basically a Commands.runOnce(commandDealer.get().schedule()) and it's named public TriggerCommandFactory(String name, Supplier commandDealer) { this.commandDealer = commandDealer; this.name = name; @@ -39,14 +40,8 @@ public void execute() { } @Override public boolean isFinished() { - if (currentActiveCommand != null) return currentActiveCommand.isFinished(); return true; } public void end(boolean wasInturrupted) { - if (currentActiveCommand != null && wasInturrupted) { - currentActiveCommand.cancel(); - // no need to worry about calling .end() on currentActiveCommand, as if the ending was caused by isFinished() -> true, then the scheduler will deal with calling .end() on the active command. - } - currentActiveCommand = null; } } From 510ff09af56862edc9f8f54ab9e06afd0621f415 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Mon, 3 Apr 2023 19:44:15 -0700 Subject: [PATCH 123/134] UT: make triggerCommandFactory take varargs --- commands/TriggerCommandFactory.java | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/commands/TriggerCommandFactory.java b/commands/TriggerCommandFactory.java index c64cf254..a77750ea 100644 --- a/commands/TriggerCommandFactory.java +++ b/commands/TriggerCommandFactory.java @@ -7,12 +7,13 @@ import edu.wpi.first.wpilibj2.command.Subsystem; public class TriggerCommandFactory extends CommandBase { - private final Supplier commandDealer; - private Command currentActiveCommand = null; + private final Supplier[] commandDealers; + private Command[] currentActiveCommands = null; private final String name; - public TriggerCommandFactory(String name, Supplier commandDealer) { - this.commandDealer = commandDealer; + public TriggerCommandFactory(String name, Supplier... commandDealers) { + this.commandDealers = commandDealers; + this.currentActiveCommands = new Command[commandDealers.length]; this.name = name; setName(name); // TODO: do we need to add requirements? @@ -25,13 +26,15 @@ public TriggerCommandFactory(String name, Supplier commandDealer) { * * @param commandDealer The Command factory */ - public TriggerCommandFactory(Supplier commandDealer) { - this("Unnamed TriggerCommandFactory", commandDealer); + public TriggerCommandFactory(Supplier... commandDealers) { + this("Unnamed TriggerCommandFactory", commandDealers); } @Override public void initialize() { - currentActiveCommand = commandDealer.get(); - currentActiveCommand.schedule(); + for (int i=0; i true, then the scheduler will deal with calling .end() on the active command. + if (wasInturrupted) { + for (int i=0; i Date: Mon, 3 Apr 2023 19:59:36 -0700 Subject: [PATCH 124/134] triggerCommandFactory non-varargs overrides --- commands/TriggerCommandFactory.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/commands/TriggerCommandFactory.java b/commands/TriggerCommandFactory.java index a77750ea..4cc07c8e 100644 --- a/commands/TriggerCommandFactory.java +++ b/commands/TriggerCommandFactory.java @@ -11,6 +11,7 @@ public class TriggerCommandFactory extends CommandBase { private Command[] currentActiveCommands = null; private final String name; + public TriggerCommandFactory(String name, Supplier commandDealer) { this(name, commandDealer, null); } public TriggerCommandFactory(String name, Supplier... commandDealers) { this.commandDealers = commandDealers; this.currentActiveCommands = new Command[commandDealers.length]; @@ -29,9 +30,12 @@ public TriggerCommandFactory(String name, Supplier... commandDealers) { public TriggerCommandFactory(Supplier... commandDealers) { this("Unnamed TriggerCommandFactory", commandDealers); } + public TriggerCommandFactory(Supplier commandDealer) { this("Unnamed TriggerCommandFactory", commandDealer); } + @Override public void initialize() { for (int i=0; i Date: Wed, 5 Apr 2023 19:46:53 -0700 Subject: [PATCH 125/134] Revert "triggerCommandFactory non-varargs overrides" This reverts commit 464662714221519419c8822fcaa837c4de035cc6. --- commands/TriggerCommandFactory.java | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/commands/TriggerCommandFactory.java b/commands/TriggerCommandFactory.java index 4cc07c8e..a77750ea 100644 --- a/commands/TriggerCommandFactory.java +++ b/commands/TriggerCommandFactory.java @@ -11,7 +11,6 @@ public class TriggerCommandFactory extends CommandBase { private Command[] currentActiveCommands = null; private final String name; - public TriggerCommandFactory(String name, Supplier commandDealer) { this(name, commandDealer, null); } public TriggerCommandFactory(String name, Supplier... commandDealers) { this.commandDealers = commandDealers; this.currentActiveCommands = new Command[commandDealers.length]; @@ -30,12 +29,9 @@ public TriggerCommandFactory(String name, Supplier... commandDealers) { public TriggerCommandFactory(Supplier... commandDealers) { this("Unnamed TriggerCommandFactory", commandDealers); } - public TriggerCommandFactory(Supplier commandDealer) { this("Unnamed TriggerCommandFactory", commandDealer); } - @Override public void initialize() { for (int i=0; i Date: Wed, 5 Apr 2023 19:47:07 -0700 Subject: [PATCH 126/134] Revert "UT: make triggerCommandFactory take varargs" This reverts commit 510ff09af56862edc9f8f54ab9e06afd0621f415. --- commands/TriggerCommandFactory.java | 28 +++++++++++++--------------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/commands/TriggerCommandFactory.java b/commands/TriggerCommandFactory.java index a77750ea..c64cf254 100644 --- a/commands/TriggerCommandFactory.java +++ b/commands/TriggerCommandFactory.java @@ -7,13 +7,12 @@ import edu.wpi.first.wpilibj2.command.Subsystem; public class TriggerCommandFactory extends CommandBase { - private final Supplier[] commandDealers; - private Command[] currentActiveCommands = null; + private final Supplier commandDealer; + private Command currentActiveCommand = null; private final String name; - public TriggerCommandFactory(String name, Supplier... commandDealers) { - this.commandDealers = commandDealers; - this.currentActiveCommands = new Command[commandDealers.length]; + public TriggerCommandFactory(String name, Supplier commandDealer) { + this.commandDealer = commandDealer; this.name = name; setName(name); // TODO: do we need to add requirements? @@ -26,15 +25,13 @@ public TriggerCommandFactory(String name, Supplier... commandDealers) { * * @param commandDealer The Command factory */ - public TriggerCommandFactory(Supplier... commandDealers) { - this("Unnamed TriggerCommandFactory", commandDealers); + public TriggerCommandFactory(Supplier commandDealer) { + this("Unnamed TriggerCommandFactory", commandDealer); } @Override public void initialize() { - for (int i=0; i true, then the scheduler will deal with calling .end() on the active command. } + currentActiveCommand = null; } } From 49bb104fc5b0dfee1b31e34ca26fbd37b2b85568 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Wed, 5 Apr 2023 19:48:02 -0700 Subject: [PATCH 127/134] trigger command factory sets own name --- commands/TriggerCommandFactory.java | 1 + 1 file changed, 1 insertion(+) diff --git a/commands/TriggerCommandFactory.java b/commands/TriggerCommandFactory.java index c64cf254..80237ea3 100644 --- a/commands/TriggerCommandFactory.java +++ b/commands/TriggerCommandFactory.java @@ -32,6 +32,7 @@ public TriggerCommandFactory(Supplier commandDealer) { public void initialize() { currentActiveCommand = commandDealer.get(); currentActiveCommand.schedule(); + setName("trigger: " + currentActiveCommand.getName()); } @Override public void execute() { From 109068d9539104d0a255b81e0f1f6efb0e6f5f2b Mon Sep 17 00:00:00 2001 From: roboticsteam4904-2 Date: Thu, 6 Apr 2023 14:09:29 -0700 Subject: [PATCH 128/134] nice --- commands/TriggerCommandFactory.java | 1 - 1 file changed, 1 deletion(-) diff --git a/commands/TriggerCommandFactory.java b/commands/TriggerCommandFactory.java index 80237ea3..5c85eb7d 100644 --- a/commands/TriggerCommandFactory.java +++ b/commands/TriggerCommandFactory.java @@ -40,7 +40,6 @@ public void execute() { } @Override public boolean isFinished() { - if (currentActiveCommand != null) return currentActiveCommand.isFinished(); return true; } public void end(boolean wasInturrupted) { From e0154e9a00d4254f9b9dd3befedc2b8504d22d94 Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Sat, 13 May 2023 21:00:50 -0500 Subject: [PATCH 129/134] Fixed OpenCV tuple2 --- custom/motioncontrollers/ezMotion.java | 17 ++++++++--------- subsystems/chassis/WestCoastDrive.java | 2 +- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/custom/motioncontrollers/ezMotion.java b/custom/motioncontrollers/ezMotion.java index e7a2f3a4..aea1d29f 100644 --- a/custom/motioncontrollers/ezMotion.java +++ b/custom/motioncontrollers/ezMotion.java @@ -5,8 +5,7 @@ import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import org.opencv.core.Mat.Tuple2; - +import edu.wpi.first.math.Pair; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.Subsystem; @@ -16,10 +15,10 @@ public class ezMotion extends CommandBase { public DoubleConsumer processVariable; public Double initialTimestamp; public DoubleSupplier feedback; - public Supplier>> setpointDealerDealer; - public DoubleFunction> setpointDealer = null; + public Supplier>> setpointDealerDealer; + public DoubleFunction> setpointDealer = null; - public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, Supplier>> setpointDealerDealer, Subsystem... requirements) { // FIXME: use Pair + public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, Supplier>> setpointDealerDealer, Subsystem... requirements) { // FIXME: use Pair addRequirements(requirements); this.control = control; this.processVariable = processVariable; @@ -27,7 +26,7 @@ public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer proce this.setpointDealerDealer = setpointDealerDealer; } - public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, DoubleFunction> setpointDealer, Subsystem... requirements) { + public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, DoubleFunction> setpointDealer, Subsystem... requirements) { this(control, feedback, processVariable, () -> setpointDealer, requirements); } @@ -47,9 +46,9 @@ public void initialize() { @Override public void execute() { - Tuple2 setpoints = this.setpointDealer.apply(getElapsedTime()); - double setpoint = setpoints.get_0(); - double setpoint_dt = setpoints.get_1(); + Pair setpoints = this.setpointDealer.apply(getElapsedTime()); + double setpoint = setpoints.getFirst(); + double setpoint_dt = setpoints.getSecond(); control.updateSetpoint(setpoint, setpoint_dt); double controlEffort = control.calculate(feedback.getAsDouble(), getElapsedTime()); diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index f0d728a8..c4ac6b43 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -69,7 +69,7 @@ public WestCoastDrive( odometry = new DifferentialDriveOdometry(gyro.getRotation2d(), getLeftDistance(), getRightDistance()); // OPTIM should probably allow specification of f, max_accumulation, and peakOutput in constructor - leftMotors.configPIDF(drive_kP, drive_kI, drive_kD, 0, 100, 1, null); + leftMotors. configPIDF(drive_kP, drive_kI, drive_kD, 0, 100, 1, null); rightMotors.configPIDF(drive_kP, drive_kI, drive_kD, 0, 100, 1, null); zeroEncoders(); } From 529e5761991cd4eb54bc3fa80bd09e49f081f3ad Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sat, 13 May 2023 19:20:25 -0700 Subject: [PATCH 130/134] fix TriggerCommandFactory moments (construct != initialize); deprecate things --- subsystems/chassis/WestCoastDrive.java | 63 ++++++++++++++++------- subsystems/motor/TalonMotorSubsystem.java | 8 +-- 2 files changed, 48 insertions(+), 23 deletions(-) diff --git a/subsystems/chassis/WestCoastDrive.java b/subsystems/chassis/WestCoastDrive.java index c4ac6b43..5d8ee0be 100644 --- a/subsystems/chassis/WestCoastDrive.java +++ b/subsystems/chassis/WestCoastDrive.java @@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -317,28 +318,52 @@ public Command c_idle() { * * Replaces ChassisMinimumDrive in pre-2023 standard. */ + @Deprecated // this is bad and only exists for feature parity with pre-2023 standard. use splines instead public Command c_chassisMinimumDistance(double distance_meters, double speed_mps) { - var left_motor_initial = this.leftMotors.getSensorPositionRotations(); - var right_motor_initial = this.rightMotors.getSensorPositionRotations(); - return this.run(() -> setChassisVelocity(new ChassisSpeeds(speed_mps, 0, 0))) - .until(() -> ( - ( - (this.leftMotors.getSensorPositionRotations() - left_motor_initial)/2 - + (this.rightMotors.getSensorPositionRotations() - right_motor_initial)/2 - ) > Math.abs(distance_meters * m_to_motorrots) )) - .andThen(() -> this.c_idle()); + return new CommandBase() { + double left_motor_initial; + double right_motor_initial; + @Override + public void initialize() { + this.left_motor_initial = leftMotors .getSensorPositionRotations(); + this.right_motor_initial = rightMotors.getSensorPositionRotations(); + } + @Override + public void execute() { + setChassisVelocity(new ChassisSpeeds(speed_mps, 0, 0)); + } + @Override + public boolean isFinished() { + return ( + (leftMotors.getSensorPositionRotations() - this.left_motor_initial)/2 + + (rightMotors.getSensorPositionRotations() - this.right_motor_initial)/2 + ) > Math.abs(distance_meters * m_to_motorrots); + } + }.andThen(this.c_idle()); } + @Deprecated // this is bad and only exists for feature parity with pre-2023 standard. use splines instead public Command c_chassisTurn(double angle_degrees, double max_turnspeed) { - var left_motor_initial = this.leftMotors.getSensorPositionRotations(); - var right_motor_initial = this.rightMotors.getSensorPositionRotations(); - return this.run(() -> setChassisVelocity(new ChassisSpeeds(0, 0, max_turnspeed*Math.signum(angle_degrees)))) - .until(() -> ( - ( - Math.abs(this.leftMotors .getSensorPositionRotations()- left_motor_initial) - +Math.abs(this.rightMotors.getSensorPositionRotations()-right_motor_initial) - ) > Math.abs(Math.PI * kinematics.trackWidthMeters * angle_degrees / 180 * m_to_motorrots) - )) // when we turn, the wheels trace out a circle with trackwidth as a diameter. this checks that the wheels have traveled the right distance aroun the circle for our angle - .andThen(() -> this.c_idle()); + return new CommandBase() { + double left_motor_initial; + double right_motor_initial; + @Override + public void initialize() { + this.left_motor_initial = leftMotors.getSensorPositionRotations(); + this.right_motor_initial = rightMotors.getSensorPositionRotations(); + } + @Override + public void execute() { + setChassisVelocity(new ChassisSpeeds(0, 0, max_turnspeed*Math.signum(angle_degrees))); + } + @Override + public boolean isFinished() { + // when we turn, the wheels trace out a circle with trackwidth as a diameter. this checks that the wheels have traveled the right distance aroun the circle for our angle + return ( + Math.abs(leftMotors .getSensorPositionRotations()-this. left_motor_initial) + +Math.abs(rightMotors.getSensorPositionRotations()-this.right_motor_initial) + ) > Math.abs(Math.PI * kinematics.trackWidthMeters * angle_degrees / 180 * m_to_motorrots); + } + }.andThen(() -> this.c_idle()); } } diff --git a/subsystems/motor/TalonMotorSubsystem.java b/subsystems/motor/TalonMotorSubsystem.java index 5e395c57..78c5fdca 100644 --- a/subsystems/motor/TalonMotorSubsystem.java +++ b/subsystems/motor/TalonMotorSubsystem.java @@ -210,7 +210,7 @@ public TalonMotorSubsystem(String name, NeutralMode neutralMode, double voltageC * @param peakOutput in units of percent output, [-1, 1] * @param pid_slot range [0, 3], pass null for default of zero. */ - @Override + @Override @Deprecated public void configPIDF(double p, double i, double d, double f, double max_integral_accumulation, double peakOutput, Integer pid_slot) { if (pid_slot == null) pid_slot = TalonMotorSubsystem.DEFAULT_PID_SLOT; @@ -229,10 +229,10 @@ public void configPIDF(double p, double i, double d, double f, double max_integr pid_configured = true; // other things in the example: motionmagic config and statusframeperiod (for updating sensor status to the aux motor?) } - @Override /** * Assumes that PID and DMP slots correspond (eg. use PID slot 0 for DMP slot 0) */ + @Override @Deprecated public void configDMP(double minRPM, double cruiseRPM, double accl_RPMps, double maxError_encoderTicks, Integer dmp_slot) { if (dmp_slot == null) dmp_slot = DEFAULT_DMP_SLOT; @@ -319,7 +319,7 @@ public Command c_holdRPM(double setpoint) { */ public Command c_setPosition(double setpoint, int dmp_slot) { this.leadMotor.selectProfileSlot(dmp_slot, dmp_slot); - //return this.runOnce(() -> setDynamicMotionProfileTargetRotations(setpoint)).andThen(new CommandBase(){}); + //return this.runOnce(() -> setDynamicMotionProfileTargetRotations(setpoint)).andThen(new CommandBase(){}); // this would go forever return new HardwareDMPUntilArrival(this, setpoint); } @Override @@ -329,7 +329,7 @@ public Command c_setPosition(double setpoint, int dmp_slot) { public Command c_controlPosition(DoubleSupplier setpointSupplier, DoubleSupplier feedforwardSupplierVolts) { return this.run(() -> this.leadMotor.set( ControlMode.Position, setpointSupplier.getAsDouble(), - DemandType.ArbitraryFeedForward, feedforwardSupplierVolts.getAsDouble()/RobotController.getBatteryVoltage() + DemandType.ArbitraryFeedForward, feedforwardSupplierVolts.getAsDouble()/RobotController.getBatteryVoltage() // hack because WPI_CANTalonFX only takes feedforward in units of %output )); } @Override From 9e5a1b873cfa1d34bd9124766cec48cb521acf86 Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sat, 13 May 2023 20:36:46 -0700 Subject: [PATCH 131/134] rename triggerCommandFactory to CreateAndDisown --- .../{TriggerCommandFactory.java => CreateAndDisown.java} | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) rename commands/{TriggerCommandFactory.java => CreateAndDisown.java} (89%) diff --git a/commands/TriggerCommandFactory.java b/commands/CreateAndDisown.java similarity index 89% rename from commands/TriggerCommandFactory.java rename to commands/CreateAndDisown.java index 5c85eb7d..ffca8acb 100644 --- a/commands/TriggerCommandFactory.java +++ b/commands/CreateAndDisown.java @@ -6,12 +6,12 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpilibj2.command.Subsystem; -public class TriggerCommandFactory extends CommandBase { +public class CreateAndDisown extends CommandBase { private final Supplier commandDealer; private Command currentActiveCommand = null; private final String name; - public TriggerCommandFactory(String name, Supplier commandDealer) { + public CreateAndDisown(String name, Supplier commandDealer) { this.commandDealer = commandDealer; this.name = name; setName(name); @@ -25,7 +25,7 @@ public TriggerCommandFactory(String name, Supplier commandDealer) { * * @param commandDealer The Command factory */ - public TriggerCommandFactory(Supplier commandDealer) { + public CreateAndDisown(Supplier commandDealer) { this("Unnamed TriggerCommandFactory", commandDealer); } @Override From d2218bf5cbde0224df29fd055acdabff2c3ec14c Mon Sep 17 00:00:00 2001 From: Exr0n Date: Sat, 13 May 2023 20:39:22 -0700 Subject: [PATCH 132/134] clean up CreateAndDisown --- commands/CreateAndDisown.java | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/commands/CreateAndDisown.java b/commands/CreateAndDisown.java index ffca8acb..1edd1539 100644 --- a/commands/CreateAndDisown.java +++ b/commands/CreateAndDisown.java @@ -4,18 +4,16 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.Subsystem; +// to be replaced by CommandBased public class CreateAndDisown extends CommandBase { private final Supplier commandDealer; private Command currentActiveCommand = null; - private final String name; public CreateAndDisown(String name, Supplier commandDealer) { this.commandDealer = commandDealer; - this.name = name; setName(name); - // TODO: do we need to add requirements? + // do we need to add requirements? probably not, this ends immediately anyway // var reqs = this.commandDealer.get().getRequirements(); addRequirements(reqs.toArray(new Subsystem[reqs.size()])); } /** @@ -26,13 +24,13 @@ public CreateAndDisown(String name, Supplier commandDealer) { * @param commandDealer The Command factory */ public CreateAndDisown(Supplier commandDealer) { - this("Unnamed TriggerCommandFactory", commandDealer); + this("Unnamed CreateAndDisown", commandDealer); } @Override public void initialize() { currentActiveCommand = commandDealer.get(); currentActiveCommand.schedule(); - setName("trigger: " + currentActiveCommand.getName()); + setName("C&D: " + currentActiveCommand.getName()); } @Override public void execute() { From ef7dc7c7bd4867dc5b91a9b4fa42c89bc7088a7a Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Sat, 13 May 2023 22:39:22 -0500 Subject: [PATCH 133/134] can end --- custom/motioncontrollers/ezMotion.java | 54 +++++++++++++++++++------- 1 file changed, 39 insertions(+), 15 deletions(-) diff --git a/custom/motioncontrollers/ezMotion.java b/custom/motioncontrollers/ezMotion.java index aea1d29f..b2735dab 100644 --- a/custom/motioncontrollers/ezMotion.java +++ b/custom/motioncontrollers/ezMotion.java @@ -1,13 +1,16 @@ package org.usfirst.frc4904.standard.custom.motioncontrollers; import java.util.function.DoubleConsumer; -import java.util.function.DoubleFunction; import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import org.usfirst.frc4904.standard.custom.motioncontrollers.ezMotion.SetpointProvider.EndSignal; + import edu.wpi.first.math.Pair; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.Subsystem; public class ezMotion extends CommandBase { @@ -15,18 +18,33 @@ public class ezMotion extends CommandBase { public DoubleConsumer processVariable; public Double initialTimestamp; public DoubleSupplier feedback; - public Supplier>> setpointDealerDealer; - public DoubleFunction> setpointDealer = null; - public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, Supplier>> setpointDealerDealer, Subsystem... requirements) { // FIXME: use Pair + private double setpoint; + private double setpoint_dt; + + public Supplier>> setpointDealerDealer; + public SetpointProvider> setpointDealer = null; + + public Command onArrival; + + public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, Supplier>> setpointDealerDealer, Command onArrival, Subsystem... requirements) { addRequirements(requirements); this.control = control; this.processVariable = processVariable; this.feedback = feedback; this.setpointDealerDealer = setpointDealerDealer; + this.onArrival = onArrival != null? (onArrival) : (new InstantCommand(() -> {})); + } + + public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, Supplier>> setpointDealerDealer, Subsystem... requirements) { + this(control, feedback, processVariable, setpointDealerDealer, new InstantCommand(() -> {}), requirements); } - public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, DoubleFunction> setpointDealer, Subsystem... requirements) { + public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, SetpointProvider> setpointDealer, Command onArrival, Subsystem... requirements) { + this(control, feedback, processVariable, () -> setpointDealer, onArrival, requirements); + } + + public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, SetpointProvider> setpointDealer, Subsystem... requirements) { this(control, feedback, processVariable, () -> setpointDealer, requirements); } @@ -46,9 +64,15 @@ public void initialize() { @Override public void execute() { - Pair setpoints = this.setpointDealer.apply(getElapsedTime()); - double setpoint = setpoints.getFirst(); - double setpoint_dt = setpoints.getSecond(); + Pair setpoints; + try { + setpoints = this.setpointDealer.apply(getElapsedTime()); + setpoint = setpoints.getFirst(); + setpoint_dt = setpoints.getSecond(); + + } catch (EndSignal e) { + onArrival.schedule(); + } control.updateSetpoint(setpoint, setpoint_dt); double controlEffort = control.calculate(feedback.getAsDouble(), getElapsedTime()); @@ -56,13 +80,13 @@ public void execute() { } @Override - public boolean isFinished() { - return false; - } + public boolean isFinished() { return false; } - @Override - public void end(boolean interrupted) { - // FIXME Auto-generated method stub - super.end(interrupted); + @FunctionalInterface + public interface SetpointProvider { + public class EndSignal extends Throwable {} + + public R apply(double num) throws EndSignal; } + } From 4a57caad5fa601dbe9cab1c1efceb69c09fdecee Mon Sep 17 00:00:00 2001 From: zbuster05 Date: Sat, 13 May 2023 22:49:38 -0500 Subject: [PATCH 134/134] Changed ezMotion formatting --- custom/motioncontrollers/ezMotion.java | 48 +++++++++++++++++--------- 1 file changed, 31 insertions(+), 17 deletions(-) diff --git a/custom/motioncontrollers/ezMotion.java b/custom/motioncontrollers/ezMotion.java index b2735dab..d64a8a4d 100644 --- a/custom/motioncontrollers/ezMotion.java +++ b/custom/motioncontrollers/ezMotion.java @@ -4,7 +4,7 @@ import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import org.usfirst.frc4904.standard.custom.motioncontrollers.ezMotion.SetpointProvider.EndSignal; +import org.usfirst.frc4904.standard.custom.motioncontrollers.ezMotion.SetpointSupplier.EndSignal; import edu.wpi.first.math.Pair; import edu.wpi.first.wpilibj.Timer; @@ -22,12 +22,18 @@ public class ezMotion extends CommandBase { private double setpoint; private double setpoint_dt; - public Supplier>> setpointDealerDealer; - public SetpointProvider> setpointDealer = null; + public Supplier>> setpointDealerDealer; + public SetpointSupplier> setpointDealer = null; public Command onArrival; - public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, Supplier>> setpointDealerDealer, Command onArrival, Subsystem... requirements) { + public ezMotion(ezControl control, + DoubleSupplier feedback, + DoubleConsumer processVariable, + Supplier>> setpointDealerDealer, + Command onArrival, + Subsystem... requirements) { + addRequirements(requirements); this.control = control; this.processVariable = processVariable; @@ -36,17 +42,26 @@ public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer proce this.onArrival = onArrival != null? (onArrival) : (new InstantCommand(() -> {})); } - public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, Supplier>> setpointDealerDealer, Subsystem... requirements) { - this(control, feedback, processVariable, setpointDealerDealer, new InstantCommand(() -> {}), requirements); - } - - public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, SetpointProvider> setpointDealer, Command onArrival, Subsystem... requirements) { - this(control, feedback, processVariable, () -> setpointDealer, onArrival, requirements); - } - - public ezMotion(ezControl control, DoubleSupplier feedback, DoubleConsumer processVariable, SetpointProvider> setpointDealer, Subsystem... requirements) { - this(control, feedback, processVariable, () -> setpointDealer, requirements); - } + public ezMotion(ezControl control, + DoubleSupplier feedback, DoubleConsumer processVariable, + Supplier>> setpointDealerDealer, + Subsystem... requirements) + { this(control, feedback, processVariable, setpointDealerDealer, new InstantCommand(() -> {}), requirements); } + + public ezMotion(ezControl control, + DoubleSupplier feedback, + DoubleConsumer processVariable, + SetpointSupplier> setpointDealer, + Command onArrival, + Subsystem... requirements) + { this(control, feedback, processVariable, () -> setpointDealer, onArrival, requirements); } + + public ezMotion(ezControl control, + DoubleSupplier feedback, + DoubleConsumer processVariable, + SetpointSupplier> setpointDealer, + Subsystem... requirements) + { this(control, feedback, processVariable, () -> setpointDealer, requirements); } public double getElapsedTime() { return Timer.getFPGATimestamp() - initialTimestamp; @@ -83,10 +98,9 @@ public void execute() { public boolean isFinished() { return false; } @FunctionalInterface - public interface SetpointProvider { + public interface SetpointSupplier { public class EndSignal extends Throwable {} public R apply(double num) throws EndSignal; } - }