diff --git a/ThirdPartyNotices.txt b/ThirdPartyNotices.txt index 528de826792..332b99e0e6a 100644 --- a/ThirdPartyNotices.txt +++ b/ThirdPartyNotices.txt @@ -49,6 +49,8 @@ Simd hal/src/main/native/systemcore/simd AdvantageScope fields/src/main/native/resources/org/wpilib/fields/ftc Avahi wpinet/src/main/native/linux/AvahiClient.hpp SDL thirdparty_libsdl repo +A301 driver protocol hal/src/main/native/cpp/A301.cpp + hal/src/main/native/include/wpi/hal/A301.h Additionally, nanopb was modified for use in WPILib. @@ -2055,3 +2057,32 @@ necessary. Here is a sample; alter the names: Moe Ghoul, President of Vice That's all there is to it! + +============================================================================== +A301 driver protocol +============================================================================== +Copyright (c) 2026 REV Robotics + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. +3. Neither the name of REV Robotics nor the names of its contributors may be + used to endorse or promote products derived from this software without + specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/drivers/CMakeLists.txt b/drivers/CMakeLists.txt index 588397968e0..41679984bb7 100644 --- a/drivers/CMakeLists.txt +++ b/drivers/CMakeLists.txt @@ -22,6 +22,7 @@ target_include_directories( install(TARGETS drivers EXPORT drivers) export(TARGETS drivers FILE drivers.cmake NAMESPACE drivers::) +install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/drivers") install(DIRECTORY src/generated/main/native/include/ DESTINATION "${include_dest}/drivers") configure_file(drivers-config.cmake.in ${WPILIB_BINARY_DIR}/drivers-config.cmake) diff --git a/drivers/robotpy_pybind_build_info.bzl b/drivers/robotpy_pybind_build_info.bzl index 80af881dac3..e6610df9311 100644 --- a/drivers/robotpy_pybind_build_info.bzl +++ b/drivers/robotpy_pybind_build_info.bzl @@ -16,6 +16,8 @@ def drivers_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], inclu "--name-transform-known-word", "5V", "--name-transform-known-word", + "A301", + "--name-transform-known-word", "CAN", "--name-transform-known-word", "CPU", @@ -58,6 +60,8 @@ def drivers_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], inclu "--name-transform-known-word", "RIO", "--name-transform-known-word", + "RPM", + "--name-transform-known-word", "SPI", "--name-transform-known-word", "URI", @@ -70,6 +74,32 @@ def drivers_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], inclu ] DRIVERS_HEADER_GEN = [ + struct( + class_name = "A301Error", + yml_file = "semiwrap/A301Error.yml", + header_root = "$(execpath :robotpy-native-wpilib-drivers.copy_headers)", + header_file = "$(execpath :robotpy-native-wpilib-drivers.copy_headers)/wpi/drivers/motor/A301Error.hpp", + tmpl_class_names = [], + trampolines = [], + ), + struct( + class_name = "A301", + yml_file = "semiwrap/A301.yml", + header_root = "$(execpath :robotpy-native-wpilib-drivers.copy_headers)", + header_file = "$(execpath :robotpy-native-wpilib-drivers.copy_headers)/wpi/drivers/motor/A301.hpp", + tmpl_class_names = [], + trampolines = [ + ("wpi::A301", "wpi__A301.hpp"), + ("wpi::A301::FirmwareVersion", "wpi__A301__FirmwareVersion.hpp"), + ("wpi::A301::Faults", "wpi__A301__Faults.hpp"), + ("wpi::A301::Warnings", "wpi__A301__Warnings.hpp"), + ("wpi::A301::BooleanStatusSignal", "wpi__A301__BooleanStatusSignal.hpp"), + ("wpi::A301::DoubleStatusSignal", "wpi__A301__DoubleStatusSignal.hpp"), + ("wpi::A301::FaultsStatusSignal", "wpi__A301__FaultsStatusSignal.hpp"), + ("wpi::A301::WarningsStatusSignal", "wpi__A301__WarningsStatusSignal.hpp"), + ("wpi::A301::GearboxRPMStatusSignal", "wpi__A301__GearboxRPMStatusSignal.hpp"), + ], + ), struct( class_name = "Koors40", yml_file = "semiwrap/Koors40.yml", diff --git a/drivers/src/main/java/org/wpilib/drivers/motor/A301.java b/drivers/src/main/java/org/wpilib/drivers/motor/A301.java new file mode 100644 index 00000000000..d6bef94169d --- /dev/null +++ b/drivers/src/main/java/org/wpilib/drivers/motor/A301.java @@ -0,0 +1,938 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.drivers.motor; + +import java.util.Objects; +import java.util.concurrent.atomic.AtomicBoolean; +import org.wpilib.hardware.bus.CANBusMap; +import org.wpilib.hardware.hal.A301FirmwareVersion; +import org.wpilib.hardware.hal.A301JNI; +import org.wpilib.hardware.hal.A301PeriodicStatus0; +import org.wpilib.hardware.hal.A301PeriodicStatus1; +import org.wpilib.hardware.hal.A301PeriodicStatus2; +import org.wpilib.hardware.hal.A301PeriodicStatus3; +import org.wpilib.hardware.hal.HAL; +import org.wpilib.hardware.motor.MotorController; + +/** High-level interface to a FIRST A301 motor controller. */ +public class A301 implements MotorController, AutoCloseable { + /** The factory-default A301 device ID. */ + public static final int kDefaultDeviceId = A301JNI.DEFAULT_DEVICE_ID; + + /** A periodic status frame produced by the A301. */ + public enum PeriodicFrame { + /** Applied output, voltage, current, temperature, inversion, and gearbox information. */ + STATUS_0(A301JNI.STATUS_0), + /** Fault and warning information. */ + STATUS_1(A301JNI.STATUS_1), + /** Relative encoder position and velocity. */ + STATUS_2(A301JNI.STATUS_2), + /** Absolute encoder position. */ + STATUS_3(A301JNI.STATUS_3); + + private final int m_value; + + PeriodicFrame(int value) { + m_value = value; + } + + /** + * Returns the frame corresponding to a HAL frame ID. + * + * @param id HAL frame ID + * @return the periodic frame + * @throws IllegalArgumentException if the ID is unknown + */ + public static PeriodicFrame fromId(int id) { + return switch (id) { + case A301JNI.STATUS_0 -> STATUS_0; + case A301JNI.STATUS_1 -> STATUS_1; + case A301JNI.STATUS_2 -> STATUS_2; + case A301JNI.STATUS_3 -> STATUS_3; + default -> throw new IllegalArgumentException("Unknown A301 periodic frame ID: " + id); + }; + } + } + + /** A301 gearbox speed variant. */ + public enum GearboxRPM { + /** The gearbox speed is unknown. */ + UNKNOWN, + /** 215 RPM gearbox. */ + RPM_215, + /** 500 RPM gearbox. */ + RPM_500; + + /** + * Returns the gearbox variant corresponding to a HAL value. + * + * @param id HAL gearbox value + * @return the gearbox variant + * @throws IllegalArgumentException if the value is unknown + */ + public static GearboxRPM fromId(int id) { + return switch (id) { + case A301JNI.GEARBOX_RPM_UNKNOWN -> UNKNOWN; + case A301JNI.GEARBOX_RPM_215 -> RPM_215; + case A301JNI.GEARBOX_RPM_500 -> RPM_500; + default -> throw new IllegalArgumentException("Unknown A301 gearbox RPM ID: " + id); + }; + } + } + + /** Neutral behavior when the controller output is zero. */ + public enum IdleMode { + /** Allow the motor to coast. */ + COAST(A301JNI.IDLE_MODE_COAST), + /** Actively brake the motor. */ + BRAKE(A301JNI.IDLE_MODE_BRAKE); + + private final int m_value; + + IdleMode(int value) { + m_value = value; + } + + private static IdleMode fromId(int id) { + return switch (id) { + case A301JNI.IDLE_MODE_COAST -> COAST; + case A301JNI.IDLE_MODE_BRAKE -> BRAKE; + default -> throw new IllegalArgumentException("Unknown A301 idle mode ID: " + id); + }; + } + } + + /** Active or sticky A301 motor faults. */ + public static final class Faults { + /** Whether an uncategorized fault is active. */ + public final boolean other; + + /** Whether a motor-type fault is active. */ + public final boolean motorType; + + /** Whether a sensor fault is active. */ + public final boolean sensor; + + /** Whether a CAN communication fault is active. */ + public final boolean can; + + /** Whether a temperature fault is active. */ + public final boolean temperature; + + /** Whether a gate-driver fault is active. */ + public final boolean gateDriver; + + /** Whether an ESC EEPROM fault is active. */ + public final boolean escEeprom; + + /** Whether a firmware fault is active. */ + public final boolean firmware; + + /** Whether a motor-startup fault is active. */ + public final boolean motorStartup; + + /** Packed fault bits reported by the controller. */ + public final int rawBits; + + private Faults(int rawBits) { + this.rawBits = rawBits; + other = getBit(rawBits, 0); + motorType = getBit(rawBits, 1); + sensor = getBit(rawBits, 2); + can = getBit(rawBits, 3); + temperature = getBit(rawBits, 4); + gateDriver = getBit(rawBits, 5); + escEeprom = getBit(rawBits, 6); + firmware = getBit(rawBits, 7); + motorStartup = getBit(rawBits, 8); + } + } + + /** Active or sticky A301 motor warnings. */ + public static final class Warnings { + /** Whether a brownout warning is active. */ + public final boolean brownout; + + /** Whether an overcurrent warning is active. */ + public final boolean overcurrent; + + /** Whether an ESC EEPROM warning is active. */ + public final boolean escEeprom; + + /** Whether an external EEPROM warning is active. */ + public final boolean extEeprom; + + /** Whether a sensor warning is active. */ + public final boolean sensor; + + /** Whether a stall warning is active. */ + public final boolean stall; + + /** Whether a controller-reset warning is active. */ + public final boolean hasReset; + + /** Whether an uncategorized warning is active. */ + public final boolean other; + + /** Whether an overvoltage warning is active. */ + public final boolean overvoltage; + + /** Whether a motor-loop-speed warning is active. */ + public final boolean motorLoopSpeed; + + /** Packed warning bits reported by the controller. */ + public final int rawBits; + + private Warnings(int rawBits) { + this.rawBits = rawBits; + brownout = getBit(rawBits, 0); + overcurrent = getBit(rawBits, 1); + escEeprom = getBit(rawBits, 2); + extEeprom = getBit(rawBits, 3); + sensor = getBit(rawBits, 4); + stall = getBit(rawBits, 5); + hasReset = getBit(rawBits, 6); + other = getBit(rawBits, 7); + overvoltage = getBit(rawBits, 8); + motorLoopSpeed = getBit(rawBits, 9); + } + } + + private enum ControlType { + DUTY_CYCLE(A301JNI.CONTROL_TYPE_DUTY_CYCLE), + VELOCITY(A301JNI.CONTROL_TYPE_VELOCITY), + VOLTAGE(A301JNI.CONTROL_TYPE_VOLTAGE), + RELATIVE_POSITION(A301JNI.CONTROL_TYPE_RELATIVE_POSITION), + ABSOLUTE_POSITION(A301JNI.CONTROL_TYPE_ABSOLUTE_POSITION), + CURRENT(A301JNI.CONTROL_TYPE_CURRENT); + + private final int m_value; + + ControlType(int value) { + m_value = value; + } + } + + private static final double kDefaultPositionSpeed = 0.0; + + private final int m_handle; + private final int m_busId; + private final int m_deviceId; + private final AtomicBoolean m_isClosed = new AtomicBoolean(); + private volatile double m_setpoint; + private volatile String m_firmwareString; + + /** + * Constructs an A301 using its factory-default device ID. + * + *

On a Motioncore CAN bus, the connected A301 device ID is automatically detected when + * possible. + * + * @param bus the CAN bus + */ + public A301(CANBusMap bus) { + this(bus, kDefaultDeviceId); + } + + /** + * Constructs an A301. + * + *

On a Motioncore CAN bus, the connected A301 device ID is automatically detected when + * possible and the supplied device ID is used as a fallback. + * + * @param bus the CAN bus + * @param deviceId the requested device ID + */ + public A301(CANBusMap bus, int deviceId) { + m_handle = A301JNI.initialize(Objects.requireNonNull(bus, "bus").value, deviceId); + m_busId = A301JNI.getBusId(m_handle); + m_deviceId = A301JNI.getDeviceId(m_handle); + HAL.reportUsage("A301[" + m_busId + "][" + m_deviceId + "]", ""); + } + + /** Releases the native A301 handle. */ + @Override + public void close() { + if (m_isClosed.compareAndSet(false, true)) { + A301JNI.free(m_handle); + } + } + + /** + * Returns the CAN bus ID. + * + * @return CAN bus ID + */ + public int getBusId() { + throwIfClosed(); + return m_busId; + } + + /** + * Returns the resolved device ID. This may differ from the requested ID after automatic detection + * on a Motioncore bus. + * + * @return resolved device ID + */ + public int getDeviceId() { + throwIfClosed(); + return m_deviceId; + } + + /** + * Returns the raw firmware version as {@code major.minor.patch} packed into an integer. + * + * @return raw firmware version + */ + public int getFirmwareVersion() { + throwIfClosed(); + return A301JNI.getFirmwareVersion(m_handle).raw; + } + + /** + * Returns the firmware version as a human-readable string. + * + * @return firmware version in {@code vMajor.Minor.Patch} form + */ + public String getFirmwareString() { + throwIfClosed(); + String result = m_firmwareString; + if (result == null) { + A301FirmwareVersion version = A301JNI.getFirmwareVersion(m_handle); + result = "v" + version.major + "." + version.minor + "." + version.patch; + m_firmwareString = result; + } + return result; + } + + /** + * Returns whether one or more active faults are present. + * + * @return active-fault signal + */ + public A301StatusSignal hasActiveFault() { + return getFaults().map(faults -> faults.rawBits != 0); + } + + /** + * Returns whether one or more sticky faults are present. + * + * @return sticky-fault signal + */ + public A301StatusSignal hasStickyFault() { + return getStickyFaults().map(faults -> faults.rawBits != 0); + } + + /** + * Returns whether one or more active warnings are present. + * + * @return active-warning signal + */ + public A301StatusSignal hasActiveWarning() { + return getWarnings().map(warnings -> warnings.rawBits != 0); + } + + /** + * Returns whether one or more sticky warnings are present. + * + * @return sticky-warning signal + */ + public A301StatusSignal hasStickyWarning() { + return getStickyWarnings().map(warnings -> warnings.rawBits != 0); + } + + /** + * Returns active motor faults. + * + * @return active-fault signal + */ + public A301StatusSignal getFaults() { + return getPeriodicStatus1().map(frame -> new Faults(frame.faults)); + } + + /** + * Returns sticky motor faults recorded since they were last cleared. + * + * @return sticky-fault signal + */ + public A301StatusSignal getStickyFaults() { + return getPeriodicStatus1().map(frame -> new Faults(frame.stickyFaults)); + } + + /** + * Returns active motor warnings. + * + * @return active-warning signal + */ + public A301StatusSignal getWarnings() { + return getPeriodicStatus1().map(frame -> new Warnings(frame.warnings)); + } + + /** + * Returns sticky motor warnings recorded since they were last cleared. + * + * @return sticky-warning signal + */ + public A301StatusSignal getStickyWarnings() { + return getPeriodicStatus1().map(frame -> new Warnings(frame.stickyWarnings)); + } + + /** + * Clears active and sticky faults and warnings. + * + * @return command status + */ + public A301Error clearFaults() { + throwIfClosed(); + return A301Error.fromHalStatus(A301JNI.clearFaults(m_handle)); + } + + /** + * Returns the controller input voltage in volts. + * + * @return bus-voltage signal + */ + public A301StatusSignal getBusVoltage() { + return getPeriodicStatus0().map(frame -> frame.voltage); + } + + /** + * Returns the applied output duty cycle. + * + * @return applied-output signal + */ + public A301StatusSignal getAppliedOutput() { + return getPeriodicStatus0().map(frame -> frame.appliedOutput); + } + + /** + * Returns motor current in amperes. + * + * @return motor-current signal + */ + public A301StatusSignal getMotorCurrent() { + return getPeriodicStatus0().map(frame -> frame.current); + } + + /** + * Returns motor temperature in degrees Celsius. + * + * @return motor-temperature signal + */ + public A301StatusSignal getMotorTemperature() { + return getPeriodicStatus0().map(frame -> (double) frame.motorTemperature); + } + + /** + * Returns the attached gearbox speed variant. + * + * @return gearbox signal + */ + public A301StatusSignal getGearboxRPM() { + return getPeriodicStatus0().map(frame -> GearboxRPM.fromId(frame.gearboxRPM)); + } + + /** + * Returns relative encoder position in motor rotations. + * + * @return relative-position signal + */ + public A301StatusSignal getRelativeEncoderPosition() { + return getPeriodicStatus2().map(frame -> frame.relativeEncoderPosition); + } + + /** + * Returns relative encoder velocity in RPM. + * + * @return encoder-velocity signal + */ + public A301StatusSignal getEncoderVelocity() { + return getPeriodicStatus2().map(frame -> frame.encoderVelocity); + } + + /** + * Returns absolute encoder position in rotations. + * + * @return absolute-position signal + */ + public A301StatusSignal getAbsoluteEncoderPosition() { + return getPeriodicStatus3().map(frame -> frame.absoluteEncoderPosition); + } + + /** + * Sets relative encoder position in motor rotations. + * + * @param position position in rotations + * @return command status + */ + public A301Error setRelativeEncoderPosition(double position) { + throwIfClosed(); + return A301Error.fromHalStatus(A301JNI.setRelativeEncoderPosition(m_handle, position)); + } + + /** + * Sets absolute encoder position in rotations. + * + * @param position position in rotations + * @return command status + */ + public A301Error setAbsoluteEncoderPosition(double position) { + throwIfClosed(); + return A301Error.fromHalStatus(A301JNI.setAbsoluteEncoderPosition(m_handle, position)); + } + + /** + * Sets a velocity setpoint in RPM. + * + * @param velocity velocity in RPM + * @return command status + */ + public A301Error setVelocity(double velocity) { + return setSetpoint(ControlType.VELOCITY, velocity); + } + + /** + * Sets a relative position setpoint using maximum speed. + * + * @param position position in motor rotations + * @return command status + */ + public A301Error setRelativePosition(double position) { + return setSetpoint(ControlType.RELATIVE_POSITION, position); + } + + /** + * Sets a relative position setpoint with a specific approach speed. + * + *

A speed less than or equal to zero requests maximum speed. + * + * @param position position in motor rotations + * @param speed approach speed in RPM + * @return command status + */ + public A301Error setRelativePositionWithSpeed(double position, double speed) { + return setSetpoint(ControlType.RELATIVE_POSITION, position, speed); + } + + /** + * Sets an absolute position setpoint using maximum speed. + * + * @param position absolute position in rotations + * @return command status + */ + public A301Error setAbsolutePosition(double position) { + return setSetpoint(ControlType.ABSOLUTE_POSITION, position); + } + + /** + * Sets an absolute position setpoint with a specific approach speed. + * + *

A speed less than or equal to zero requests maximum speed. + * + * @param position absolute position in rotations + * @param speed approach speed in RPM + * @return command status + */ + public A301Error setAbsolutePositionWithSpeed(double position, double speed) { + return setSetpoint(ControlType.ABSOLUTE_POSITION, position, speed); + } + + /** + * Sets a motor-current setpoint. + * + * @param current current in amperes + * @return command status + */ + public A301Error setCurrent(double current) { + return setSetpoint(ControlType.CURRENT, current); + } + + /** + * Sets the idle mode. + * + * @param idleMode idle mode + * @return command status + */ + public A301Error setIdleMode(IdleMode idleMode) { + throwIfClosed(); + return A301Error.fromHalStatus( + A301JNI.setIdleMode(m_handle, Objects.requireNonNull(idleMode, "idleMode").m_value)); + } + + /** + * Returns the configured idle mode. + * + * @return idle mode + */ + public IdleMode getIdleMode() { + throwIfClosed(); + return IdleMode.fromId(A301JNI.getIdleMode(m_handle)); + } + + /** + * Enables continuous input for absolute position control. + * + * @return command status + */ + public A301Error enableAbsolutePositionContinuousInput() { + return setAbsolutePositionContinuousInput(true); + } + + /** + * Disables continuous input for absolute position control. + * + * @return command status + */ + public A301Error disableAbsolutePositionContinuousInput() { + return setAbsolutePositionContinuousInput(false); + } + + /** + * Returns whether continuous input is enabled for absolute position control. + * + * @return true when continuous input is enabled + */ + public boolean isAbsolutePositionContinuousInputEnabled() { + throwIfClosed(); + return A301JNI.getAbsolutePositionContinuousInput(m_handle); + } + + /** + * Sets the absolute encoder range offset. + * + * @param offset offset in rotations, from -0.5 to 0.5 + * @return command status + */ + public A301Error setAbsoluteEncoderRangeOffset(double offset) { + throwIfClosed(); + return A301Error.fromHalStatus(A301JNI.setAbsoluteEncoderRangeOffset(m_handle, offset)); + } + + /** + * Returns the absolute encoder range offset. + * + * @return range offset in rotations + */ + public double getAbsoluteEncoderRangeOffset() { + throwIfClosed(); + return A301JNI.getAbsoluteEncoderRangeOffset(m_handle); + } + + /** + * Sets the controller duty cycle. + * + * @param throttle duty cycle, conventionally from -1.0 to 1.0 + */ + @Override + public void setThrottle(double throttle) { + throwIfClosed(); + m_setpoint = throttle; + setSetpoint(ControlType.DUTY_CYCLE, throttle); + } + + /** + * Returns the last commanded duty cycle. + * + * @return last commanded duty cycle + */ + @Override + public double getThrottle() { + throwIfClosed(); + return m_setpoint; + } + + /** + * Sets the controller's internal voltage setpoint. + * + *

Unlike the default {@link MotorController} implementation, the A301 regulates this voltage + * internally, so this is a set-and-forget command. + * + * @param outputVolts voltage setpoint in volts + */ + @Override + public void setVoltage(double outputVolts) { + throwIfClosed(); + m_setpoint = outputVolts / 12.0; + setSetpoint(ControlType.VOLTAGE, outputVolts); + } + + /** + * Sets whether non-position control output is inverted. + * + * @param isInverted true to invert the output + */ + @Override + public void setInverted(boolean isInverted) { + throwIfClosed(); + A301JNI.setInverted(m_handle, isInverted); + } + + /** + * Returns whether non-position control output is inverted. + * + * @return true when the output is inverted + */ + @Override + public boolean getInverted() { + throwIfClosed(); + return A301JNI.getInverted(m_handle); + } + + /** Stops the motor output. */ + @Override + public void disable() { + throwIfClosed(); + m_setpoint = 0.0; + setSetpoint(ControlType.DUTY_CYCLE, 0.0); + } + + /** + * Sets a periodic status frame period. + * + *

The controller may normalize the requested period. Use {@link #getStatusFramePeriod} to + * retrieve the effective value. + * + * @param frame periodic status frame + * @param periodMs requested period in milliseconds, from 0 to 1000 + * @return this object for method chaining + */ + public A301 setStatusFramePeriod(PeriodicFrame frame, int periodMs) { + throwIfClosed(); + A301JNI.setStatusFramePeriod( + m_handle, Objects.requireNonNull(frame, "frame").m_value, periodMs); + return this; + } + + /** + * Returns the effective periodic status frame period. + * + * @param frame periodic status frame + * @return period in milliseconds + */ + public int getStatusFramePeriod(PeriodicFrame frame) { + throwIfClosed(); + return A301JNI.getStatusFramePeriod(m_handle, Objects.requireNonNull(frame, "frame").m_value); + } + + /** + * Sets the fault signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + public A301 faultsPeriodMs(int periodMs) { + return setStatusFramePeriod(PeriodicFrame.STATUS_1, periodMs); + } + + /** + * Returns the effective fault signal period. + * + * @return period in milliseconds + */ + public int getFaultsPeriodMs() { + return getStatusFramePeriod(PeriodicFrame.STATUS_1); + } + + /** + * Sets the warning signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + public A301 warningsPeriodMs(int periodMs) { + return setStatusFramePeriod(PeriodicFrame.STATUS_1, periodMs); + } + + /** + * Returns the effective warning signal period. + * + * @return period in milliseconds + */ + public int getWarningsPeriodMs() { + return getStatusFramePeriod(PeriodicFrame.STATUS_1); + } + + /** + * Sets the bus-voltage signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + public A301 busVoltagePeriodMs(int periodMs) { + return setStatusFramePeriod(PeriodicFrame.STATUS_0, periodMs); + } + + /** + * Returns the effective bus-voltage signal period. + * + * @return period in milliseconds + */ + public int getBusVoltagePeriodMs() { + return getStatusFramePeriod(PeriodicFrame.STATUS_0); + } + + /** + * Sets the applied-output signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + public A301 appliedOutputPeriodMs(int periodMs) { + return setStatusFramePeriod(PeriodicFrame.STATUS_0, periodMs); + } + + /** + * Returns the effective applied-output signal period. + * + * @return period in milliseconds + */ + public int getAppliedOutputPeriodMs() { + return getStatusFramePeriod(PeriodicFrame.STATUS_0); + } + + /** + * Sets the motor-current signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + public A301 motorCurrentPeriodMs(int periodMs) { + return setStatusFramePeriod(PeriodicFrame.STATUS_0, periodMs); + } + + /** + * Returns the effective motor-current signal period. + * + * @return period in milliseconds + */ + public int getMotorCurrentPeriodMs() { + return getStatusFramePeriod(PeriodicFrame.STATUS_0); + } + + /** + * Sets the motor-temperature signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + public A301 motorTemperaturePeriodMs(int periodMs) { + return setStatusFramePeriod(PeriodicFrame.STATUS_0, periodMs); + } + + /** + * Returns the effective motor-temperature signal period. + * + * @return period in milliseconds + */ + public int getMotorTemperaturePeriodMs() { + return getStatusFramePeriod(PeriodicFrame.STATUS_0); + } + + /** + * Sets the relative-encoder-position signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + public A301 relativeEncoderPositionPeriodMs(int periodMs) { + return setStatusFramePeriod(PeriodicFrame.STATUS_2, periodMs); + } + + /** + * Returns the effective relative-encoder-position signal period. + * + * @return period in milliseconds + */ + public int getRelativeEncoderPositionPeriodMs() { + return getStatusFramePeriod(PeriodicFrame.STATUS_2); + } + + /** + * Sets the encoder-velocity signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + public A301 encoderVelocityPeriodMs(int periodMs) { + return setStatusFramePeriod(PeriodicFrame.STATUS_2, periodMs); + } + + /** + * Returns the effective encoder-velocity signal period. + * + * @return period in milliseconds + */ + public int getEncoderVelocityPeriodMs() { + return getStatusFramePeriod(PeriodicFrame.STATUS_2); + } + + /** + * Sets the absolute-encoder-position signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + public A301 absoluteEncoderPositionPeriodMs(int periodMs) { + return setStatusFramePeriod(PeriodicFrame.STATUS_3, periodMs); + } + + /** + * Returns the effective absolute-encoder-position signal period. + * + * @return period in milliseconds + */ + public int getAbsoluteEncoderPositionPeriodMs() { + return getStatusFramePeriod(PeriodicFrame.STATUS_3); + } + + private static boolean getBit(int value, int bit) { + return (value & (1 << bit)) != 0; + } + + private A301Error setAbsolutePositionContinuousInput(boolean enabled) { + throwIfClosed(); + return A301Error.fromHalStatus(A301JNI.setAbsolutePositionContinuousInput(m_handle, enabled)); + } + + private A301StatusSignal getPeriodicStatus0() { + throwIfClosed(); + A301PeriodicStatus0 frame = A301JNI.getPeriodicStatus0(m_handle); + return A301StatusSignal.of(frame, frame.status, frame.timestamp); + } + + private A301StatusSignal getPeriodicStatus1() { + throwIfClosed(); + A301PeriodicStatus1 frame = A301JNI.getPeriodicStatus1(m_handle); + return A301StatusSignal.of(frame, frame.status, frame.timestamp); + } + + private A301StatusSignal getPeriodicStatus2() { + throwIfClosed(); + A301PeriodicStatus2 frame = A301JNI.getPeriodicStatus2(m_handle); + return A301StatusSignal.of(frame, frame.status, frame.timestamp); + } + + private A301StatusSignal getPeriodicStatus3() { + throwIfClosed(); + A301PeriodicStatus3 frame = A301JNI.getPeriodicStatus3(m_handle); + return A301StatusSignal.of(frame, frame.status, frame.timestamp); + } + + private A301Error setSetpoint(ControlType controlType, double setpoint) { + return setSetpoint(controlType, setpoint, kDefaultPositionSpeed); + } + + private A301Error setSetpoint(ControlType controlType, double setpoint, double positionSpeed) { + throwIfClosed(); + return A301Error.fromHalStatus( + A301JNI.setSetpoint(m_handle, setpoint, controlType.m_value, positionSpeed)); + } + + private void throwIfClosed() { + if (m_isClosed.get()) { + throw new IllegalStateException("This FIRST A301 object has been closed"); + } + } +} diff --git a/drivers/src/main/java/org/wpilib/drivers/motor/A301Error.java b/drivers/src/main/java/org/wpilib/drivers/motor/A301Error.java new file mode 100644 index 00000000000..bd9d1e4dba7 --- /dev/null +++ b/drivers/src/main/java/org/wpilib/drivers/motor/A301Error.java @@ -0,0 +1,87 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.drivers.motor; + +import org.wpilib.hardware.hal.HALUtil; + +/** REVLib-compatible error categories returned by high-level A301 commands. */ +public enum A301Error { + /** No error occurred. */ + kOk(0), + /** A general controller error occurred. */ + kError(1), + /** The operation timed out. */ + kTimeout(2), + /** The requested operation is not implemented. */ + kNotImplemented(3), + /** The HAL returned an error. */ + kHALError(4), + /** The controller firmware could not be read. */ + kCantFindFirmware(5), + /** The controller firmware is too old. */ + kFirmwareTooOld(6), + /** The controller firmware is newer than this driver supports. */ + kFirmwareTooNew(7), + /** The parameter ID is invalid. */ + kParamInvalidID(8), + /** The parameter type does not match the expected type. */ + kParamMismatchType(9), + /** The parameter does not support the requested access mode. */ + kParamAccessMode(10), + /** The parameter value is invalid. */ + kParamInvalid(11), + /** The parameter is deprecated or not implemented. */ + kParamNotImplementedDeprecated(12), + /** The follower configuration does not match its leader. */ + kFollowConfigMismatch(13), + /** The returned error value is invalid. */ + kInvalid(14), + /** The requested setpoint is outside the accepted range. */ + kSetpointOutOfRange(15), + /** An unknown controller error occurred. */ + kUnknown(16), + /** The controller is disconnected from the CAN bus. */ + kCANDisconnected(17), + /** Another device is using the requested CAN ID. */ + kDuplicateCANId(18), + /** The requested CAN ID is invalid. */ + kInvalidCANId(19); + + /** REVLib-compatible numeric value. */ + @SuppressWarnings("MemberName") + public final int value; + + A301Error(int value) { + this.value = value; + } + + /** + * Returns the error corresponding to a REVLib-compatible numeric value. + * + * @param value numeric error value + * @return corresponding error, or {@link #kInvalid} when unknown + */ + public static A301Error fromInt(int value) { + for (A301Error error : values()) { + if (error.value == value) { + return error; + } + } + return kInvalid; + } + + static A301Error fromHalStatus(int status) { + return switch (status) { + case 0 -> kOk; + case -1154, -44087 -> kTimeout; + case HALUtil.PARAMETER_OUT_OF_RANGE -> kParamInvalid; + case -1029 -> kDuplicateCANId; + case -1030 -> kInvalidCANId; + case HALUtil.INCOMPATIBLE_STATE -> kError; + case -35007 -> kCANDisconnected; + default -> kHALError; + }; + } +} diff --git a/drivers/src/main/java/org/wpilib/drivers/motor/A301StatusSignal.java b/drivers/src/main/java/org/wpilib/drivers/motor/A301StatusSignal.java new file mode 100644 index 00000000000..a97dc93ddea --- /dev/null +++ b/drivers/src/main/java/org/wpilib/drivers/motor/A301StatusSignal.java @@ -0,0 +1,100 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.drivers.motor; + +import java.util.Objects; +import java.util.function.Function; + +/** + * A value read from an A301 periodic status frame. + * + * @param value type + */ +public final class A301StatusSignal { + private final T m_value; + private final int m_status; + private final long m_timestamp; + + private A301StatusSignal(T value, int status, long timestamp) { + m_value = value; + m_status = status; + m_timestamp = timestamp; + } + + /** + * Returns the most recently received value. + * + *

The value may be stale or may have been produced by a failed read. Call {@link #isValid()} + * when freshness matters. + * + * @return the most recently received value + */ + public T get() { + return m_value; + } + + /** + * Returns the most recently received value when valid, or a default value otherwise. + * + * @param defaultValue the value to return when this signal is invalid + * @return the received value or {@code defaultValue} + */ + public T get(T defaultValue) { + return isValid() ? m_value : defaultValue; + } + + /** + * Returns whether the value was read without an error and is recent enough for its configured + * status-frame period. + * + * @return true when the value is valid + */ + public boolean isValid() { + return getError() == A301Error.kOk; + } + + /** + * Returns the REVLib-compatible error associated with the read. + * + * @return the signal error + */ + public A301Error getError() { + return A301Error.fromHalStatus(m_status); + } + + /** + * Returns the HAL status associated with the read. + * + * @return zero on success, or a HAL error code + */ + public int getStatus() { + return m_status; + } + + /** + * Returns the timestamp of the underlying CAN status frame in microseconds. + * + * @return the frame timestamp + */ + public long getTimestamp() { + return m_timestamp; + } + + /** + * Maps this signal's value while retaining its status and timestamp. + * + * @param mapper function that maps the current value + * @param mapped value type + * @return a mapped signal with the same status and timestamp + */ + public A301StatusSignal map(Function mapper) { + return new A301StatusSignal<>( + Objects.requireNonNull(mapper, "mapper").apply(m_value), m_status, m_timestamp); + } + + static A301StatusSignal of(T value, int status, long timestamp) { + return new A301StatusSignal<>(value, status, timestamp); + } +} diff --git a/drivers/src/main/native/cpp/motor/A301.cpp b/drivers/src/main/native/cpp/motor/A301.cpp new file mode 100644 index 00000000000..cb2aff3c7a1 --- /dev/null +++ b/drivers/src/main/native/cpp/motor/A301.cpp @@ -0,0 +1,465 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/drivers/motor/A301.hpp" + +#include +#include + +#include "wpi/hal/UsageReporting.hpp" +#include "wpi/system/Errors.hpp" +#include "wpi/util/StackTrace.hpp" + +using namespace wpi; + +namespace { + +constexpr uint16_t BitMask(int offset) { + return static_cast(1u << offset); +} + +uint16_t PackFaults(const HAL_A301PeriodicStatus1& frame, bool sticky) { + if (sticky) { + return frame.otherStickyFault | (frame.motorTypeStickyFault << 1) | + (frame.sensorStickyFault << 2) | (frame.canStickyFault << 3) | + (frame.temperatureStickyFault << 4) | (frame.drvStickyFault << 5) | + (frame.escEepromStickyFault << 6) | + (frame.firmwareStickyFault << 7) | + (frame.motorStartupStickyFault << 8); + } + return frame.otherFault | (frame.motorTypeFault << 1) | + (frame.sensorFault << 2) | (frame.canFault << 3) | + (frame.temperatureFault << 4) | (frame.drvFault << 5) | + (frame.escEepromFault << 6) | (frame.firmwareFault << 7) | + (frame.motorStartupFault << 8); +} + +uint16_t PackWarnings(const HAL_A301PeriodicStatus1& frame, bool sticky) { + if (sticky) { + return frame.brownoutStickyWarning | (frame.overcurrentStickyWarning << 1) | + (frame.escEepromStickyWarning << 2) | + (frame.extEepromStickyWarning << 3) | + (frame.sensorStickyWarning << 4) | (frame.stallStickyWarning << 5) | + (frame.hasResetStickyWarning << 6) | + (frame.otherStickyWarning << 7) | + (frame.overvoltageStickyWarning << 8) | + (frame.motorLoopSpeedStickyWarning << 9); + } + return frame.brownoutWarning | (frame.overcurrentWarning << 1) | + (frame.escEepromWarning << 2) | (frame.extEepromWarning << 3) | + (frame.sensorWarning << 4) | (frame.stallWarning << 5) | + (frame.hasResetWarning << 6) | (frame.otherWarning << 7) | + (frame.overvoltageWarning << 8) | (frame.motorLoopSpeedWarning << 9); +} + +} // namespace + +A301::Faults::Faults(uint16_t faults) : rawBits{faults} { + other = (faults & BitMask(0)) != 0; + motorType = (faults & BitMask(1)) != 0; + sensor = (faults & BitMask(2)) != 0; + can = (faults & BitMask(3)) != 0; + temperature = (faults & BitMask(4)) != 0; + gateDriver = (faults & BitMask(5)) != 0; + escEeprom = (faults & BitMask(6)) != 0; + firmware = (faults & BitMask(7)) != 0; + motorStartup = (faults & BitMask(8)) != 0; +} + +A301::Warnings::Warnings(uint16_t warnings) : rawBits{warnings} { + brownout = (warnings & BitMask(0)) != 0; + overcurrent = (warnings & BitMask(1)) != 0; + escEeprom = (warnings & BitMask(2)) != 0; + extEeprom = (warnings & BitMask(3)) != 0; + sensor = (warnings & BitMask(4)) != 0; + stall = (warnings & BitMask(5)) != 0; + hasReset = (warnings & BitMask(6)) != 0; + other = (warnings & BitMask(7)) != 0; + overvoltage = (warnings & BitMask(8)) != 0; + motorLoopSpeed = (warnings & BitMask(9)) != 0; +} + +A301::A301(CANBusMap bus) : A301{bus, kDefaultDeviceId} {} + +A301::A301(CANBusMap bus, int deviceId) { + auto stack = wpi::util::GetStackTrace(1); + int32_t status = 0; + m_handle = HAL_InitializeA301(static_cast(bus), deviceId, stack.c_str(), + &status); + WPILIB_CheckErrorStatus(status, "A301 bus {} device {}", + static_cast(bus), deviceId); + + m_busId = HAL_GetA301BusId(m_handle, &status); + WPILIB_CheckErrorStatus(status, "A301 bus ID"); + m_deviceId = HAL_GetA301DeviceId(m_handle, &status); + WPILIB_CheckErrorStatus(status, "A301 device ID"); + + HAL_ReportUsage(std::format("A301[{}][{}]", m_busId, m_deviceId), ""); +} + +int A301::GetBusId() const { + return m_busId; +} + +int A301::GetDeviceId() const { + return m_deviceId; +} + +A301::FirmwareVersion A301::GetFirmwareVersion() const { + HAL_A301FirmwareVersion version{}; + int32_t status = 0; + HAL_GetA301FirmwareVersion(m_handle, &version, &status); + WPILIB_ReportError(status, "A301 firmware version"); + return {.major = version.major, + .minor = version.minor, + .patch = version.patch, + .prerelease = version.prerelease, + .hardwareRevision = version.hardwareRevision, + .raw = version.raw}; +} + +std::string A301::GetFirmwareString() const { + auto version = GetFirmwareVersion(); + if (version.prerelease != 0) { + return std::format("v{}.{}.{} {} Debug Build", version.major, version.minor, + version.patch, version.prerelease); + } + return std::format("v{}.{}.{}", version.major, version.minor, version.patch); +} + +A301::BooleanStatusSignal A301::HasActiveFault() const { + auto signal = GetFaults(); + return {signal.Get().rawBits != 0, signal.GetStatus(), signal.GetTimestamp()}; +} + +A301::BooleanStatusSignal A301::HasStickyFault() const { + auto signal = GetStickyFaults(); + return {signal.Get().rawBits != 0, signal.GetStatus(), signal.GetTimestamp()}; +} + +A301::BooleanStatusSignal A301::HasActiveWarning() const { + auto signal = GetWarnings(); + return {signal.Get().rawBits != 0, signal.GetStatus(), signal.GetTimestamp()}; +} + +A301::BooleanStatusSignal A301::HasStickyWarning() const { + auto signal = GetStickyWarnings(); + return {signal.Get().rawBits != 0, signal.GetStatus(), signal.GetTimestamp()}; +} + +A301::FaultsStatusSignal A301::GetFaults() const { + auto signal = GetPeriodicStatus1(); + return {Faults{PackFaults(signal.Get(), false)}, signal.GetStatus(), + signal.GetTimestamp()}; +} + +A301::FaultsStatusSignal A301::GetStickyFaults() const { + auto signal = GetPeriodicStatus1(); + return {Faults{PackFaults(signal.Get(), true)}, signal.GetStatus(), + signal.GetTimestamp()}; +} + +A301::WarningsStatusSignal A301::GetWarnings() const { + auto signal = GetPeriodicStatus1(); + return {Warnings{PackWarnings(signal.Get(), false)}, signal.GetStatus(), + signal.GetTimestamp()}; +} + +A301::WarningsStatusSignal A301::GetStickyWarnings() const { + auto signal = GetPeriodicStatus1(); + return {Warnings{PackWarnings(signal.Get(), true)}, signal.GetStatus(), + signal.GetTimestamp()}; +} + +A301Error A301::ClearFaults() { + int32_t status = 0; + HAL_ClearA301Faults(m_handle, &status); + return detail::A301ErrorFromHalStatus(status); +} + +A301::DoubleStatusSignal A301::GetBusVoltage() const { + auto signal = GetPeriodicStatus0(); + return {signal.Get().voltage, signal.GetStatus(), signal.GetTimestamp()}; +} + +A301::DoubleStatusSignal A301::GetAppliedOutput() const { + auto signal = GetPeriodicStatus0(); + return {signal.Get().appliedOutput, signal.GetStatus(), + signal.GetTimestamp()}; +} + +A301::DoubleStatusSignal A301::GetMotorCurrent() const { + auto signal = GetPeriodicStatus0(); + return {signal.Get().current, signal.GetStatus(), signal.GetTimestamp()}; +} + +A301::DoubleStatusSignal A301::GetMotorTemperature() const { + auto signal = GetPeriodicStatus0(); + return {static_cast(signal.Get().motorTemperature), + signal.GetStatus(), signal.GetTimestamp()}; +} + +A301::GearboxRPMStatusSignal A301::GetGearboxRPM() const { + auto signal = GetPeriodicStatus0(); + GearboxRPM rpm = GearboxRPM::kUnknown; + if (signal.Get().gearboxRPM == HAL_A301_GEARBOX_RPM_215) { + rpm = GearboxRPM::kRPM215; + } else if (signal.Get().gearboxRPM == HAL_A301_GEARBOX_RPM_500) { + rpm = GearboxRPM::kRPM500; + } + return {rpm, signal.GetStatus(), signal.GetTimestamp()}; +} + +A301::DoubleStatusSignal A301::GetRelativeEncoderPosition() const { + auto signal = GetPeriodicStatus2(); + return {signal.Get().relativeEncoderPosition, signal.GetStatus(), + signal.GetTimestamp()}; +} + +A301::DoubleStatusSignal A301::GetEncoderVelocity() const { + auto signal = GetPeriodicStatus2(); + return {signal.Get().encoderVelocity, signal.GetStatus(), + signal.GetTimestamp()}; +} + +A301::DoubleStatusSignal A301::GetAbsoluteEncoderPosition() const { + auto signal = GetPeriodicStatus3(); + return {signal.Get().absoluteEncoderPosition, signal.GetStatus(), + signal.GetTimestamp()}; +} + +A301Error A301::SetRelativeEncoderPosition(double position) { + int32_t status = 0; + HAL_SetA301RelativeEncoderPosition(m_handle, position, &status); + return detail::A301ErrorFromHalStatus(status); +} + +A301Error A301::SetAbsoluteEncoderPosition(double position) { + int32_t status = 0; + HAL_SetA301AbsoluteEncoderPosition(m_handle, position, &status); + return detail::A301ErrorFromHalStatus(status); +} + +A301Error A301::SetVelocity(double velocity) { + return SetSetpoint(HAL_A301_CONTROL_TYPE_VELOCITY, velocity); +} + +A301Error A301::SetRelativePosition(double position) { + return SetSetpoint(HAL_A301_CONTROL_TYPE_RELATIVE_POSITION, position); +} + +A301Error A301::SetRelativePositionWithSpeed(double position, double speed) { + return SetSetpoint(HAL_A301_CONTROL_TYPE_RELATIVE_POSITION, position, speed); +} + +A301Error A301::SetAbsolutePosition(double position) { + return SetSetpoint(HAL_A301_CONTROL_TYPE_ABSOLUTE_POSITION, position); +} + +A301Error A301::SetAbsolutePositionWithSpeed(double position, double speed) { + return SetSetpoint(HAL_A301_CONTROL_TYPE_ABSOLUTE_POSITION, position, speed); +} + +A301Error A301::SetCurrent(double current) { + return SetSetpoint(HAL_A301_CONTROL_TYPE_CURRENT, current); +} + +A301Error A301::SetIdleMode(IdleMode idleMode) { + int32_t status = 0; + HAL_SetA301IdleMode(m_handle, static_cast(idleMode), + &status); + return detail::A301ErrorFromHalStatus(status); +} + +A301::IdleMode A301::GetIdleMode() const { + int32_t status = 0; + auto mode = HAL_GetA301IdleMode(m_handle, &status); + WPILIB_ReportError(status, "A301 idle mode"); + return static_cast(mode); +} + +A301Error A301::EnableAbsolutePositionContinuousInput() { + int32_t status = 0; + HAL_SetA301AbsolutePositionContinuousInput(m_handle, true, &status); + return detail::A301ErrorFromHalStatus(status); +} + +A301Error A301::DisableAbsolutePositionContinuousInput() { + int32_t status = 0; + HAL_SetA301AbsolutePositionContinuousInput(m_handle, false, &status); + return detail::A301ErrorFromHalStatus(status); +} + +bool A301::IsAbsolutePositionContinuousInputEnabled() const { + int32_t status = 0; + bool enabled = + HAL_GetA301AbsolutePositionContinuousInput(m_handle, &status) != 0; + WPILIB_ReportError(status, "A301 absolute position continuous input"); + return enabled; +} + +A301Error A301::SetAbsoluteEncoderRangeOffset(double offset) { + int32_t status = 0; + HAL_SetA301AbsoluteEncoderRangeOffset(m_handle, offset, &status); + return detail::A301ErrorFromHalStatus(status); +} + +double A301::GetAbsoluteEncoderRangeOffset() const { + int32_t status = 0; + double offset = HAL_GetA301AbsoluteEncoderRangeOffset(m_handle, &status); + WPILIB_ReportError(status, "A301 absolute encoder range offset"); + return offset; +} + +void A301::SetThrottle(double throttle) { + m_setpoint = throttle; + SetSetpoint(HAL_A301_CONTROL_TYPE_DUTY_CYCLE, throttle); +} + +double A301::GetThrottle() const { + return m_setpoint; +} + +void A301::SetVoltage(wpi::units::volt_t outputVolts) { + m_setpoint = outputVolts.value() / 12.0; + SetSetpoint(HAL_A301_CONTROL_TYPE_VOLTAGE, outputVolts.value()); +} + +void A301::SetInverted(bool isInverted) { + int32_t status = 0; + HAL_SetA301Inverted(m_handle, isInverted, &status); +} + +bool A301::GetInverted() const { + int32_t status = 0; + bool inverted = HAL_GetA301Inverted(m_handle, &status) != 0; + WPILIB_ReportError(status, "A301 inversion"); + return inverted; +} + +void A301::Disable() { + SetThrottle(0.0); +} + +A301& A301::SetStatusFramePeriod(PeriodicFrame frame, int periodMs) { + int32_t status = 0; + HAL_SetA301StatusFramePeriod( + m_handle, static_cast(frame), periodMs, &status); + return *this; +} + +int A301::GetStatusFramePeriod(PeriodicFrame frame) const { + int32_t status = 0; + int period = HAL_GetA301StatusFramePeriod( + m_handle, static_cast(frame), &status); + WPILIB_ReportError(status, "A301 status frame period"); + return period; +} + +A301& A301::FaultsPeriodMs(int periodMs) { + return SetStatusFramePeriod(PeriodicFrame::kStatus1, periodMs); +} + +int A301::GetFaultsPeriodMs() const { + return GetStatusFramePeriod(PeriodicFrame::kStatus1); +} + +A301& A301::WarningsPeriodMs(int periodMs) { + return SetStatusFramePeriod(PeriodicFrame::kStatus1, periodMs); +} + +int A301::GetWarningsPeriodMs() const { + return GetStatusFramePeriod(PeriodicFrame::kStatus1); +} + +A301& A301::BusVoltagePeriodMs(int periodMs) { + return SetStatusFramePeriod(PeriodicFrame::kStatus0, periodMs); +} + +int A301::GetBusVoltagePeriodMs() const { + return GetStatusFramePeriod(PeriodicFrame::kStatus0); +} + +A301& A301::AppliedOutputPeriodMs(int periodMs) { + return SetStatusFramePeriod(PeriodicFrame::kStatus0, periodMs); +} + +int A301::GetAppliedOutputPeriodMs() const { + return GetStatusFramePeriod(PeriodicFrame::kStatus0); +} + +A301& A301::MotorCurrentPeriodMs(int periodMs) { + return SetStatusFramePeriod(PeriodicFrame::kStatus0, periodMs); +} + +int A301::GetMotorCurrentPeriodMs() const { + return GetStatusFramePeriod(PeriodicFrame::kStatus0); +} + +A301& A301::MotorTemperaturePeriodMs(int periodMs) { + return SetStatusFramePeriod(PeriodicFrame::kStatus0, periodMs); +} + +int A301::GetMotorTemperaturePeriodMs() const { + return GetStatusFramePeriod(PeriodicFrame::kStatus0); +} + +A301& A301::RelativeEncoderPositionPeriodMs(int periodMs) { + return SetStatusFramePeriod(PeriodicFrame::kStatus2, periodMs); +} + +int A301::GetRelativeEncoderPositionPeriodMs() const { + return GetStatusFramePeriod(PeriodicFrame::kStatus2); +} + +A301& A301::EncoderVelocityPeriodMs(int periodMs) { + return SetStatusFramePeriod(PeriodicFrame::kStatus2, periodMs); +} + +int A301::GetEncoderVelocityPeriodMs() const { + return GetStatusFramePeriod(PeriodicFrame::kStatus2); +} + +A301& A301::AbsoluteEncoderPositionPeriodMs(int periodMs) { + return SetStatusFramePeriod(PeriodicFrame::kStatus3, periodMs); +} + +int A301::GetAbsoluteEncoderPositionPeriodMs() const { + return GetStatusFramePeriod(PeriodicFrame::kStatus3); +} + +A301StatusSignal A301::GetPeriodicStatus0() const { + HAL_A301PeriodicStatus0 frame{}; + int32_t status = 0; + HAL_GetA301PeriodicStatus0(m_handle, &frame, &status); + return {frame, status, frame.timestamp}; +} + +A301StatusSignal A301::GetPeriodicStatus1() const { + HAL_A301PeriodicStatus1 frame{}; + int32_t status = 0; + HAL_GetA301PeriodicStatus1(m_handle, &frame, &status); + return {frame, status, frame.timestamp}; +} + +A301StatusSignal A301::GetPeriodicStatus2() const { + HAL_A301PeriodicStatus2 frame{}; + int32_t status = 0; + HAL_GetA301PeriodicStatus2(m_handle, &frame, &status); + return {frame, status, frame.timestamp}; +} + +A301StatusSignal A301::GetPeriodicStatus3() const { + HAL_A301PeriodicStatus3 frame{}; + int32_t status = 0; + HAL_GetA301PeriodicStatus3(m_handle, &frame, &status); + return {frame, status, frame.timestamp}; +} + +A301Error A301::SetSetpoint(HAL_A301ControlType controlType, double setpoint, + double positionSpeed) { + int32_t status = 0; + HAL_SetA301Setpoint(m_handle, setpoint, controlType, positionSpeed, &status); + return detail::A301ErrorFromHalStatus(status); +} diff --git a/drivers/src/main/native/include/wpi/drivers/motor/A301.hpp b/drivers/src/main/native/include/wpi/drivers/motor/A301.hpp new file mode 100644 index 00000000000..ddaa0faf85d --- /dev/null +++ b/drivers/src/main/native/include/wpi/drivers/motor/A301.hpp @@ -0,0 +1,687 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include + +#include "wpi/drivers/motor/A301Error.hpp" +#include "wpi/drivers/motor/A301StatusSignal.hpp" +#include "wpi/hal/A301.h" +#include "wpi/hardware/bus/CANBusMap.hpp" +#include "wpi/hardware/motor/MotorController.hpp" +#include "wpi/units/voltage.hpp" +#include "wpi/util/Handle.hpp" + +namespace wpi { + +/** High-level interface to a FIRST A301 motor controller. */ +class A301 final : public MotorController { + public: + /** The factory-default A301 device ID. */ + static constexpr int kDefaultDeviceId = HAL_A301_DEFAULT_DEVICE_ID; + + /** A periodic status frame produced by the A301. */ + enum class PeriodicFrame { + /** Applied output, voltage, current, temperature, and controller state. */ + kStatus0 = HAL_A301_STATUS_0, + /** Fault and warning state. */ + kStatus1 = HAL_A301_STATUS_1, + /** Relative encoder position and velocity. */ + kStatus2 = HAL_A301_STATUS_2, + /** Absolute encoder position. */ + kStatus3 = HAL_A301_STATUS_3, + }; + + /** A301 gearbox speed variant. */ + enum class GearboxRPM { + /** The gearbox speed is unknown. */ + kUnknown = HAL_A301_GEARBOX_RPM_UNKNOWN, + /** 215 RPM gearbox. */ + kRPM215 = HAL_A301_GEARBOX_RPM_215, + /** 500 RPM gearbox. */ + kRPM500 = HAL_A301_GEARBOX_RPM_500, + }; + + /** Neutral behavior when the controller output is zero. */ + enum class IdleMode { + /** Allow the motor to coast. */ + kCoast = HAL_A301_IDLE_MODE_COAST, + /** Actively brake the motor. */ + kBrake = HAL_A301_IDLE_MODE_BRAKE, + }; + + /** A301 firmware version information. */ + struct FirmwareVersion { + uint8_t major{}; + uint8_t minor{}; + uint16_t patch{}; + uint8_t prerelease{}; + uint8_t hardwareRevision{}; + uint32_t raw{}; + }; + + /** Active or sticky A301 motor faults. */ + struct Faults { + bool other{}; + bool motorType{}; + bool sensor{}; + bool can{}; + bool temperature{}; + bool gateDriver{}; + bool escEeprom{}; + bool firmware{}; + bool motorStartup{}; + uint16_t rawBits{}; + + /** Constructs an empty fault set. */ + Faults() = default; + + /** + * Decodes a packed A301 fault bit field. + * + * @param faults packed fault bits + */ + explicit Faults(uint16_t faults); + }; + + /** Active or sticky A301 motor warnings. */ + struct Warnings { + bool brownout{}; + bool overcurrent{}; + bool escEeprom{}; + bool extEeprom{}; + bool sensor{}; + bool stall{}; + bool hasReset{}; + bool other{}; + bool overvoltage{}; + bool motorLoopSpeed{}; + uint16_t rawBits{}; + + /** Constructs an empty warning set. */ + Warnings() = default; + + /** + * Decodes a packed A301 warning bit field. + * + * @param warnings packed warning bits + */ + explicit Warnings(uint16_t warnings); + }; + + /** A boolean value read from an A301 periodic status frame. */ + class BooleanStatusSignal : public A301StatusSignal { + public: + /** Inherits the A301 status signal constructors. */ + using A301StatusSignal::A301StatusSignal; + + /** @return the most recently received value */ + bool Get() const { return A301StatusSignal::Get(); } + + /** + * @param defaultValue value to return when this signal is invalid + * @return the received value or @p defaultValue + */ + bool Get(bool defaultValue) const { + return A301StatusSignal::Get(defaultValue); + } + + /** @return true when the value is valid */ + bool IsValid() const { return A301StatusSignal::IsValid(); } + + /** @return the error associated with the read */ + A301Error GetError() const { return A301StatusSignal::GetError(); } + + /** @return zero on success, or a HAL error code */ + int32_t GetStatus() const { return A301StatusSignal::GetStatus(); } + + /** @return the CAN frame timestamp in microseconds */ + uint64_t GetTimestamp() const { + return A301StatusSignal::GetTimestamp(); + } + }; + + /** A floating-point value read from an A301 periodic status frame. */ + class DoubleStatusSignal : public A301StatusSignal { + public: + /** Inherits the A301 status signal constructors. */ + using A301StatusSignal::A301StatusSignal; + + /** @return the most recently received value */ + double Get() const { return A301StatusSignal::Get(); } + + /** + * @param defaultValue value to return when this signal is invalid + * @return the received value or @p defaultValue + */ + double Get(double defaultValue) const { + return A301StatusSignal::Get(defaultValue); + } + + /** @return true when the value is valid */ + bool IsValid() const { return A301StatusSignal::IsValid(); } + + /** @return the error associated with the read */ + A301Error GetError() const { return A301StatusSignal::GetError(); } + + /** @return zero on success, or a HAL error code */ + int32_t GetStatus() const { return A301StatusSignal::GetStatus(); } + + /** @return the CAN frame timestamp in microseconds */ + uint64_t GetTimestamp() const { + return A301StatusSignal::GetTimestamp(); + } + }; + + /** A fault value read from an A301 periodic status frame. */ + class FaultsStatusSignal : public A301StatusSignal { + public: + /** Inherits the A301 status signal constructors. */ + using A301StatusSignal::A301StatusSignal; + + /** @return the most recently received fault set */ + Faults Get() const { return A301StatusSignal::Get(); } + + /** + * @param defaultValue value to return when this signal is invalid + * @return the received value or @p defaultValue + */ + Faults Get(Faults defaultValue) const { + return A301StatusSignal::Get(defaultValue); + } + + /** @return true when the value is valid */ + bool IsValid() const { return A301StatusSignal::IsValid(); } + + /** @return the error associated with the read */ + A301Error GetError() const { return A301StatusSignal::GetError(); } + + /** @return zero on success, or a HAL error code */ + int32_t GetStatus() const { return A301StatusSignal::GetStatus(); } + + /** @return the CAN frame timestamp in microseconds */ + uint64_t GetTimestamp() const { + return A301StatusSignal::GetTimestamp(); + } + }; + + /** A warning value read from an A301 periodic status frame. */ + class WarningsStatusSignal : public A301StatusSignal { + public: + /** Inherits the A301 status signal constructors. */ + using A301StatusSignal::A301StatusSignal; + + /** @return the most recently received warning set */ + Warnings Get() const { return A301StatusSignal::Get(); } + + /** + * @param defaultValue value to return when this signal is invalid + * @return the received value or @p defaultValue + */ + Warnings Get(Warnings defaultValue) const { + return A301StatusSignal::Get(defaultValue); + } + + /** @return true when the value is valid */ + bool IsValid() const { return A301StatusSignal::IsValid(); } + + /** @return the error associated with the read */ + A301Error GetError() const { + return A301StatusSignal::GetError(); + } + + /** @return zero on success, or a HAL error code */ + int32_t GetStatus() const { + return A301StatusSignal::GetStatus(); + } + + /** @return the CAN frame timestamp in microseconds */ + uint64_t GetTimestamp() const { + return A301StatusSignal::GetTimestamp(); + } + }; + + /** A gearbox value read from an A301 periodic status frame. */ + class GearboxRPMStatusSignal : public A301StatusSignal { + public: + /** Inherits the A301 status signal constructors. */ + using A301StatusSignal::A301StatusSignal; + + /** @return the most recently received gearbox speed variant */ + GearboxRPM Get() const { return A301StatusSignal::Get(); } + + /** + * @param defaultValue value to return when this signal is invalid + * @return the received value or @p defaultValue + */ + GearboxRPM Get(GearboxRPM defaultValue) const { + return A301StatusSignal::Get(defaultValue); + } + + /** @return true when the value is valid */ + bool IsValid() const { return A301StatusSignal::IsValid(); } + + /** @return the error associated with the read */ + A301Error GetError() const { + return A301StatusSignal::GetError(); + } + + /** @return zero on success, or a HAL error code */ + int32_t GetStatus() const { + return A301StatusSignal::GetStatus(); + } + + /** @return the CAN frame timestamp in microseconds */ + uint64_t GetTimestamp() const { + return A301StatusSignal::GetTimestamp(); + } + }; + + /** + * Constructs an A301 using its factory-default device ID. + * + * @param bus CAN bus containing the controller + */ + explicit A301(CANBusMap bus); + + /** + * Constructs an A301. + * + * On a Motioncore CAN bus, the connected device ID is automatically + * detected when possible and the supplied ID is used as a fallback. + * + * @param bus CAN bus containing the controller + * @param deviceId requested device ID + */ + A301(CANBusMap bus, int deviceId); + + /** + * Copy construction is disabled. + * + * @param other A301 that would be copied + */ + A301(const A301& other) = delete; + + /** + * Copy assignment is disabled. + * + * @param other A301 that would be copied + * @return this object + */ + A301& operator=(const A301& other) = delete; + + /** + * Move construction is disabled. + * + * @param other A301 that would be moved + */ + A301(A301&& other) = delete; + + /** + * Move assignment is disabled. + * + * @param other A301 that would be moved + * @return this object + */ + A301& operator=(A301&& other) = delete; + + /** Releases the native A301 handle. */ + ~A301() override = default; + + /** @return the CAN bus ID */ + int GetBusId() const; + + /** @return the resolved device ID */ + int GetDeviceId() const; + + /** @return the controller firmware version */ + FirmwareVersion GetFirmwareVersion() const; + + /** @return the firmware version in human-readable form */ + std::string GetFirmwareString() const; + + /** @return a signal indicating whether an active fault is present */ + BooleanStatusSignal HasActiveFault() const; + + /** @return a signal indicating whether a sticky fault is present */ + BooleanStatusSignal HasStickyFault() const; + + /** @return a signal indicating whether an active warning is present */ + BooleanStatusSignal HasActiveWarning() const; + + /** @return a signal indicating whether a sticky warning is present */ + BooleanStatusSignal HasStickyWarning() const; + + /** @return active motor faults */ + FaultsStatusSignal GetFaults() const; + + /** @return sticky motor faults recorded since they were last cleared */ + FaultsStatusSignal GetStickyFaults() const; + + /** @return active motor warnings */ + WarningsStatusSignal GetWarnings() const; + + /** @return sticky motor warnings recorded since they were last cleared */ + WarningsStatusSignal GetStickyWarnings() const; + + /** + * Clears active and sticky faults and warnings. + * + * @return command status + */ + A301Error ClearFaults(); + + /** @return the controller input-voltage signal, in volts */ + DoubleStatusSignal GetBusVoltage() const; + + /** @return the applied-output duty-cycle signal */ + DoubleStatusSignal GetAppliedOutput() const; + + /** @return the motor-current signal, in amperes */ + DoubleStatusSignal GetMotorCurrent() const; + + /** @return the motor-temperature signal, in degrees Celsius */ + DoubleStatusSignal GetMotorTemperature() const; + + /** @return the attached gearbox speed-variant signal */ + GearboxRPMStatusSignal GetGearboxRPM() const; + + /** @return the relative encoder-position signal, in motor rotations */ + DoubleStatusSignal GetRelativeEncoderPosition() const; + + /** @return the relative encoder-velocity signal, in RPM */ + DoubleStatusSignal GetEncoderVelocity() const; + + /** @return the absolute encoder-position signal, in rotations */ + DoubleStatusSignal GetAbsoluteEncoderPosition() const; + + /** + * Sets the relative encoder position. + * + * @param position position in motor rotations + * @return command status + */ + A301Error SetRelativeEncoderPosition(double position); + + /** + * Sets the absolute encoder position. + * + * @param position position in rotations + * @return command status + */ + A301Error SetAbsoluteEncoderPosition(double position); + + /** + * Sets a velocity setpoint. + * + * @param velocity velocity in RPM + * @return command status + */ + A301Error SetVelocity(double velocity); + + /** + * Sets a relative position setpoint using maximum speed. + * + * @param position position in motor rotations + * @return command status + */ + A301Error SetRelativePosition(double position); + + /** + * Sets a relative position setpoint with a specific approach speed. + * + * A speed less than or equal to zero requests maximum speed. + * + * @param position position in motor rotations + * @param speed approach speed in RPM + * @return command status + */ + A301Error SetRelativePositionWithSpeed(double position, double speed); + + /** + * Sets an absolute position setpoint using maximum speed. + * + * @param position absolute position in rotations + * @return command status + */ + A301Error SetAbsolutePosition(double position); + + /** + * Sets an absolute position setpoint with a specific approach speed. + * + * A speed less than or equal to zero requests maximum speed. + * + * @param position absolute position in rotations + * @param speed approach speed in RPM + * @return command status + */ + A301Error SetAbsolutePositionWithSpeed(double position, double speed); + + /** + * Sets a motor-current setpoint. + * + * @param current current in amperes + * @return command status + */ + A301Error SetCurrent(double current); + + /** + * Sets the idle mode. + * + * @param idleMode idle mode + * @return command status + */ + A301Error SetIdleMode(IdleMode idleMode); + + /** @return the configured idle mode */ + IdleMode GetIdleMode() const; + + /** + * Enables continuous input for absolute position control. + * + * @return command status + */ + A301Error EnableAbsolutePositionContinuousInput(); + + /** + * Disables continuous input for absolute position control. + * + * @return command status + */ + A301Error DisableAbsolutePositionContinuousInput(); + + /** @return true when absolute position continuous input is enabled */ + bool IsAbsolutePositionContinuousInputEnabled() const; + + /** + * Sets the absolute encoder range offset. + * + * @param offset offset in rotations, from -0.5 to 0.5 + * @return command status + */ + A301Error SetAbsoluteEncoderRangeOffset(double offset); + + /** @return the absolute encoder range offset in rotations */ + double GetAbsoluteEncoderRangeOffset() const; + + /** + * Sets the controller duty cycle. + * + * @param throttle duty cycle, conventionally from -1.0 to 1.0 + */ + void SetThrottle(double throttle) override; + + /** @return the last commanded duty cycle */ + double GetThrottle() const override; + + /** + * Sets the controller's internal voltage setpoint. + * + * @param outputVolts voltage setpoint + */ + void SetVoltage(wpi::units::volt_t outputVolts) override; + + /** + * Sets whether non-position control output is inverted. + * + * @param isInverted true to invert the output + */ + void SetInverted(bool isInverted) override; + + /** @return true when non-position control output is inverted */ + bool GetInverted() const override; + + /** Stops the motor output. */ + void Disable() override; + + /** + * Sets a periodic status frame period. + * + * The controller may normalize the requested period. Use + * GetStatusFramePeriod() to retrieve the effective value. + * + * @param frame periodic status frame + * @param periodMs requested period in milliseconds, from 0 to 1000 + * @return this object for method chaining + */ + A301& SetStatusFramePeriod(PeriodicFrame frame, int periodMs); + + /** + * Returns the effective periodic status frame period. + * + * @param frame periodic status frame + * @return period in milliseconds + */ + int GetStatusFramePeriod(PeriodicFrame frame) const; + + /** + * Sets the fault signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + A301& FaultsPeriodMs(int periodMs); + + /** @return the effective fault signal period in milliseconds */ + int GetFaultsPeriodMs() const; + + /** + * Sets the warning signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + A301& WarningsPeriodMs(int periodMs); + + /** @return the effective warning signal period in milliseconds */ + int GetWarningsPeriodMs() const; + + /** + * Sets the bus-voltage signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + A301& BusVoltagePeriodMs(int periodMs); + + /** @return the effective bus-voltage signal period in milliseconds */ + int GetBusVoltagePeriodMs() const; + + /** + * Sets the applied-output signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + A301& AppliedOutputPeriodMs(int periodMs); + + /** @return the effective applied-output signal period in milliseconds */ + int GetAppliedOutputPeriodMs() const; + + /** + * Sets the motor-current signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + A301& MotorCurrentPeriodMs(int periodMs); + + /** @return the effective motor-current signal period in milliseconds */ + int GetMotorCurrentPeriodMs() const; + + /** + * Sets the motor-temperature signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + A301& MotorTemperaturePeriodMs(int periodMs); + + /** @return the effective motor-temperature signal period in milliseconds */ + int GetMotorTemperaturePeriodMs() const; + + /** + * Sets the relative encoder-position signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + A301& RelativeEncoderPositionPeriodMs(int periodMs); + + /** + * @return the effective relative encoder-position signal period in + * milliseconds + */ + int GetRelativeEncoderPositionPeriodMs() const; + + /** + * Sets the encoder-velocity signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + A301& EncoderVelocityPeriodMs(int periodMs); + + /** @return the effective encoder-velocity signal period in milliseconds */ + int GetEncoderVelocityPeriodMs() const; + + /** + * Sets the absolute encoder-position signal period. + * + * @param periodMs requested period in milliseconds + * @return this object for method chaining + */ + A301& AbsoluteEncoderPositionPeriodMs(int periodMs); + + /** + * @return the effective absolute encoder-position signal period in + * milliseconds + */ + int GetAbsoluteEncoderPositionPeriodMs() const; + + private: + static constexpr double kDefaultPositionSpeed = 0.0; + + A301StatusSignal GetPeriodicStatus0() const; + + A301StatusSignal GetPeriodicStatus1() const; + + A301StatusSignal GetPeriodicStatus2() const; + + A301StatusSignal GetPeriodicStatus3() const; + + A301Error SetSetpoint(HAL_A301ControlType controlType, double setpoint, + double positionSpeed = kDefaultPositionSpeed); + + wpi::util::Handle m_handle; + int m_busId{}; + int m_deviceId{}; + std::atomic m_setpoint{}; +}; + +} // namespace wpi diff --git a/drivers/src/main/native/include/wpi/drivers/motor/A301Error.hpp b/drivers/src/main/native/include/wpi/drivers/motor/A301Error.hpp new file mode 100644 index 00000000000..e7ff306dc43 --- /dev/null +++ b/drivers/src/main/native/include/wpi/drivers/motor/A301Error.hpp @@ -0,0 +1,89 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "wpi/hal/CAN.h" +#include "wpi/hal/Errors.h" + +namespace wpi { + +/** REVLib-compatible error categories returned by high-level A301 commands. */ +enum class A301Error { + /** No error occurred. */ + kOk = 0, + /** A general controller error occurred. */ + kError = 1, + /** The operation timed out. */ + kTimeout = 2, + /** The requested operation is not implemented. */ + kNotImplemented = 3, + /** The HAL returned an error. */ + kHALError = 4, + /** The controller firmware could not be read. */ + kCantFindFirmware = 5, + /** The controller firmware is too old. */ + kFirmwareTooOld = 6, + /** The controller firmware is newer than this driver supports. */ + kFirmwareTooNew = 7, + /** The parameter ID is invalid. */ + kParamInvalidID = 8, + /** The parameter type does not match the expected type. */ + kParamMismatchType = 9, + /** The parameter does not support the requested access mode. */ + kParamAccessMode = 10, + /** The parameter value is invalid. */ + kParamInvalid = 11, + /** The parameter is deprecated or not implemented. */ + kParamNotImplementedDeprecated = 12, + /** The follower configuration does not match its leader. */ + kFollowConfigMismatch = 13, + /** The returned error value is invalid. */ + kInvalid = 14, + /** The requested setpoint is outside the accepted range. */ + kSetpointOutOfRange = 15, + /** An unknown controller error occurred. */ + kUnknown = 16, + /** The controller is disconnected from the CAN bus. */ + kCANDisconnected = 17, + /** Another device is using the requested CAN ID. */ + kDuplicateCANId = 18, + /** The requested CAN ID is invalid. */ + kInvalidCANId = 19, +}; + +namespace detail { + +/** + * Converts a HAL status code to its REVLib-compatible A301 error category. + * + * @param status HAL status code + * @return corresponding A301 error category + */ +constexpr A301Error A301ErrorFromHalStatus(int32_t status) { + switch (status) { + case 0: + return A301Error::kOk; + case HAL_CAN_TIMEOUT: + case HAL_ERR_CANSessionMux_MessageNotFound: + return A301Error::kTimeout; + case HAL_PARAMETER_OUT_OF_RANGE: + return A301Error::kParamInvalid; + case HAL_RESOURCE_IS_ALLOCATED: + return A301Error::kDuplicateCANId; + case HAL_RESOURCE_OUT_OF_RANGE: + return A301Error::kInvalidCANId; + case HAL_INCOMPATIBLE_STATE: + return A301Error::kError; + case HAL_CAN_BUFFER_OVERRUN: + return A301Error::kCANDisconnected; + default: + return A301Error::kHALError; + } +} + +} // namespace detail +} // namespace wpi diff --git a/drivers/src/main/native/include/wpi/drivers/motor/A301StatusSignal.hpp b/drivers/src/main/native/include/wpi/drivers/motor/A301StatusSignal.hpp new file mode 100644 index 00000000000..036f0b35310 --- /dev/null +++ b/drivers/src/main/native/include/wpi/drivers/motor/A301StatusSignal.hpp @@ -0,0 +1,99 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include +#include + +#include "wpi/drivers/motor/A301Error.hpp" + +namespace wpi { + +/** A value read from an A301 periodic status frame. */ +template +class A301StatusSignal { + public: + /** + * Constructs an A301 status signal. + * + * @param value most recently received value + * @param status HAL status associated with the read + * @param timestamp CAN frame timestamp in microseconds + */ + A301StatusSignal(T value, int32_t status, uint64_t timestamp) + : m_value{std::move(value)}, m_status{status}, m_timestamp{timestamp} {} + + /** + * Returns the most recently received value. + * + * @return most recently received value + */ + T Get() const { return m_value; } + + /** + * Returns the value when valid, or the supplied default value otherwise. + * + * @param defaultValue value to return when this signal is invalid + * @return received value or @p defaultValue + */ + T Get(T defaultValue) const { + return IsValid() ? m_value : std::move(defaultValue); + } + + /** + * Returns whether the value was read without an error. + * + * @return true when the value is valid + */ + bool IsValid() const { return GetError() == A301Error::kOk; } + + /** + * Returns the REVLib-compatible error associated with the read. + * + * @return signal error + */ + A301Error GetError() const { + return detail::A301ErrorFromHalStatus(m_status); + } + + /** + * Returns the underlying HAL status. + * + * @return zero on success, or a HAL error code + */ + int32_t GetStatus() const { return m_status; } + + /** + * Returns the CAN frame timestamp in microseconds. + * + * @return frame timestamp in microseconds + */ + uint64_t GetTimestamp() const { return m_timestamp; } + + /** + * Maps the value while retaining its status and timestamp. + * + * @tparam F mapper function type + * @param mapper function that maps the current value + * @return mapped signal with the same status and timestamp + */ + template + auto Map(F&& mapper) const + -> A301StatusSignal> { + using U = std::invoke_result_t; + return A301StatusSignal{std::invoke(std::forward(mapper), m_value), + m_status, m_timestamp}; + } + + private: + T m_value{}; + int32_t m_status{}; + uint64_t m_timestamp{}; +}; + +} // namespace wpi diff --git a/drivers/src/main/python/pyproject.toml b/drivers/src/main/python/pyproject.toml index 5ba62d88bf4..ea7b1ea9950 100644 --- a/drivers/src/main/python/pyproject.toml +++ b/drivers/src/main/python/pyproject.toml @@ -38,11 +38,14 @@ packages = ["wpilib_drivers"] [tool.semiwrap] update_init = ["wpilib_drivers"] +scan_headers_ignore = [ + "wpi/drivers/motor/A301StatusSignal.hpp", +] [tool.semiwrap.name_transform] default = "snake_case" enum_value = "CAPS_CASE" -known_words = ["3V3", "5V", "CAN", "CPU", "DS", "FMS", "FPGA", "HAL", "HTTP", "I2C", "IMU", "JNI", "JSON", "mDNS", "NT", "OpMode", "PCM", "PDH", "PDP", "PID", "POVs", "PWM", "RIO", "SPI", "URI", "URL", "USB", "VIn"] +known_words = ["3V3", "5V", "A301", "CAN", "CPU", "DS", "FMS", "FPGA", "HAL", "HTTP", "I2C", "IMU", "JNI", "JSON", "mDNS", "NT", "OpMode", "PCM", "PDH", "PDP", "PID", "POVs", "PWM", "RIO", "RPM", "SPI", "URI", "URL", "USB", "VIn"] [tool.semiwrap.extension_modules."wpilib_drivers._drivers"] name = "drivers" @@ -50,6 +53,8 @@ wraps = ["robotpy-native-wpilib-drivers"] depends = ["wpilib"] [tool.semiwrap.extension_modules."wpilib_drivers._drivers".headers] +A301Error = "wpi/drivers/motor/A301Error.hpp" +A301 = "wpi/drivers/motor/A301.hpp" Koors40 = "wpi/drivers/motor/Koors40.hpp" PWMSparkFlex = "wpi/drivers/motor/PWMSparkFlex.hpp" PWMSparkMax = "wpi/drivers/motor/PWMSparkMax.hpp" diff --git a/drivers/src/main/python/semiwrap/A301.yml b/drivers/src/main/python/semiwrap/A301.yml new file mode 100644 index 00000000000..5e681b6c97d --- /dev/null +++ b/drivers/src/main/python/semiwrap/A301.yml @@ -0,0 +1,199 @@ +classes: + wpi::A301: + force_no_trampoline: true + methods: + A301: + overloads: + CANBusMap: + CANBusMap, int: + GetBusId: + GetDeviceId: + GetFirmwareVersion: + GetFirmwareString: + HasActiveFault: + HasStickyFault: + HasActiveWarning: + HasStickyWarning: + GetFaults: + GetStickyFaults: + GetWarnings: + GetStickyWarnings: + ClearFaults: + GetBusVoltage: + GetAppliedOutput: + GetMotorCurrent: + GetMotorTemperature: + GetGearboxRPM: + GetRelativeEncoderPosition: + GetEncoderVelocity: + GetAbsoluteEncoderPosition: + SetRelativeEncoderPosition: + SetAbsoluteEncoderPosition: + SetVelocity: + SetRelativePosition: + SetRelativePositionWithSpeed: + SetAbsolutePosition: + SetAbsolutePositionWithSpeed: + SetCurrent: + SetIdleMode: + GetIdleMode: + EnableAbsolutePositionContinuousInput: + DisableAbsolutePositionContinuousInput: + IsAbsolutePositionContinuousInputEnabled: + SetAbsoluteEncoderRangeOffset: + GetAbsoluteEncoderRangeOffset: + SetThrottle: + GetThrottle: + SetVoltage: + SetInverted: + GetInverted: + Disable: + SetStatusFramePeriod: + GetStatusFramePeriod: + FaultsPeriodMs: + GetFaultsPeriodMs: + WarningsPeriodMs: + GetWarningsPeriodMs: + BusVoltagePeriodMs: + GetBusVoltagePeriodMs: + AppliedOutputPeriodMs: + GetAppliedOutputPeriodMs: + MotorCurrentPeriodMs: + GetMotorCurrentPeriodMs: + MotorTemperaturePeriodMs: + GetMotorTemperaturePeriodMs: + RelativeEncoderPositionPeriodMs: + GetRelativeEncoderPositionPeriodMs: + EncoderVelocityPeriodMs: + GetEncoderVelocityPeriodMs: + AbsoluteEncoderPositionPeriodMs: + GetAbsoluteEncoderPositionPeriodMs: + attributes: + kDefaultDeviceId: + enums: + PeriodicFrame: + GearboxRPM: + IdleMode: + wpi::A301::FirmwareVersion: + attributes: + major: + minor: + patch: + prerelease: + hardwareRevision: + raw: + wpi::A301::Faults: + attributes: + other: + motorType: + sensor: + can: + temperature: + gateDriver: + escEeprom: + firmware: + motorStartup: + rawBits: + methods: + Faults: + overloads: + "": + uint16_t: + wpi::A301::Warnings: + attributes: + brownout: + overcurrent: + escEeprom: + extEeprom: + sensor: + stall: + hasReset: + other: + overvoltage: + motorLoopSpeed: + rawBits: + methods: + Warnings: + overloads: + "": + uint16_t: + wpi::A301::BooleanStatusSignal: + force_no_default_constructor: true + force_no_trampoline: true + ignored_bases: + - A301StatusSignal + methods: + A301StatusSignal: + ignore: true + Get: + overloads: + '[const]': + bool [const]: + IsValid: + GetError: + GetStatus: + GetTimestamp: + wpi::A301::DoubleStatusSignal: + force_no_default_constructor: true + force_no_trampoline: true + ignored_bases: + - A301StatusSignal + methods: + A301StatusSignal: + ignore: true + Get: + overloads: + '[const]': + double [const]: + IsValid: + GetError: + GetStatus: + GetTimestamp: + wpi::A301::FaultsStatusSignal: + force_no_default_constructor: true + force_no_trampoline: true + ignored_bases: + - A301StatusSignal + methods: + A301StatusSignal: + ignore: true + Get: + overloads: + '[const]': + Faults [const]: + IsValid: + GetError: + GetStatus: + GetTimestamp: + wpi::A301::WarningsStatusSignal: + force_no_default_constructor: true + force_no_trampoline: true + ignored_bases: + - A301StatusSignal + methods: + A301StatusSignal: + ignore: true + Get: + overloads: + '[const]': + Warnings [const]: + IsValid: + GetError: + GetStatus: + GetTimestamp: + wpi::A301::GearboxRPMStatusSignal: + force_no_default_constructor: true + force_no_trampoline: true + ignored_bases: + - A301StatusSignal + methods: + A301StatusSignal: + ignore: true + Get: + overloads: + '[const]': + GearboxRPM [const]: + IsValid: + GetError: + GetStatus: + GetTimestamp: diff --git a/drivers/src/main/python/semiwrap/A301Error.yml b/drivers/src/main/python/semiwrap/A301Error.yml new file mode 100644 index 00000000000..483be769521 --- /dev/null +++ b/drivers/src/main/python/semiwrap/A301Error.yml @@ -0,0 +1,5 @@ +enums: + A301Error: +functions: + A301ErrorFromHalStatus: + ignore: true diff --git a/drivers/src/main/python/wpilib_drivers/__init__.py b/drivers/src/main/python/wpilib_drivers/__init__.py index 390beedb84f..d395d6ca873 100644 --- a/drivers/src/main/python/wpilib_drivers/__init__.py +++ b/drivers/src/main/python/wpilib_drivers/__init__.py @@ -2,6 +2,8 @@ # autogenerated by 'semiwrap create-imports wpilib_drivers wpilib_drivers._drivers' from ._drivers import ( + A301, + A301Error, Koors40, PWMSparkFlex, PWMSparkMax, @@ -16,6 +18,8 @@ ) __all__ = [ + "A301", + "A301Error", "Koors40", "PWMSparkFlex", "PWMSparkMax", diff --git a/drivers/src/test/java/org/wpilib/drivers/motor/A301StatusSignalTest.java b/drivers/src/test/java/org/wpilib/drivers/motor/A301StatusSignalTest.java new file mode 100644 index 00000000000..dd2c3c83307 --- /dev/null +++ b/drivers/src/test/java/org/wpilib/drivers/motor/A301StatusSignalTest.java @@ -0,0 +1,52 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.drivers.motor; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.Test; + +class A301StatusSignalTest { + @Test + void convertsRevLibCompatibleErrors() { + assertEquals(A301Error.kOk, A301Error.fromInt(0)); + assertEquals(A301Error.kTimeout, A301Error.fromHalStatus(-1154)); + assertEquals(A301Error.kParamInvalid, A301Error.fromHalStatus(-1028)); + assertEquals(A301Error.kInvalid, A301Error.fromInt(Integer.MAX_VALUE)); + } + + @Test + void validSignalReturnsAndMapsValue() { + var signal = A301StatusSignal.of(4, 0, 301L); + + assertTrue(signal.isValid()); + assertEquals(A301Error.kOk, signal.getError()); + assertEquals(4, signal.get()); + assertEquals(4, signal.get(8)); + assertEquals(301L, signal.getTimestamp()); + + var mapped = signal.map(value -> value * 2); + assertEquals(8, mapped.get()); + assertEquals(A301Error.kOk, mapped.getError()); + assertEquals(301L, mapped.getTimestamp()); + } + + @Test + void invalidSignalReturnsDefaultAndPreservesErrorWhenMapped() { + var signal = A301StatusSignal.of(4, -1154, 302L); + + assertFalse(signal.isValid()); + assertEquals(A301Error.kTimeout, signal.getError()); + assertEquals(4, signal.get()); + assertEquals(8, signal.get(8)); + + var mapped = signal.map(value -> value * 2); + assertEquals(8, mapped.get()); + assertEquals(A301Error.kTimeout, mapped.getError()); + assertEquals(302L, mapped.getTimestamp()); + } +} diff --git a/drivers/src/test/java/org/wpilib/drivers/motor/A301Test.java b/drivers/src/test/java/org/wpilib/drivers/motor/A301Test.java new file mode 100644 index 00000000000..a13b4da1e46 --- /dev/null +++ b/drivers/src/test/java/org/wpilib/drivers/motor/A301Test.java @@ -0,0 +1,62 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.drivers.motor; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.Test; +import org.wpilib.hardware.bus.CANBusMap; +import org.wpilib.hardware.hal.HAL; + +class A301Test { + @BeforeAll + static void initializeHal() { + HAL.initialize(); + } + + @Test + void controlsA301ThroughHighLevelApi() { + A301 a301 = new A301(CANBusMap.CAN_S0, 31); + assertEquals(0, a301.getBusId()); + assertEquals(31, a301.getDeviceId()); + + a301.setThrottle(0.25); + assertEquals(0.25, a301.getThrottle()); + a301.setVoltage(6.0); + assertEquals(0.5, a301.getThrottle()); + + assertEquals(A301Error.kOk, a301.setVelocity(125.0)); + assertEquals(A301Error.kOk, a301.setRelativePosition(3.0)); + assertEquals(A301Error.kOk, a301.setRelativePositionWithSpeed(3.0, 50.0)); + assertEquals(A301Error.kOk, a301.setAbsolutePosition(0.25)); + assertEquals(A301Error.kOk, a301.setAbsolutePositionWithSpeed(0.25, 50.0)); + assertEquals(A301Error.kOk, a301.setCurrent(10.0)); + assertEquals(A301Error.kOk, a301.setRelativeEncoderPosition(2.0)); + assertEquals(A301Error.kOk, a301.setAbsoluteEncoderPosition(0.1)); + assertEquals(A301Error.kOk, a301.setIdleMode(A301.IdleMode.BRAKE)); + assertEquals(A301Error.kOk, a301.enableAbsolutePositionContinuousInput()); + assertEquals(A301Error.kOk, a301.disableAbsolutePositionContinuousInput()); + assertEquals(A301Error.kOk, a301.clearFaults()); + + a301.setInverted(true); + assertTrue(a301.getInverted()); + assertEquals(A301Error.kOk, a301.setAbsoluteEncoderRangeOffset(0.25)); + assertEquals(A301Error.kParamInvalid, a301.setAbsoluteEncoderRangeOffset(0.75)); + + A301StatusSignal voltage = a301.getBusVoltage(); + assertFalse(voltage.isValid()); + assertEquals(A301Error.kTimeout, voltage.getError()); + + a301.disable(); + assertEquals(0.0, a301.getThrottle()); + a301.close(); + a301.close(); + assertThrows(IllegalStateException.class, a301::getThrottle); + } +} diff --git a/drivers/src/test/native/cpp/motor/A301Test.cpp b/drivers/src/test/native/cpp/motor/A301Test.cpp new file mode 100644 index 00000000000..d7cdbfe7e0c --- /dev/null +++ b/drivers/src/test/native/cpp/motor/A301Test.cpp @@ -0,0 +1,63 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/drivers/motor/A301.hpp" + +#include +#include + +#include "wpi/hal/Errors.h" + +using namespace wpi; + +TEST_CASE("A301 high-level API", "[drivers][a301]") { + A301 a301{CANBusMap::CAN_S0, 31}; + CHECK(a301.GetBusId() == 0); + CHECK(a301.GetDeviceId() == 31); + + a301.SetThrottle(0.25); + CHECK(a301.GetThrottle() == Catch::Approx(0.25)); + a301.SetVoltage(wpi::units::volt_t{6.0}); + CHECK(a301.GetThrottle() == Catch::Approx(0.5)); + + CHECK(a301.SetVelocity(125.0) == A301Error::kOk); + CHECK(a301.SetRelativePosition(3.0) == A301Error::kOk); + CHECK(a301.SetRelativePositionWithSpeed(3.0, 50.0) == A301Error::kOk); + CHECK(a301.SetAbsolutePosition(0.25) == A301Error::kOk); + CHECK(a301.SetAbsolutePositionWithSpeed(0.25, 50.0) == A301Error::kOk); + CHECK(a301.SetCurrent(10.0) == A301Error::kOk); + CHECK(a301.SetRelativeEncoderPosition(2.0) == A301Error::kOk); + CHECK(a301.SetAbsoluteEncoderPosition(0.1) == A301Error::kOk); + CHECK(a301.SetIdleMode(A301::IdleMode::kBrake) == A301Error::kOk); + CHECK(a301.EnableAbsolutePositionContinuousInput() == A301Error::kOk); + CHECK(a301.DisableAbsolutePositionContinuousInput() == A301Error::kOk); + CHECK(a301.ClearFaults() == A301Error::kOk); + + a301.SetInverted(true); + CHECK(a301.GetInverted()); + CHECK(a301.SetAbsoluteEncoderRangeOffset(0.25) == A301Error::kOk); + CHECK(a301.SetAbsoluteEncoderRangeOffset(0.75) == A301Error::kParamInvalid); + + auto voltage = a301.GetBusVoltage(); + CHECK_FALSE(voltage.IsValid()); + CHECK(voltage.GetError() == A301Error::kTimeout); + + a301.Disable(); + CHECK(a301.GetThrottle() == Catch::Approx(0.0)); +} + +TEST_CASE("A301 status signal maps HAL errors", "[drivers][a301]") { + A301StatusSignal valid{4, 0, 301}; + CHECK(valid.IsValid()); + CHECK(valid.Get() == 4); + CHECK(valid.Get(8) == 4); + CHECK(valid.GetTimestamp() == 301); + CHECK(valid.GetStatus() == 0); + CHECK(valid.Map([](int value) { return value * 2; }).Get() == 8); + + A301StatusSignal timeout{4, HAL_CAN_TIMEOUT, 302}; + CHECK_FALSE(timeout.IsValid()); + CHECK(timeout.Get(8) == 8); + CHECK(timeout.GetError() == A301Error::kTimeout); +} diff --git a/drivers/src/test/python/test_a301.py b/drivers/src/test/python/test_a301.py new file mode 100644 index 00000000000..a8f29f991ca --- /dev/null +++ b/drivers/src/test/python/test_a301.py @@ -0,0 +1,55 @@ +import pytest +import wpilib +import wpilib_drivers + + +def test_a301_high_level_api(): + a301 = wpilib_drivers.A301(wpilib.CANBusMap.CAN_S0, 31) + + assert a301.get_bus_id() == 0 + assert a301.get_device_id() == 31 + + a301.set_throttle(0.25) + assert a301.get_throttle() == pytest.approx(0.25) + a301.set_voltage(6.0) + assert a301.get_throttle() == pytest.approx(0.5) + + assert a301.set_velocity(125.0) == wpilib_drivers.A301Error.OK + assert a301.set_relative_position(3.0) == wpilib_drivers.A301Error.OK + assert ( + a301.set_relative_position_with_speed(3.0, 50.0) == wpilib_drivers.A301Error.OK + ) + assert a301.set_absolute_position(0.25) == wpilib_drivers.A301Error.OK + assert ( + a301.set_absolute_position_with_speed(0.25, 50.0) == wpilib_drivers.A301Error.OK + ) + assert a301.set_current(10.0) == wpilib_drivers.A301Error.OK + assert a301.set_relative_encoder_position(2.0) == wpilib_drivers.A301Error.OK + assert a301.set_absolute_encoder_position(0.1) == wpilib_drivers.A301Error.OK + assert ( + a301.set_idle_mode(wpilib_drivers.A301.IdleMode.BRAKE) + == wpilib_drivers.A301Error.OK + ) + assert ( + a301.enable_absolute_position_continuous_input() == wpilib_drivers.A301Error.OK + ) + assert ( + a301.disable_absolute_position_continuous_input() == wpilib_drivers.A301Error.OK + ) + assert a301.clear_faults() == wpilib_drivers.A301Error.OK + + a301.set_inverted(True) + assert a301.get_inverted() + assert a301.set_absolute_encoder_range_offset(0.25) == wpilib_drivers.A301Error.OK + assert ( + a301.set_absolute_encoder_range_offset(0.75) + == wpilib_drivers.A301Error.PARAM_INVALID + ) + + voltage = a301.get_bus_voltage() + assert not voltage.is_valid() + assert voltage.get_error() == wpilib_drivers.A301Error.TIMEOUT + assert voltage.get(-1.0) == -1.0 + + a301.disable() + assert a301.get_throttle() == pytest.approx(0.0) diff --git a/hal/src/main/java/org/wpilib/hardware/hal/A301FirmwareVersion.java b/hal/src/main/java/org/wpilib/hardware/hal/A301FirmwareVersion.java new file mode 100644 index 00000000000..2b200fd7fcc --- /dev/null +++ b/hal/src/main/java/org/wpilib/hardware/hal/A301FirmwareVersion.java @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.hardware.hal; + +/** Firmware and hardware version data received from a FIRST A301. */ +public class A301FirmwareVersion { + /** Major firmware version. */ + public final int major; + + /** Minor firmware version. */ + public final int minor; + + /** Firmware patch version. */ + public final int patch; + + /** Firmware prerelease identifier. */ + public final int prerelease; + + /** Controller hardware revision. */ + public final int hardwareRevision; + + /** Packed firmware version. */ + public final int raw; + + /** + * Constructs A301 version data (called from the HAL). + * + * @param major major firmware version + * @param minor minor firmware version + * @param patch patch firmware version + * @param prerelease prerelease identifier + * @param hardwareRevision hardware revision + * @param raw packed firmware version + */ + public A301FirmwareVersion( + int major, int minor, int patch, int prerelease, int hardwareRevision, int raw) { + this.major = major; + this.minor = minor; + this.patch = patch; + this.prerelease = prerelease; + this.hardwareRevision = hardwareRevision; + this.raw = raw; + } +} diff --git a/hal/src/main/java/org/wpilib/hardware/hal/A301JNI.java b/hal/src/main/java/org/wpilib/hardware/hal/A301JNI.java new file mode 100644 index 00000000000..50a0d90374d --- /dev/null +++ b/hal/src/main/java/org/wpilib/hardware/hal/A301JNI.java @@ -0,0 +1,277 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.hardware.hal; + +/** FIRST A301 motor controller HAL JNI functions. */ +public class A301JNI extends JNIWrapper { + /** Minimum valid A301 device ID. */ + public static final int MIN_DEVICE_ID = 0; + + /** Maximum valid A301 device ID. */ + public static final int MAX_DEVICE_ID = 62; + + /** Factory-default A301 device ID. */ + public static final int DEFAULT_DEVICE_ID = 3; + + /** Periodic status frame containing output, electrical, and controller-state data. */ + public static final int STATUS_0 = 0; + + /** Periodic status frame containing fault and warning data. */ + public static final int STATUS_1 = 1; + + /** Periodic status frame containing relative encoder data. */ + public static final int STATUS_2 = 2; + + /** Periodic status frame containing absolute encoder data. */ + public static final int STATUS_3 = 3; + + /** Unknown gearbox speed variant. */ + public static final int GEARBOX_RPM_UNKNOWN = 0; + + /** 215 RPM gearbox speed variant. */ + public static final int GEARBOX_RPM_215 = 1; + + /** 500 RPM gearbox speed variant. */ + public static final int GEARBOX_RPM_500 = 2; + + /** Coast idle mode. */ + public static final int IDLE_MODE_COAST = 0; + + /** Brake idle mode. */ + public static final int IDLE_MODE_BRAKE = 1; + + /** Duty-cycle control type. */ + public static final int CONTROL_TYPE_DUTY_CYCLE = 0; + + /** Velocity control type. */ + public static final int CONTROL_TYPE_VELOCITY = 1; + + /** Voltage control type. */ + public static final int CONTROL_TYPE_VOLTAGE = 2; + + /** Relative-position control type. */ + public static final int CONTROL_TYPE_RELATIVE_POSITION = 3; + + /** Absolute-position control type. */ + public static final int CONTROL_TYPE_ABSOLUTE_POSITION = 4; + + /** Motor-current control type. */ + public static final int CONTROL_TYPE_CURRENT = 5; + + /** + * Detects the device ID of an A301 attached to a CAN bus. + * + * @param busId CAN bus ID + * @return detected device ID + */ + public static native int detectDeviceId(int busId); + + /** + * Initializes an A301. + * + * @param busId CAN bus ID + * @param deviceId requested device ID + * @return native A301 handle + */ + public static native int initialize(int busId, int deviceId); + + /** + * Frees an A301 handle. + * + * @param handle native A301 handle + */ + public static native void free(int handle); + + /** + * Returns whether an A301 device ID is valid. + * + * @param deviceId device ID to validate + * @return true when the device ID is valid + */ + public static native boolean checkDeviceId(int deviceId); + + /** + * Returns the CAN bus ID associated with an A301 handle. + * + * @param handle native A301 handle + * @return CAN bus ID + */ + public static native int getBusId(int handle); + + /** + * Returns the resolved device ID associated with an A301 handle. + * + * @param handle native A301 handle + * @return resolved device ID + */ + public static native int getDeviceId(int handle); + + /** + * Returns the controller firmware version. + * + * @param handle native A301 handle + * @return firmware and hardware version data + */ + public static native A301FirmwareVersion getFirmwareVersion(int handle); + + /** + * Reads and decodes periodic status frame 0. + * + * @param handle native A301 handle + * @return decoded status frame + */ + public static native A301PeriodicStatus0 getPeriodicStatus0(int handle); + + /** + * Reads and decodes periodic status frame 1. + * + * @param handle native A301 handle + * @return decoded status frame + */ + public static native A301PeriodicStatus1 getPeriodicStatus1(int handle); + + /** + * Reads and decodes periodic status frame 2. + * + * @param handle native A301 handle + * @return decoded status frame + */ + public static native A301PeriodicStatus2 getPeriodicStatus2(int handle); + + /** + * Reads and decodes periodic status frame 3. + * + * @param handle native A301 handle + * @return decoded status frame + */ + public static native A301PeriodicStatus3 getPeriodicStatus3(int handle); + + /** + * Sets the relative encoder position. + * + * @param handle native A301 handle + * @param position position in motor rotations + * @return HAL status code + */ + public static native int setRelativeEncoderPosition(int handle, double position); + + /** + * Sets the absolute encoder position. + * + * @param handle native A301 handle + * @param position position in rotations + * @return HAL status code + */ + public static native int setAbsoluteEncoderPosition(int handle, double position); + + /** + * Sends a repeating control setpoint. + * + * @param handle native A301 handle + * @param value requested control value + * @param controlType controller control mode + * @param positionSpeed approach speed for position controls, or zero for maximum speed + * @return HAL status code + */ + public static native int setSetpoint( + int handle, double value, int controlType, double positionSpeed); + + /** + * Sets the controller idle mode. + * + * @param handle native A301 handle + * @param idleMode idle mode + * @return HAL status code + */ + public static native int setIdleMode(int handle, int idleMode); + + /** + * Returns the configured idle mode. + * + * @param handle native A301 handle + * @return idle mode + */ + public static native int getIdleMode(int handle); + + /** + * Enables or disables continuous input for absolute position control. + * + * @param handle native A301 handle + * @param enabled true to enable continuous input + * @return HAL status code + */ + public static native int setAbsolutePositionContinuousInput(int handle, boolean enabled); + + /** + * Returns whether continuous input is enabled for absolute position control. + * + * @param handle native A301 handle + * @return true when continuous input is enabled + */ + public static native boolean getAbsolutePositionContinuousInput(int handle); + + /** + * Sets the absolute encoder range offset. + * + * @param handle native A301 handle + * @param offset offset in rotations, from -0.5 to 0.5 + * @return HAL status code + */ + public static native int setAbsoluteEncoderRangeOffset(int handle, double offset); + + /** + * Returns the absolute encoder range offset. + * + * @param handle native A301 handle + * @return range offset in rotations + */ + public static native double getAbsoluteEncoderRangeOffset(int handle); + + /** + * Sets whether non-position control output is inverted. + * + * @param handle native A301 handle + * @param inverted true to invert the output + * @return HAL status code + */ + public static native int setInverted(int handle, boolean inverted); + + /** + * Returns whether non-position control output is inverted. + * + * @param handle native A301 handle + * @return true when the output is inverted + */ + public static native boolean getInverted(int handle); + + /** + * Clears active and sticky faults and warnings. + * + * @param handle native A301 handle + * @return HAL status code + */ + public static native int clearFaults(int handle); + + /** + * Sets a periodic status frame period. + * + * @param handle native A301 handle + * @param frame periodic status frame + * @param periodMs requested period in milliseconds, from 0 to 1000 + * @return HAL status code + */ + public static native int setStatusFramePeriod(int handle, int frame, int periodMs); + + /** + * Returns the effective periodic status frame period. + * + * @param handle native A301 handle + * @param frame periodic status frame + * @return period in milliseconds + */ + public static native int getStatusFramePeriod(int handle, int frame); + + private A301JNI() {} +} diff --git a/hal/src/main/java/org/wpilib/hardware/hal/A301PeriodicStatus0.java b/hal/src/main/java/org/wpilib/hardware/hal/A301PeriodicStatus0.java new file mode 100644 index 00000000000..24ce5b61580 --- /dev/null +++ b/hal/src/main/java/org/wpilib/hardware/hal/A301PeriodicStatus0.java @@ -0,0 +1,69 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.hardware.hal; + +/** Data decoded from an A301 periodic status 0 frame. */ +public class A301PeriodicStatus0 { + /** Applied output duty cycle. */ + public final double appliedOutput; + + /** Controller input voltage in volts. */ + public final double voltage; + + /** Motor current in amperes. */ + public final double current; + + /** Motor temperature in degrees Celsius. */ + public final int motorTemperature; + + /** Whether non-position control output is inverted. */ + public final boolean inverted; + + /** Whether the primary heartbeat is locked. */ + public final boolean primaryHeartbeatLock; + + /** Gearbox speed variant. */ + public final int gearboxRPM; + + /** HAL status associated with the read. */ + public final int status; + + /** CAN frame timestamp in microseconds. */ + public final long timestamp; + + /** + * Constructs A301 periodic status 0 data (called from the HAL). + * + * @param appliedOutput applied output duty cycle + * @param voltage controller input voltage in volts + * @param current motor current in amperes + * @param motorTemperature motor temperature in degrees Celsius + * @param inverted whether non-position control output is inverted + * @param primaryHeartbeatLock whether the primary heartbeat is locked + * @param gearboxRPM gearbox speed variant + * @param status HAL status associated with the read + * @param timestamp CAN frame timestamp in microseconds + */ + public A301PeriodicStatus0( + double appliedOutput, + double voltage, + double current, + int motorTemperature, + boolean inverted, + boolean primaryHeartbeatLock, + int gearboxRPM, + int status, + long timestamp) { + this.appliedOutput = appliedOutput; + this.voltage = voltage; + this.current = current; + this.motorTemperature = motorTemperature; + this.inverted = inverted; + this.primaryHeartbeatLock = primaryHeartbeatLock; + this.gearboxRPM = gearboxRPM; + this.status = status; + this.timestamp = timestamp; + } +} diff --git a/hal/src/main/java/org/wpilib/hardware/hal/A301PeriodicStatus1.java b/hal/src/main/java/org/wpilib/hardware/hal/A301PeriodicStatus1.java new file mode 100644 index 00000000000..60154820506 --- /dev/null +++ b/hal/src/main/java/org/wpilib/hardware/hal/A301PeriodicStatus1.java @@ -0,0 +1,213 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.hardware.hal; + +/** Data decoded from an A301 periodic status 1 frame. */ +public class A301PeriodicStatus1 { + /** Packed active fault bits. */ + public final int faults; + + /** Packed active warning bits. */ + public final int warnings; + + /** Packed sticky fault bits. */ + public final int stickyFaults; + + /** Packed sticky warning bits. */ + public final int stickyWarnings; + + /** Whether an uncategorized fault is active. */ + public final boolean otherFault; + + /** Whether a motor-type fault is active. */ + public final boolean motorTypeFault; + + /** Whether a sensor fault is active. */ + public final boolean sensorFault; + + /** Whether a CAN communication fault is active. */ + public final boolean canFault; + + /** Whether a temperature fault is active. */ + public final boolean temperatureFault; + + /** Whether a gate-driver fault is active. */ + public final boolean drvFault; + + /** Whether an ESC EEPROM fault is active. */ + public final boolean escEepromFault; + + /** Whether a firmware fault is active. */ + public final boolean firmwareFault; + + /** Whether a motor-startup fault is active. */ + public final boolean motorStartupFault; + + /** Whether a brownout warning is active. */ + public final boolean brownoutWarning; + + /** Whether an overcurrent warning is active. */ + public final boolean overcurrentWarning; + + /** Whether an ESC EEPROM warning is active. */ + public final boolean escEepromWarning; + + /** Whether an external EEPROM warning is active. */ + public final boolean extEepromWarning; + + /** Whether a sensor warning is active. */ + public final boolean sensorWarning; + + /** Whether a stall warning is active. */ + public final boolean stallWarning; + + /** Whether a controller-reset warning is active. */ + public final boolean hasResetWarning; + + /** Whether an uncategorized warning is active. */ + public final boolean otherWarning; + + /** Whether an overvoltage warning is active. */ + public final boolean overvoltageWarning; + + /** Whether a motor-loop-speed warning is active. */ + public final boolean motorLoopSpeedWarning; + + /** Whether an uncategorized fault has occurred since faults were last cleared. */ + public final boolean otherStickyFault; + + /** Whether a motor-type fault has occurred since faults were last cleared. */ + public final boolean motorTypeStickyFault; + + /** Whether a sensor fault has occurred since faults were last cleared. */ + public final boolean sensorStickyFault; + + /** Whether a CAN communication fault has occurred since faults were last cleared. */ + public final boolean canStickyFault; + + /** Whether a temperature fault has occurred since faults were last cleared. */ + public final boolean temperatureStickyFault; + + /** Whether a gate-driver fault has occurred since faults were last cleared. */ + public final boolean drvStickyFault; + + /** Whether an ESC EEPROM fault has occurred since faults were last cleared. */ + public final boolean escEepromStickyFault; + + /** Whether a firmware fault has occurred since faults were last cleared. */ + public final boolean firmwareStickyFault; + + /** Whether a motor-startup fault has occurred since faults were last cleared. */ + public final boolean motorStartupStickyFault; + + /** Whether a brownout warning has occurred since warnings were last cleared. */ + public final boolean brownoutStickyWarning; + + /** Whether an overcurrent warning has occurred since warnings were last cleared. */ + public final boolean overcurrentStickyWarning; + + /** Whether an ESC EEPROM warning has occurred since warnings were last cleared. */ + public final boolean escEepromStickyWarning; + + /** Whether an external EEPROM warning has occurred since warnings were last cleared. */ + public final boolean extEepromStickyWarning; + + /** Whether a sensor warning has occurred since warnings were last cleared. */ + public final boolean sensorStickyWarning; + + /** Whether a stall warning has occurred since warnings were last cleared. */ + public final boolean stallStickyWarning; + + /** Whether a controller reset has occurred since warnings were last cleared. */ + public final boolean hasResetStickyWarning; + + /** Whether an uncategorized warning has occurred since warnings were last cleared. */ + public final boolean otherStickyWarning; + + /** Whether an overvoltage warning has occurred since warnings were last cleared. */ + public final boolean overvoltageStickyWarning; + + /** Whether a motor-loop-speed warning has occurred since warnings were last cleared. */ + public final boolean motorLoopSpeedStickyWarning; + + /** Whether the controller is following another controller. */ + public final boolean isFollower; + + /** HAL status associated with the read. */ + public final int status; + + /** CAN frame timestamp in microseconds. */ + public final long timestamp; + + /** + * Constructs A301 periodic status 1 data (called from the HAL). + * + * @param faults packed active fault bits + * @param warnings packed active warning bits + * @param stickyFaults packed sticky fault bits + * @param stickyWarnings packed sticky warning bits + * @param isFollower whether the controller is following another controller + * @param status HAL status associated with the read + * @param timestamp CAN frame timestamp in microseconds + */ + public A301PeriodicStatus1( + int faults, + int warnings, + int stickyFaults, + int stickyWarnings, + boolean isFollower, + int status, + long timestamp) { + this.faults = faults; + this.warnings = warnings; + this.stickyFaults = stickyFaults; + this.stickyWarnings = stickyWarnings; + otherFault = getBit(faults, 0); + motorTypeFault = getBit(faults, 1); + sensorFault = getBit(faults, 2); + canFault = getBit(faults, 3); + temperatureFault = getBit(faults, 4); + drvFault = getBit(faults, 5); + escEepromFault = getBit(faults, 6); + firmwareFault = getBit(faults, 7); + motorStartupFault = getBit(faults, 8); + brownoutWarning = getBit(warnings, 0); + overcurrentWarning = getBit(warnings, 1); + escEepromWarning = getBit(warnings, 2); + extEepromWarning = getBit(warnings, 3); + sensorWarning = getBit(warnings, 4); + stallWarning = getBit(warnings, 5); + hasResetWarning = getBit(warnings, 6); + otherWarning = getBit(warnings, 7); + overvoltageWarning = getBit(warnings, 8); + motorLoopSpeedWarning = getBit(warnings, 9); + otherStickyFault = getBit(stickyFaults, 0); + motorTypeStickyFault = getBit(stickyFaults, 1); + sensorStickyFault = getBit(stickyFaults, 2); + canStickyFault = getBit(stickyFaults, 3); + temperatureStickyFault = getBit(stickyFaults, 4); + drvStickyFault = getBit(stickyFaults, 5); + escEepromStickyFault = getBit(stickyFaults, 6); + firmwareStickyFault = getBit(stickyFaults, 7); + motorStartupStickyFault = getBit(stickyFaults, 8); + brownoutStickyWarning = getBit(stickyWarnings, 0); + overcurrentStickyWarning = getBit(stickyWarnings, 1); + escEepromStickyWarning = getBit(stickyWarnings, 2); + extEepromStickyWarning = getBit(stickyWarnings, 3); + sensorStickyWarning = getBit(stickyWarnings, 4); + stallStickyWarning = getBit(stickyWarnings, 5); + hasResetStickyWarning = getBit(stickyWarnings, 6); + otherStickyWarning = getBit(stickyWarnings, 7); + overvoltageStickyWarning = getBit(stickyWarnings, 8); + motorLoopSpeedStickyWarning = getBit(stickyWarnings, 9); + this.isFollower = isFollower; + this.status = status; + this.timestamp = timestamp; + } + + private static boolean getBit(int value, int bit) { + return (value & (1 << bit)) != 0; + } +} diff --git a/hal/src/main/java/org/wpilib/hardware/hal/A301PeriodicStatus2.java b/hal/src/main/java/org/wpilib/hardware/hal/A301PeriodicStatus2.java new file mode 100644 index 00000000000..2a2ace032fc --- /dev/null +++ b/hal/src/main/java/org/wpilib/hardware/hal/A301PeriodicStatus2.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.hardware.hal; + +/** Data decoded from an A301 periodic status 2 frame. */ +public class A301PeriodicStatus2 { + /** Relative encoder velocity in RPM. */ + public final double encoderVelocity; + + /** Relative encoder position in motor rotations. */ + public final double relativeEncoderPosition; + + /** HAL status associated with the read. */ + public final int status; + + /** CAN frame timestamp in microseconds. */ + public final long timestamp; + + /** + * Constructs A301 periodic status 2 data (called from the HAL). + * + * @param encoderVelocity relative encoder velocity in RPM + * @param relativeEncoderPosition relative encoder position in motor rotations + * @param status HAL status associated with the read + * @param timestamp CAN frame timestamp in microseconds + */ + public A301PeriodicStatus2( + double encoderVelocity, double relativeEncoderPosition, int status, long timestamp) { + this.encoderVelocity = encoderVelocity; + this.relativeEncoderPosition = relativeEncoderPosition; + this.status = status; + this.timestamp = timestamp; + } +} diff --git a/hal/src/main/java/org/wpilib/hardware/hal/A301PeriodicStatus3.java b/hal/src/main/java/org/wpilib/hardware/hal/A301PeriodicStatus3.java new file mode 100644 index 00000000000..1aff67ab8ce --- /dev/null +++ b/hal/src/main/java/org/wpilib/hardware/hal/A301PeriodicStatus3.java @@ -0,0 +1,30 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.hardware.hal; + +/** Data decoded from an A301 periodic status 3 frame. */ +public class A301PeriodicStatus3 { + /** Absolute encoder position in rotations. */ + public final double absoluteEncoderPosition; + + /** HAL status associated with the read. */ + public final int status; + + /** CAN frame timestamp in microseconds. */ + public final long timestamp; + + /** + * Constructs A301 periodic status 3 data (called from the HAL). + * + * @param absoluteEncoderPosition absolute encoder position in rotations + * @param status HAL status associated with the read + * @param timestamp CAN frame timestamp in microseconds + */ + public A301PeriodicStatus3(double absoluteEncoderPosition, int status, long timestamp) { + this.absoluteEncoderPosition = absoluteEncoderPosition; + this.status = status; + this.timestamp = timestamp; + } +} diff --git a/hal/src/main/native/cpp/A301.cpp b/hal/src/main/native/cpp/A301.cpp new file mode 100644 index 00000000000..8f96656f8dc --- /dev/null +++ b/hal/src/main/native/cpp/A301.cpp @@ -0,0 +1,857 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/hal/A301.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "HALInitializer.hpp" +#include "wpi/hal/CAN.h" +#include "wpi/hal/CANAPI.h" +#include "wpi/hal/ErrorHandling.hpp" +#include "wpi/hal/Errors.h" +#include "wpi/hal/handles/UnlimitedHandleResource.hpp" +#include "wpi/util/mutex.hpp" +#include "wpi/util/timestamp.hpp" + +using namespace wpi::hal; + +namespace { + +constexpr uint32_t kVelocitySetpointId = 0x02030000; +constexpr uint32_t kDutyCycleSetpointId = 0x02030080; +constexpr uint32_t kRelativePositionSetpointId = 0x02030100; +constexpr uint32_t kCurrentSetpointId = 0x02030180; +constexpr uint32_t kAbsolutePositionSetpointId = 0x02030200; +constexpr uint32_t kClearFaultsId = 0x02031b80; +constexpr uint32_t kGetFirmwareVersionId = 0x02032600; +constexpr uint32_t kSetRelativeEncoderPositionId = 0x02032800; +constexpr uint32_t kSetAbsoluteEncoderPositionId = 0x02032880; +constexpr uint32_t kSetContinuousInputId = 0x02032900; +constexpr uint32_t kSetAbsoluteRangeOffsetId = 0x02032980; +constexpr uint32_t kSetIdleModeId = 0x02033d00; +constexpr uint32_t kSetStatusPeriodId = 0x02034400; +constexpr uint32_t kSetStatusPeriodResponseId = 0x02034440; +constexpr uint32_t kGetIdleModeId = 0x02035100; +constexpr uint32_t kGetIdleModeResponseId = 0x02035140; +constexpr uint32_t kGetContinuousInputId = 0x02035180; +constexpr uint32_t kGetContinuousInputResponseId = 0x020351c0; +constexpr uint32_t kGetAbsoluteRangeOffsetId = 0x02035200; +constexpr uint32_t kGetAbsoluteRangeOffsetResponseId = 0x02035240; +constexpr uint32_t kGetStatusPeriodId = 0x02036000; +constexpr uint32_t kGetStatusPeriodResponseId = 0x02036040; +constexpr uint32_t kStatus0Id = 0x0203b800; +constexpr uint32_t kStatus1Id = 0x0203b840; +constexpr uint32_t kStatus2Id = 0x0203b880; +constexpr uint32_t kStatus3Id = 0x0203b8c0; + +constexpr int32_t ApiFromExtId(uint32_t id) { + return (id >> 6) & 0x3ff; +} + +constexpr int32_t kVelocitySetpointApi = ApiFromExtId(kVelocitySetpointId); +constexpr int32_t kDutyCycleSetpointApi = ApiFromExtId(kDutyCycleSetpointId); +constexpr int32_t kRelativePositionSetpointApi = + ApiFromExtId(kRelativePositionSetpointId); +constexpr int32_t kCurrentSetpointApi = ApiFromExtId(kCurrentSetpointId); +constexpr int32_t kAbsolutePositionSetpointApi = + ApiFromExtId(kAbsolutePositionSetpointId); +constexpr int32_t kClearFaultsApi = ApiFromExtId(kClearFaultsId); +constexpr int32_t kGetFirmwareVersionApi = ApiFromExtId(kGetFirmwareVersionId); +constexpr int32_t kSetRelativeEncoderPositionApi = + ApiFromExtId(kSetRelativeEncoderPositionId); +constexpr int32_t kSetAbsoluteEncoderPositionApi = + ApiFromExtId(kSetAbsoluteEncoderPositionId); +constexpr int32_t kSetContinuousInputApi = ApiFromExtId(kSetContinuousInputId); +constexpr int32_t kSetAbsoluteRangeOffsetApi = + ApiFromExtId(kSetAbsoluteRangeOffsetId); +constexpr int32_t kSetIdleModeApi = ApiFromExtId(kSetIdleModeId); +constexpr int32_t kSetStatusPeriodApi = ApiFromExtId(kSetStatusPeriodId); +constexpr int32_t kSetStatusPeriodResponseApi = + ApiFromExtId(kSetStatusPeriodResponseId); +constexpr int32_t kGetIdleModeApi = ApiFromExtId(kGetIdleModeId); +constexpr int32_t kGetIdleModeResponseApi = + ApiFromExtId(kGetIdleModeResponseId); +constexpr int32_t kGetContinuousInputApi = ApiFromExtId(kGetContinuousInputId); +constexpr int32_t kGetContinuousInputResponseApi = + ApiFromExtId(kGetContinuousInputResponseId); +constexpr int32_t kGetAbsoluteRangeOffsetApi = + ApiFromExtId(kGetAbsoluteRangeOffsetId); +constexpr int32_t kGetAbsoluteRangeOffsetResponseApi = + ApiFromExtId(kGetAbsoluteRangeOffsetResponseId); +constexpr int32_t kGetStatusPeriodApi = ApiFromExtId(kGetStatusPeriodId); +constexpr int32_t kGetStatusPeriodResponseApi = + ApiFromExtId(kGetStatusPeriodResponseId); +constexpr int32_t kStatus0Api = ApiFromExtId(kStatus0Id); +constexpr int32_t kStatus1Api = ApiFromExtId(kStatus1Id); +constexpr int32_t kStatus2Api = ApiFromExtId(kStatus2Id); +constexpr int32_t kStatus3Api = ApiFromExtId(kStatus3Id); + +constexpr int32_t kControlPeriodMs = 20; +constexpr int32_t kDefaultReadTimeoutMs = 20; +constexpr int32_t kConfigurationReadTimeoutMs = 500; +constexpr int32_t kMaxReadRetries = 5; +constexpr int32_t kMinimumPeriodicTimeoutMs = 500; +constexpr int32_t kMaxStatusPeriodMs = 1000; +constexpr int32_t kFirstMotioncoreBus = 5; + +constexpr std::array kControlApis{ + kDutyCycleSetpointApi, kVelocitySetpointApi, kRelativePositionSetpointApi, + kCurrentSetpointApi, kAbsolutePositionSetpointApi}; + +struct A301Obj { + ~A301Obj(); + + int32_t busId; + int32_t deviceId; + HAL_CANHandle canHandle; + std::array statusPeriods{10, 250, 20, 20}; + bool inverted = false; + int32_t activeSetpointApi = kDutyCycleSetpointApi; + wpi::util::mutex stateMutex; + wpi::util::mutex configurationMutex; + std::string previousAllocation; +}; + +UnlimitedHandleResource* + gA301Handles; +wpi::util::mutex gAllocationsMutex; +std::unordered_map gAllocations; + +uint32_t MakeAllocationKey(int32_t busId, int32_t deviceId) { + return (static_cast(busId) << 8) | static_cast(deviceId); +} + +A301Obj::~A301Obj() { + HAL_CleanCAN(canHandle); + std::scoped_lock lock{gAllocationsMutex}; + gAllocations.erase(MakeAllocationKey(busId, deviceId)); +} + +std::shared_ptr GetA301(HAL_A301Handle handle, int32_t* status) { + auto a301 = gA301Handles->Get(handle); + if (!a301) { + *status = HAL_HANDLE_ERROR; + } + return a301; +} + +void StoreU32LE(uint8_t* data, uint32_t value) { + data[0] = value; + data[1] = value >> 8; + data[2] = value >> 16; + data[3] = value >> 24; +} + +uint16_t LoadU16LE(const uint8_t* data) { + return static_cast(data[0]) | (static_cast(data[1]) << 8); +} + +uint32_t LoadU32LE(const uint8_t* data) { + return static_cast(data[0]) | + (static_cast(data[1]) << 8) | + (static_cast(data[2]) << 16) | + (static_cast(data[3]) << 24); +} + +void StoreFloatLE(uint8_t* data, double value) { + StoreU32LE(data, std::bit_cast(static_cast(value))); +} + +double LoadFloatLE(const uint8_t* data) { + return std::bit_cast(LoadU32LE(data)); +} + +HAL_CANMessage MakeMessage(const uint8_t* data, uint8_t size) { + HAL_CANMessage message{}; + message.dataSize = size; + if (size != 0) { + std::memcpy(message.data, data, size); + } + return message; +} + +void WritePacket(const A301Obj& a301, int32_t api, const uint8_t* data, + uint8_t size, int32_t* status) { + auto message = MakeMessage(data, size); + HAL_WriteCANPacket(a301.canHandle, api, &message, status); +} + +void WritePacketRepeating(const A301Obj& a301, int32_t api, const uint8_t* data, + uint8_t size, int32_t* status) { + auto message = MakeMessage(data, size); + HAL_WriteCANPacketRepeating(a301.canHandle, api, &message, kControlPeriodMs, + status); +} + +std::array WriteAndReadPacket(A301Obj& a301, int32_t writeApi, + const uint8_t* writeData, + uint8_t writeSize, int32_t readApi, + uint8_t expectedReadSize, + int32_t readTimeoutMs, bool rtr, + int32_t* status) { + std::array data{}; + std::scoped_lock lock{a301.configurationMutex}; + + HAL_CANReceiveMessage oldMessage{}; + int32_t oldStatus = 0; + HAL_ReadCANPacketNew(a301.canHandle, readApi, &oldMessage, &oldStatus); + + int32_t lastSendStatus = 0; + for (int32_t retry = 0; retry <= kMaxReadRetries; ++retry) { + if (rtr) { + HAL_CANMessage message{}; + message.dataSize = expectedReadSize; + HAL_WriteCANRTRFrame(a301.canHandle, writeApi, &message, &lastSendStatus); + } else { + WritePacket(a301, writeApi, writeData, writeSize, &lastSendStatus); + } + if (lastSendStatus != 0) { + continue; + } + + for (int32_t elapsed = 0; elapsed <= readTimeoutMs; ++elapsed) { + HAL_CANReceiveMessage message{}; + int32_t readStatus = 0; + HAL_ReadCANPacketNew(a301.canHandle, readApi, &message, &readStatus); + if (readStatus == 0) { + auto copySize = + std::min(message.message.dataSize, data.size()); + std::memcpy(data.data(), message.message.data, copySize); + if (message.message.dataSize != expectedReadSize) { + *status = HAL_ERR_CANSessionMux_InvalidBuffer; + return data; + } + *status = 0; + return data; + } + if (elapsed != readTimeoutMs) { + std::this_thread::sleep_for(std::chrono::milliseconds{1}); + } + } + } + + *status = lastSendStatus != 0 ? lastSendStatus : HAL_CAN_TIMEOUT; + return data; +} + +std::array ReadPeriodicPacket(A301Obj& a301, int32_t api, + int32_t periodMs, uint64_t* timestamp, + int32_t* status) { + std::array data{}; + HAL_CANReceiveMessage message{}; + HAL_ReadCANPacketLatest(a301.canHandle, api, &message, status); + if (*status != 0) { + return data; + } + + *timestamp = message.timeStamp; + auto copySize = std::min(message.message.dataSize, data.size()); + std::memcpy(data.data(), message.message.data, copySize); + if (message.message.dataSize != data.size()) { + *status = HAL_ERR_CANSessionMux_InvalidBuffer; + return data; + } + + int32_t timeoutMs = + std::max(static_cast(periodMs * 2.1), kMinimumPeriodicTimeoutMs); + uint64_t now = wpi::util::Now(); + if (now >= message.timeStamp && + now - message.timeStamp > static_cast(timeoutMs) * 1000) { + *status = HAL_CAN_TIMEOUT; + } + return data; +} + +int32_t GetStatusPeriod(A301Obj& a301, HAL_A301PeriodicFrame frame, + int32_t* status) { + auto index = static_cast(frame); + if (index < 0 || index >= static_cast(a301.statusPeriods.size())) { + *status = HAL_PARAMETER_OUT_OF_RANGE; + return 0; + } + std::scoped_lock lock{a301.stateMutex}; + return a301.statusPeriods[index]; +} + +HAL_Bool Bit(uint8_t value, int bit) { + return (value >> bit) & 1; +} + +} // namespace + +namespace wpi::hal::init { +void InitializeA301() { + static UnlimitedHandleResource + handles; + gA301Handles = &handles; +} +} // namespace wpi::hal::init + +extern "C" { + +int32_t HAL_DetectA301DeviceId(int32_t busId, int32_t* status) { + wpi::hal::init::CheckInit(); + *status = 0; + + constexpr uint32_t kDeviceIdMask = 0x3f; + constexpr uint32_t kAllExceptDeviceIdMask = 0x1fffffc0; + HAL_CANStreamHandle stream = HAL_CAN_OpenStreamSession( + busId, kStatus0Id, kAllExceptDeviceIdMask, 1, status); + if (*status != 0) { + return -1; + } + + std::this_thread::sleep_for(std::chrono::milliseconds{10}); + int32_t lastStatus = HAL_ERR_CANSessionMux_MessageNotFound; + for (int retry = 0; retry < 4; ++retry) { + HAL_CANStreamMessage message{}; + uint32_t messagesRead = 0; + int32_t readStatus = 0; + HAL_CAN_ReadStreamSession(stream, &message, 1, &messagesRead, &readStatus); + if ((readStatus == 0 || + readStatus == HAL_ERR_CANSessionMux_SessionOverrun) && + messagesRead == 1) { + HAL_CAN_CloseStreamSession(stream); + *status = 0; + return message.messageId & kDeviceIdMask; + } + lastStatus = + readStatus != 0 ? readStatus : HAL_ERR_CANSessionMux_MessageNotFound; + if (retry != 3) { + std::this_thread::sleep_for(std::chrono::milliseconds{100}); + } + } + + HAL_CAN_CloseStreamSession(stream); + *status = lastStatus; + return -1; +} + +HAL_A301Handle HAL_InitializeA301(int32_t busId, int32_t deviceId, + const char* allocationLocation, + int32_t* status) { + wpi::hal::init::CheckInit(); + *status = 0; + + if (busId >= kFirstMotioncoreBus) { + int32_t detectStatus = 0; + int32_t detectedId = HAL_DetectA301DeviceId(busId, &detectStatus); + if (detectStatus == 0) { + deviceId = detectedId; + } + } + + if (!HAL_CheckA301DeviceId(deviceId)) { + *status = HAL_PARAMETER_OUT_OF_RANGE; + return HAL_INVALID_HANDLE; + } + + HAL_CANHandle canHandle = HAL_InitializeCAN( + busId, HAL_CANManufacturer::HAL_CAN_MAN_FIRST, deviceId, + HAL_CANDeviceType::HAL_CAN_DEV_MOTOR_CONTROLLER, status); + if (*status != 0) { + return HAL_INVALID_HANDLE; + } + + uint32_t allocationKey = MakeAllocationKey(busId, deviceId); + { + std::scoped_lock lock{gAllocationsMutex}; + auto [it, inserted] = gAllocations.try_emplace( + allocationKey, allocationLocation ? allocationLocation : ""); + if (!inserted) { + HAL_CleanCAN(canHandle); + *status = MakeErrorPreviouslyAllocated(HAL_RESOURCE_IS_ALLOCATED, "A301", + deviceId, it->second); + return HAL_INVALID_HANDLE; + } + } + + auto a301 = std::make_shared(); + a301->busId = busId; + a301->deviceId = deviceId; + a301->canHandle = canHandle; + a301->previousAllocation = allocationLocation ? allocationLocation : ""; + + HAL_A301Handle handle = gA301Handles->Allocate(a301); + if (handle == HAL_INVALID_HANDLE) { + *status = HAL_NO_AVAILABLE_RESOURCES; + return HAL_INVALID_HANDLE; + } + + for (int32_t api : kControlApis) { + int32_t stopStatus = 0; + HAL_StopCANPacketRepeating(canHandle, api, &stopStatus); + } + + std::array zeroSetpoint{}; + int32_t setpointStatus = 0; + WritePacketRepeating(*a301, kDutyCycleSetpointApi, zeroSetpoint.data(), + zeroSetpoint.size(), &setpointStatus); + return handle; +} + +void HAL_FreeA301(HAL_A301Handle handle) { + gA301Handles->Free(handle); +} + +HAL_Bool HAL_CheckA301DeviceId(int32_t deviceId) { + return deviceId >= HAL_A301_MIN_DEVICE_ID && + deviceId <= HAL_A301_MAX_DEVICE_ID; +} + +int32_t HAL_GetA301BusId(HAL_A301Handle handle, int32_t* status) { + auto a301 = GetA301(handle, status); + return a301 ? a301->busId : -1; +} + +int32_t HAL_GetA301DeviceId(HAL_A301Handle handle, int32_t* status) { + auto a301 = GetA301(handle, status); + return a301 ? a301->deviceId : -1; +} + +void HAL_GetA301FirmwareVersion(HAL_A301Handle handle, + HAL_A301FirmwareVersion* version, + int32_t* status) { + if (!version) { + *status = HAL_NULL_PARAMETER; + return; + } + *version = {}; + auto a301 = GetA301(handle, status); + if (!a301) { + return; + } + + auto data = WriteAndReadPacket(*a301, kGetFirmwareVersionApi, nullptr, 0, + kGetFirmwareVersionApi, 8, + kDefaultReadTimeoutMs, true, status); + version->major = data[0]; + version->minor = data[1]; + version->patch = (static_cast(data[2]) << 8) | data[3]; + version->prerelease = data[4]; + version->hardwareRevision = data[5]; + version->raw = (static_cast(version->major) << 24) | + (static_cast(version->minor) << 16) | version->patch; +} + +void HAL_GetA301PeriodicStatus0(HAL_A301Handle handle, + HAL_A301PeriodicStatus0* frame, + int32_t* status) { + if (!frame) { + *status = HAL_NULL_PARAMETER; + return; + } + *frame = {}; + auto a301 = GetA301(handle, status); + if (!a301) { + return; + } + + bool inverted; + int32_t periodMs; + { + std::scoped_lock lock{a301->stateMutex}; + inverted = a301->inverted; + periodMs = a301->statusPeriods[0]; + } + auto data = ReadPeriodicPacket(*a301, kStatus0Api, periodMs, + &frame->timestamp, status); + int16_t appliedOutput = static_cast(LoadU16LE(data.data())); + uint16_t voltage = static_cast(data[2]) | + (static_cast(data[3] & 0x0f) << 8); + uint16_t current = static_cast(data[3] >> 4) | + (static_cast(data[4]) << 4); + frame->appliedOutput = + appliedOutput * 3.082369457075716e-05 * (inverted ? -1.0 : 1.0); + frame->voltage = voltage * 0.0073260073260073; + frame->current = current * 0.0366300366300366; + frame->motorTemperature = data[5]; + frame->inverted = inverted; + frame->primaryHeartbeatLock = data[6] & 0x01; + frame->gearboxRPM = static_cast((data[6] >> 1) & 0x0f); +} + +void HAL_GetA301PeriodicStatus1(HAL_A301Handle handle, + HAL_A301PeriodicStatus1* frame, + int32_t* status) { + if (!frame) { + *status = HAL_NULL_PARAMETER; + return; + } + *frame = {}; + auto a301 = GetA301(handle, status); + if (!a301) { + return; + } + + int32_t periodMs = GetStatusPeriod(*a301, HAL_A301_STATUS_1, status); + if (*status != 0) { + return; + } + auto data = ReadPeriodicPacket(*a301, kStatus1Api, periodMs, + &frame->timestamp, status); + frame->otherFault = Bit(data[0], 0); + frame->motorTypeFault = Bit(data[0], 1); + frame->sensorFault = Bit(data[0], 2); + frame->canFault = Bit(data[0], 3); + frame->temperatureFault = Bit(data[0], 4); + frame->drvFault = Bit(data[0], 5); + frame->escEepromFault = Bit(data[0], 6); + frame->firmwareFault = Bit(data[0], 7); + frame->motorStartupFault = Bit(data[1], 0); + frame->overvoltageWarning = Bit(data[1], 6); + frame->motorLoopSpeedWarning = Bit(data[1], 7); + frame->brownoutWarning = Bit(data[2], 0); + frame->overcurrentWarning = Bit(data[2], 1); + frame->escEepromWarning = Bit(data[2], 2); + frame->extEepromWarning = Bit(data[2], 3); + frame->sensorWarning = Bit(data[2], 4); + frame->stallWarning = Bit(data[2], 5); + frame->hasResetWarning = Bit(data[2], 6); + frame->otherWarning = Bit(data[2], 7); + frame->otherStickyFault = Bit(data[3], 0); + frame->motorTypeStickyFault = Bit(data[3], 1); + frame->sensorStickyFault = Bit(data[3], 2); + frame->canStickyFault = Bit(data[3], 3); + frame->temperatureStickyFault = Bit(data[3], 4); + frame->drvStickyFault = Bit(data[3], 5); + frame->escEepromStickyFault = Bit(data[3], 6); + frame->firmwareStickyFault = Bit(data[3], 7); + frame->motorStartupStickyFault = Bit(data[4], 0); + frame->overvoltageStickyWarning = Bit(data[4], 6); + frame->motorLoopSpeedStickyWarning = Bit(data[4], 7); + frame->brownoutStickyWarning = Bit(data[5], 0); + frame->overcurrentStickyWarning = Bit(data[5], 1); + frame->escEepromStickyWarning = Bit(data[5], 2); + frame->extEepromStickyWarning = Bit(data[5], 3); + frame->sensorStickyWarning = Bit(data[5], 4); + frame->stallStickyWarning = Bit(data[5], 5); + frame->hasResetStickyWarning = Bit(data[5], 6); + frame->otherStickyWarning = Bit(data[5], 7); + frame->isFollower = Bit(data[6], 0); +} + +void HAL_GetA301PeriodicStatus2(HAL_A301Handle handle, + HAL_A301PeriodicStatus2* frame, + int32_t* status) { + if (!frame) { + *status = HAL_NULL_PARAMETER; + return; + } + *frame = {}; + auto a301 = GetA301(handle, status); + if (!a301) { + return; + } + + bool inverted; + int32_t periodMs; + { + std::scoped_lock lock{a301->stateMutex}; + inverted = a301->inverted; + periodMs = a301->statusPeriods[2]; + } + auto data = ReadPeriodicPacket(*a301, kStatus2Api, periodMs, + &frame->timestamp, status); + frame->encoderVelocity = LoadFloatLE(data.data()) * (inverted ? -1.0 : 1.0); + frame->relativeEncoderPosition = LoadFloatLE(data.data() + 4); +} + +void HAL_GetA301PeriodicStatus3(HAL_A301Handle handle, + HAL_A301PeriodicStatus3* frame, + int32_t* status) { + if (!frame) { + *status = HAL_NULL_PARAMETER; + return; + } + *frame = {}; + auto a301 = GetA301(handle, status); + if (!a301) { + return; + } + + int32_t periodMs = GetStatusPeriod(*a301, HAL_A301_STATUS_3, status); + if (*status != 0) { + return; + } + auto data = ReadPeriodicPacket(*a301, kStatus3Api, periodMs, + &frame->timestamp, status); + frame->absoluteEncoderPosition = LoadFloatLE(data.data() + 4); +} + +void HAL_SetA301RelativeEncoderPosition(HAL_A301Handle handle, double position, + int32_t* status) { + auto a301 = GetA301(handle, status); + if (!a301) { + return; + } + std::array data{}; + StoreFloatLE(data.data(), position); + WritePacket(*a301, kSetRelativeEncoderPositionApi, data.data(), data.size(), + status); +} + +void HAL_SetA301AbsoluteEncoderPosition(HAL_A301Handle handle, double position, + int32_t* status) { + auto a301 = GetA301(handle, status); + if (!a301) { + return; + } + std::array data{}; + StoreFloatLE(data.data(), position); + WritePacket(*a301, kSetAbsoluteEncoderPositionApi, data.data(), data.size(), + status); +} + +void HAL_SetA301Setpoint(HAL_A301Handle handle, double value, + HAL_A301ControlType controlType, double positionSpeed, + int32_t* status) { + auto a301 = GetA301(handle, status); + if (!a301) { + return; + } + + auto controlIndex = static_cast(controlType); + constexpr std::array kControlTypeApis{ + kDutyCycleSetpointApi, kVelocitySetpointApi, + kDutyCycleSetpointApi, kRelativePositionSetpointApi, + kAbsolutePositionSetpointApi, kCurrentSetpointApi}; + if (controlIndex < 0 || + controlIndex >= static_cast(kControlTypeApis.size())) { + *status = HAL_PARAMETER_OUT_OF_RANGE; + return; + } + + if (controlType == HAL_A301_CONTROL_TYPE_VOLTAGE) { + value /= 12.0; + } + + int32_t api = kControlTypeApis[controlIndex]; + std::array data{}; + { + std::scoped_lock lock{a301->stateMutex}; + // Position targets use the same coordinate system as the encoder position + // setters and periodic position feedback, which are not inverted. + if (a301->inverted && + controlType != HAL_A301_CONTROL_TYPE_RELATIVE_POSITION && + controlType != HAL_A301_CONTROL_TYPE_ABSOLUTE_POSITION) { + value *= -1.0; + } + if (a301->activeSetpointApi != api) { + int32_t stopStatus = 0; + HAL_StopCANPacketRepeating(a301->canHandle, a301->activeSetpointApi, + &stopStatus); + if (stopStatus != 0) { + *status = stopStatus; + return; + } + } + a301->activeSetpointApi = api; + StoreFloatLE(data.data(), value); + StoreFloatLE(data.data() + 4, positionSpeed); + WritePacketRepeating(*a301, api, data.data(), data.size(), status); + } +} + +void HAL_SetA301IdleMode(HAL_A301Handle handle, HAL_A301IdleMode idleMode, + int32_t* status) { + auto a301 = GetA301(handle, status); + if (!a301) { + return; + } + if (idleMode != HAL_A301_IDLE_MODE_COAST && + idleMode != HAL_A301_IDLE_MODE_BRAKE) { + *status = HAL_PARAMETER_OUT_OF_RANGE; + return; + } + uint8_t data[] = {static_cast(idleMode)}; + WritePacket(*a301, kSetIdleModeApi, data, sizeof(data), status); +} + +HAL_A301IdleMode HAL_GetA301IdleMode(HAL_A301Handle handle, int32_t* status) { + auto a301 = GetA301(handle, status); + if (!a301) { + return HAL_A301_IDLE_MODE_COAST; + } + auto data = WriteAndReadPacket(*a301, kGetIdleModeApi, nullptr, 0, + kGetIdleModeResponseApi, 1, + kConfigurationReadTimeoutMs, false, status); + return static_cast(data[0]); +} + +void HAL_SetA301AbsolutePositionContinuousInput(HAL_A301Handle handle, + HAL_Bool enabled, + int32_t* status) { + auto a301 = GetA301(handle, status); + if (!a301) { + return; + } + uint8_t data[] = {static_cast(enabled != 0)}; + WritePacket(*a301, kSetContinuousInputApi, data, sizeof(data), status); +} + +HAL_Bool HAL_GetA301AbsolutePositionContinuousInput(HAL_A301Handle handle, + int32_t* status) { + auto a301 = GetA301(handle, status); + if (!a301) { + return false; + } + auto data = WriteAndReadPacket(*a301, kGetContinuousInputApi, nullptr, 0, + kGetContinuousInputResponseApi, 1, + kConfigurationReadTimeoutMs, false, status); + return data[0] & 1; +} + +void HAL_SetA301AbsoluteEncoderRangeOffset(HAL_A301Handle handle, double offset, + int32_t* status) { + auto a301 = GetA301(handle, status); + if (!a301) { + return; + } + if (!std::isfinite(offset) || offset < -0.5 || offset > 0.5) { + *status = HAL_PARAMETER_OUT_OF_RANGE; + return; + } + { + std::scoped_lock lock{a301->stateMutex}; + if (a301->inverted) { + offset *= -1.0; + } + } + std::array data{}; + StoreFloatLE(data.data(), offset); + WritePacket(*a301, kSetAbsoluteRangeOffsetApi, data.data(), data.size(), + status); +} + +double HAL_GetA301AbsoluteEncoderRangeOffset(HAL_A301Handle handle, + int32_t* status) { + auto a301 = GetA301(handle, status); + if (!a301) { + return 0.0; + } + auto data = WriteAndReadPacket(*a301, kGetAbsoluteRangeOffsetApi, nullptr, 0, + kGetAbsoluteRangeOffsetResponseApi, 4, + kConfigurationReadTimeoutMs, false, status); + double offset = LoadFloatLE(data.data()); + { + std::scoped_lock lock{a301->stateMutex}; + if (a301->inverted) { + offset *= -1.0; + } + } + return offset; +} + +void HAL_SetA301Inverted(HAL_A301Handle handle, HAL_Bool inverted, + int32_t* status) { + auto a301 = GetA301(handle, status); + if (!a301) { + return; + } + std::scoped_lock lock{a301->stateMutex}; + a301->inverted = inverted; +} + +HAL_Bool HAL_GetA301Inverted(HAL_A301Handle handle, int32_t* status) { + auto a301 = GetA301(handle, status); + if (!a301) { + return false; + } + std::scoped_lock lock{a301->stateMutex}; + return a301->inverted; +} + +void HAL_ClearA301Faults(HAL_A301Handle handle, int32_t* status) { + auto a301 = GetA301(handle, status); + if (!a301) { + return; + } + WritePacket(*a301, kClearFaultsApi, nullptr, 0, status); +} + +void HAL_SetA301StatusFramePeriod(HAL_A301Handle handle, + HAL_A301PeriodicFrame frame, int32_t periodMs, + int32_t* status) { + auto a301 = GetA301(handle, status); + if (!a301) { + return; + } + auto frameIndex = static_cast(frame); + if (frameIndex < 0 || + frameIndex >= static_cast(a301->statusPeriods.size()) || + periodMs < 0) { + *status = HAL_PARAMETER_OUT_OF_RANGE; + return; + } + periodMs = std::min(periodMs, kMaxStatusPeriodMs); + + std::array request{}; + request[0] = frameIndex; + StoreU32LE(request.data() + 1, periodMs); + auto response = + WriteAndReadPacket(*a301, kSetStatusPeriodApi, request.data(), + request.size(), kSetStatusPeriodResponseApi, 6, + kConfigurationReadTimeoutMs, false, status); + if (*status != 0) { + return; + } + if (response[0] != 0 || response[1] != frameIndex) { + *status = MakeError(HAL_INCOMPATIBLE_STATE, + "A301 rejected the status frame period"); + return; + } + + int32_t effectivePeriodMs = LoadU32LE(response.data() + 2); + std::scoped_lock lock{a301->stateMutex}; + a301->statusPeriods[frameIndex] = effectivePeriodMs; +} + +int32_t HAL_GetA301StatusFramePeriod(HAL_A301Handle handle, + HAL_A301PeriodicFrame frame, + int32_t* status) { + auto a301 = GetA301(handle, status); + if (!a301) { + return 0; + } + auto frameIndex = static_cast(frame); + if (frameIndex < 0 || + frameIndex >= static_cast(a301->statusPeriods.size())) { + *status = HAL_PARAMETER_OUT_OF_RANGE; + return 0; + } + + uint8_t request[] = {static_cast(frameIndex)}; + auto response = + WriteAndReadPacket(*a301, kGetStatusPeriodApi, request, sizeof(request), + kGetStatusPeriodResponseApi, 5, + kConfigurationReadTimeoutMs, false, status); + if (*status != 0) { + return 0; + } + if (response[0] != frameIndex) { + *status = MakeError(HAL_INCOMPATIBLE_STATE, + "A301 returned a mismatched status frame"); + return 0; + } + int32_t periodMs = LoadU32LE(response.data() + 1); + { + std::scoped_lock lock{a301->stateMutex}; + a301->statusPeriods[frameIndex] = periodMs; + } + return periodMs; +} + +} // extern "C" diff --git a/hal/src/main/native/cpp/HALInitializer.hpp b/hal/src/main/native/cpp/HALInitializer.hpp new file mode 100644 index 00000000000..9d73898880d --- /dev/null +++ b/hal/src/main/native/cpp/HALInitializer.hpp @@ -0,0 +1,18 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +namespace wpi::hal::init { +extern std::atomic_bool HAL_IsInitialized; +extern void RunInitialize(); +inline void CheckInit() { + if (HAL_IsInitialized.load(std::memory_order_relaxed)) { + return; + } + RunInitialize(); +} +} // namespace wpi::hal::init diff --git a/hal/src/main/native/cpp/handles/HandlesInternal.cpp b/hal/src/main/native/cpp/handles/HandlesInternal.cpp index 48135338470..70ca5356e35 100644 --- a/hal/src/main/native/cpp/handles/HandlesInternal.cpp +++ b/hal/src/main/native/cpp/handles/HandlesInternal.cpp @@ -41,10 +41,10 @@ void HandleBase::ResetHandles() { } void HandleBase::ResetGlobalHandles() { std::unique_lock lock(globalHandleMutex); - for (auto&& i : *globalHandles) { - if (i != nullptr) { + for (auto i = globalHandles->rbegin(); i != globalHandles->rend(); ++i) { + if (*i != nullptr) { lock.unlock(); - i->ResetHandles(); + (*i)->ResetHandles(); lock.lock(); } } diff --git a/hal/src/main/native/cpp/jni/A301JNI.cpp b/hal/src/main/native/cpp/jni/A301JNI.cpp new file mode 100644 index 00000000000..b1be0ca01ea --- /dev/null +++ b/hal/src/main/native/cpp/jni/A301JNI.cpp @@ -0,0 +1,521 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include "HALUtil.hpp" +#include "org_wpilib_hardware_hal_A301JNI.h" +#include "wpi/hal/A301.h" +#include "wpi/util/jni_util.hpp" + +using namespace wpi::hal; + +static_assert(org_wpilib_hardware_hal_A301JNI_MIN_DEVICE_ID == + HAL_A301_MIN_DEVICE_ID); +static_assert(org_wpilib_hardware_hal_A301JNI_MAX_DEVICE_ID == + HAL_A301_MAX_DEVICE_ID); +static_assert(org_wpilib_hardware_hal_A301JNI_DEFAULT_DEVICE_ID == + HAL_A301_DEFAULT_DEVICE_ID); +static_assert(org_wpilib_hardware_hal_A301JNI_STATUS_0 == HAL_A301_STATUS_0); +static_assert(org_wpilib_hardware_hal_A301JNI_STATUS_1 == HAL_A301_STATUS_1); +static_assert(org_wpilib_hardware_hal_A301JNI_STATUS_2 == HAL_A301_STATUS_2); +static_assert(org_wpilib_hardware_hal_A301JNI_STATUS_3 == HAL_A301_STATUS_3); +static_assert(org_wpilib_hardware_hal_A301JNI_GEARBOX_RPM_UNKNOWN == + HAL_A301_GEARBOX_RPM_UNKNOWN); +static_assert(org_wpilib_hardware_hal_A301JNI_GEARBOX_RPM_215 == + HAL_A301_GEARBOX_RPM_215); +static_assert(org_wpilib_hardware_hal_A301JNI_GEARBOX_RPM_500 == + HAL_A301_GEARBOX_RPM_500); +static_assert(org_wpilib_hardware_hal_A301JNI_IDLE_MODE_COAST == + HAL_A301_IDLE_MODE_COAST); +static_assert(org_wpilib_hardware_hal_A301JNI_IDLE_MODE_BRAKE == + HAL_A301_IDLE_MODE_BRAKE); +static_assert(org_wpilib_hardware_hal_A301JNI_CONTROL_TYPE_DUTY_CYCLE == + HAL_A301_CONTROL_TYPE_DUTY_CYCLE); +static_assert(org_wpilib_hardware_hal_A301JNI_CONTROL_TYPE_VELOCITY == + HAL_A301_CONTROL_TYPE_VELOCITY); +static_assert(org_wpilib_hardware_hal_A301JNI_CONTROL_TYPE_VOLTAGE == + HAL_A301_CONTROL_TYPE_VOLTAGE); +static_assert(org_wpilib_hardware_hal_A301JNI_CONTROL_TYPE_RELATIVE_POSITION == + HAL_A301_CONTROL_TYPE_RELATIVE_POSITION); +static_assert(org_wpilib_hardware_hal_A301JNI_CONTROL_TYPE_ABSOLUTE_POSITION == + HAL_A301_CONTROL_TYPE_ABSOLUTE_POSITION); +static_assert(org_wpilib_hardware_hal_A301JNI_CONTROL_TYPE_CURRENT == + HAL_A301_CONTROL_TYPE_CURRENT); + +namespace { + +jobject MakeFirmwareVersion(JNIEnv* env, + const HAL_A301FirmwareVersion& version) { + jclass cls = env->FindClass("org/wpilib/hardware/hal/A301FirmwareVersion"); + if (!cls) { + return nullptr; + } + jmethodID constructor = env->GetMethodID(cls, "", "(IIIIII)V"); + jobject result = + env->NewObject(cls, constructor, version.major, version.minor, + version.patch, version.prerelease, + version.hardwareRevision, static_cast(version.raw)); + env->DeleteLocalRef(cls); + return result; +} + +jobject MakePeriodicStatus0(JNIEnv* env, const HAL_A301PeriodicStatus0& frame, + int32_t status) { + jclass cls = env->FindClass("org/wpilib/hardware/hal/A301PeriodicStatus0"); + if (!cls) { + return nullptr; + } + jmethodID constructor = env->GetMethodID(cls, "", "(DDDIZZIIJ)V"); + jobject result = env->NewObject( + cls, constructor, frame.appliedOutput, frame.voltage, frame.current, + frame.motorTemperature, static_cast(frame.inverted), + static_cast(frame.primaryHeartbeatLock), + static_cast(frame.gearboxRPM), status, + static_cast(frame.timestamp)); + env->DeleteLocalRef(cls); + return result; +} + +jobject MakePeriodicStatus1(JNIEnv* env, const HAL_A301PeriodicStatus1& frame, + int32_t status) { + int32_t faults = frame.otherFault | (frame.motorTypeFault << 1) | + (frame.sensorFault << 2) | (frame.canFault << 3) | + (frame.temperatureFault << 4) | (frame.drvFault << 5) | + (frame.escEepromFault << 6) | (frame.firmwareFault << 7) | + (frame.motorStartupFault << 8); + int32_t warnings = + frame.brownoutWarning | (frame.overcurrentWarning << 1) | + (frame.escEepromWarning << 2) | (frame.extEepromWarning << 3) | + (frame.sensorWarning << 4) | (frame.stallWarning << 5) | + (frame.hasResetWarning << 6) | (frame.otherWarning << 7) | + (frame.overvoltageWarning << 8) | (frame.motorLoopSpeedWarning << 9); + int32_t stickyFaults = + frame.otherStickyFault | (frame.motorTypeStickyFault << 1) | + (frame.sensorStickyFault << 2) | (frame.canStickyFault << 3) | + (frame.temperatureStickyFault << 4) | (frame.drvStickyFault << 5) | + (frame.escEepromStickyFault << 6) | (frame.firmwareStickyFault << 7) | + (frame.motorStartupStickyFault << 8); + int32_t stickyWarnings = + frame.brownoutStickyWarning | (frame.overcurrentStickyWarning << 1) | + (frame.escEepromStickyWarning << 2) | + (frame.extEepromStickyWarning << 3) | (frame.sensorStickyWarning << 4) | + (frame.stallStickyWarning << 5) | (frame.hasResetStickyWarning << 6) | + (frame.otherStickyWarning << 7) | (frame.overvoltageStickyWarning << 8) | + (frame.motorLoopSpeedStickyWarning << 9); + + jclass cls = env->FindClass("org/wpilib/hardware/hal/A301PeriodicStatus1"); + if (!cls) { + return nullptr; + } + jmethodID constructor = env->GetMethodID(cls, "", "(IIIIZIJ)V"); + jobject result = + env->NewObject(cls, constructor, faults, warnings, stickyFaults, + stickyWarnings, static_cast(frame.isFollower), + status, static_cast(frame.timestamp)); + env->DeleteLocalRef(cls); + return result; +} + +jobject MakePeriodicStatus2(JNIEnv* env, const HAL_A301PeriodicStatus2& frame, + int32_t status) { + jclass cls = env->FindClass("org/wpilib/hardware/hal/A301PeriodicStatus2"); + if (!cls) { + return nullptr; + } + jmethodID constructor = env->GetMethodID(cls, "", "(DDIJ)V"); + jobject result = env->NewObject(cls, constructor, frame.encoderVelocity, + frame.relativeEncoderPosition, status, + static_cast(frame.timestamp)); + env->DeleteLocalRef(cls); + return result; +} + +jobject MakePeriodicStatus3(JNIEnv* env, const HAL_A301PeriodicStatus3& frame, + int32_t status) { + jclass cls = env->FindClass("org/wpilib/hardware/hal/A301PeriodicStatus3"); + if (!cls) { + return nullptr; + } + jmethodID constructor = env->GetMethodID(cls, "", "(DIJ)V"); + jobject result = + env->NewObject(cls, constructor, frame.absoluteEncoderPosition, status, + static_cast(frame.timestamp)); + env->DeleteLocalRef(cls); + return result; +} + +} // namespace + +extern "C" { + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: detectDeviceId + * Signature: (I)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_A301JNI_detectDeviceId + (JNIEnv* env, jclass, jint busId) +{ + int32_t status = 0; + int32_t deviceId = HAL_DetectA301DeviceId(busId, &status); + CheckStatusForceThrow(env, status); + return deviceId; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: initialize + * Signature: (II)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_A301JNI_initialize + (JNIEnv* env, jclass, jint busId, jint deviceId) +{ + int32_t status = 0; + auto stack = wpi::util::java::GetJavaStackTrace(env, "org.wpilib"); + HAL_A301Handle handle = + HAL_InitializeA301(busId, deviceId, stack.c_str(), &status); + CheckStatusForceThrow(env, status); + return handle; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: free + * Signature: (I)V + */ +JNIEXPORT void JNICALL +Java_org_wpilib_hardware_hal_A301JNI_free + (JNIEnv*, jclass, jint handle) +{ + HAL_FreeA301(handle); +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: checkDeviceId + * Signature: (I)Z + */ +JNIEXPORT jboolean JNICALL +Java_org_wpilib_hardware_hal_A301JNI_checkDeviceId + (JNIEnv*, jclass, jint deviceId) +{ + return HAL_CheckA301DeviceId(deviceId); +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: getBusId + * Signature: (I)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_A301JNI_getBusId + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + int32_t result = HAL_GetA301BusId(handle, &status); + CheckStatus(env, status, false); + return result; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: getDeviceId + * Signature: (I)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_A301JNI_getDeviceId + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + int32_t result = HAL_GetA301DeviceId(handle, &status); + CheckStatus(env, status, false); + return result; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: getFirmwareVersion + * Signature: (I)Ljava/lang/Object; + */ +JNIEXPORT jobject JNICALL +Java_org_wpilib_hardware_hal_A301JNI_getFirmwareVersion + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + HAL_A301FirmwareVersion version{}; + HAL_GetA301FirmwareVersion(handle, &version, &status); + CheckStatus(env, status, false); + return MakeFirmwareVersion(env, version); +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: getPeriodicStatus0 + * Signature: (I)Ljava/lang/Object; + */ +JNIEXPORT jobject JNICALL +Java_org_wpilib_hardware_hal_A301JNI_getPeriodicStatus0 + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + HAL_A301PeriodicStatus0 frame{}; + HAL_GetA301PeriodicStatus0(handle, &frame, &status); + return MakePeriodicStatus0(env, frame, status); +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: getPeriodicStatus1 + * Signature: (I)Ljava/lang/Object; + */ +JNIEXPORT jobject JNICALL +Java_org_wpilib_hardware_hal_A301JNI_getPeriodicStatus1 + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + HAL_A301PeriodicStatus1 frame{}; + HAL_GetA301PeriodicStatus1(handle, &frame, &status); + return MakePeriodicStatus1(env, frame, status); +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: getPeriodicStatus2 + * Signature: (I)Ljava/lang/Object; + */ +JNIEXPORT jobject JNICALL +Java_org_wpilib_hardware_hal_A301JNI_getPeriodicStatus2 + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + HAL_A301PeriodicStatus2 frame{}; + HAL_GetA301PeriodicStatus2(handle, &frame, &status); + return MakePeriodicStatus2(env, frame, status); +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: getPeriodicStatus3 + * Signature: (I)Ljava/lang/Object; + */ +JNIEXPORT jobject JNICALL +Java_org_wpilib_hardware_hal_A301JNI_getPeriodicStatus3 + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + HAL_A301PeriodicStatus3 frame{}; + HAL_GetA301PeriodicStatus3(handle, &frame, &status); + return MakePeriodicStatus3(env, frame, status); +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: setRelativeEncoderPosition + * Signature: (ID)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_A301JNI_setRelativeEncoderPosition + (JNIEnv*, jclass, jint handle, jdouble position) +{ + int32_t status = 0; + HAL_SetA301RelativeEncoderPosition(handle, position, &status); + return status; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: setAbsoluteEncoderPosition + * Signature: (ID)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_A301JNI_setAbsoluteEncoderPosition + (JNIEnv*, jclass, jint handle, jdouble position) +{ + int32_t status = 0; + HAL_SetA301AbsoluteEncoderPosition(handle, position, &status); + return status; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: setSetpoint + * Signature: (IDID)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_A301JNI_setSetpoint + (JNIEnv*, jclass, jint handle, jdouble value, jint controlType, + jdouble positionSpeed) +{ + int32_t status = 0; + HAL_SetA301Setpoint(handle, value, + static_cast(controlType), + positionSpeed, &status); + return status; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: setIdleMode + * Signature: (II)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_A301JNI_setIdleMode + (JNIEnv*, jclass, jint handle, jint idleMode) +{ + int32_t status = 0; + HAL_SetA301IdleMode(handle, static_cast(idleMode), &status); + return status; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: getIdleMode + * Signature: (I)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_A301JNI_getIdleMode + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + auto result = HAL_GetA301IdleMode(handle, &status); + CheckStatus(env, status, false); + return result; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: setAbsolutePositionContinuousInput + * Signature: (IZ)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_A301JNI_setAbsolutePositionContinuousInput + (JNIEnv*, jclass, jint handle, jboolean enabled) +{ + int32_t status = 0; + HAL_SetA301AbsolutePositionContinuousInput(handle, enabled, &status); + return status; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: getAbsolutePositionContinuousInput + * Signature: (I)Z + */ +JNIEXPORT jboolean JNICALL +Java_org_wpilib_hardware_hal_A301JNI_getAbsolutePositionContinuousInput + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + auto result = HAL_GetA301AbsolutePositionContinuousInput(handle, &status); + CheckStatus(env, status, false); + return result; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: setAbsoluteEncoderRangeOffset + * Signature: (ID)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_A301JNI_setAbsoluteEncoderRangeOffset + (JNIEnv*, jclass, jint handle, jdouble offset) +{ + int32_t status = 0; + HAL_SetA301AbsoluteEncoderRangeOffset(handle, offset, &status); + return status; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: getAbsoluteEncoderRangeOffset + * Signature: (I)D + */ +JNIEXPORT jdouble JNICALL +Java_org_wpilib_hardware_hal_A301JNI_getAbsoluteEncoderRangeOffset + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + auto result = HAL_GetA301AbsoluteEncoderRangeOffset(handle, &status); + CheckStatus(env, status, false); + return result; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: setInverted + * Signature: (IZ)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_A301JNI_setInverted + (JNIEnv*, jclass, jint handle, jboolean inverted) +{ + int32_t status = 0; + HAL_SetA301Inverted(handle, inverted, &status); + return status; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: getInverted + * Signature: (I)Z + */ +JNIEXPORT jboolean JNICALL +Java_org_wpilib_hardware_hal_A301JNI_getInverted + (JNIEnv* env, jclass, jint handle) +{ + int32_t status = 0; + auto result = HAL_GetA301Inverted(handle, &status); + CheckStatus(env, status, false); + return result; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: clearFaults + * Signature: (I)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_A301JNI_clearFaults + (JNIEnv*, jclass, jint handle) +{ + int32_t status = 0; + HAL_ClearA301Faults(handle, &status); + return status; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: setStatusFramePeriod + * Signature: (III)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_A301JNI_setStatusFramePeriod + (JNIEnv*, jclass, jint handle, jint frame, jint periodMs) +{ + int32_t status = 0; + HAL_SetA301StatusFramePeriod( + handle, static_cast(frame), periodMs, &status); + return status; +} + +/* + * Class: org_wpilib_hardware_hal_A301JNI + * Method: getStatusFramePeriod + * Signature: (II)I + */ +JNIEXPORT jint JNICALL +Java_org_wpilib_hardware_hal_A301JNI_getStatusFramePeriod + (JNIEnv* env, jclass, jint handle, jint frame) +{ + int32_t status = 0; + int32_t result = HAL_GetA301StatusFramePeriod( + handle, static_cast(frame), &status); + CheckStatus(env, status, false); + return result; +} + +} // extern "C" diff --git a/hal/src/main/native/include/wpi/hal/A301.h b/hal/src/main/native/include/wpi/hal/A301.h new file mode 100644 index 00000000000..acba51667a5 --- /dev/null +++ b/hal/src/main/native/include/wpi/hal/A301.h @@ -0,0 +1,421 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "wpi/hal/Types.h" + +/** + * @defgroup hal_a301 FIRST A301 Motor Controller Functions + * @ingroup hal_capi + * @{ + */ + +/** Minimum device ID accepted by an A301. */ +#define HAL_A301_MIN_DEVICE_ID 0 + +/** Maximum device ID accepted by an A301. */ +#define HAL_A301_MAX_DEVICE_ID 62 + +/** The factory-default A301 device ID. */ +#define HAL_A301_DEFAULT_DEVICE_ID 3 + +/** A301 periodic status frames. */ +HAL_ENUM(HAL_A301PeriodicFrame) { + /** Applied output, voltage, current, temperature, and controller state. */ + HAL_A301_STATUS_0 = 0, + /** Fault and warning state. */ + HAL_A301_STATUS_1 = 1, + /** Relative encoder position and velocity. */ + HAL_A301_STATUS_2 = 2, + /** Absolute encoder position. */ + HAL_A301_STATUS_3 = 3, +}; + +/** A301 gearbox speed variants. */ +HAL_ENUM(HAL_A301GearboxRPM) { + /** The gearbox speed is unknown. */ + HAL_A301_GEARBOX_RPM_UNKNOWN = 0, + /** 215 RPM gearbox. */ + HAL_A301_GEARBOX_RPM_215 = 1, + /** 500 RPM gearbox. */ + HAL_A301_GEARBOX_RPM_500 = 2, +}; + +/** A301 idle modes. */ +HAL_ENUM(HAL_A301IdleMode) { + /** Allow the motor to coast. */ + HAL_A301_IDLE_MODE_COAST = 0, + /** Actively brake the motor. */ + HAL_A301_IDLE_MODE_BRAKE = 1, +}; + +/** A301 closed-loop control types. */ +HAL_ENUM(HAL_A301ControlType) { + /** Duty-cycle control. */ + HAL_A301_CONTROL_TYPE_DUTY_CYCLE = 0, + /** Velocity control. */ + HAL_A301_CONTROL_TYPE_VELOCITY = 1, + /** Voltage control. */ + HAL_A301_CONTROL_TYPE_VOLTAGE = 2, + /** Relative-position control. */ + HAL_A301_CONTROL_TYPE_RELATIVE_POSITION = 3, + /** Absolute-position control. */ + HAL_A301_CONTROL_TYPE_ABSOLUTE_POSITION = 4, + /** Motor-current control. */ + HAL_A301_CONTROL_TYPE_CURRENT = 5, +}; + +/** A301 firmware version information. */ +struct HAL_A301FirmwareVersion { + uint8_t major; + uint8_t minor; + uint16_t patch; + uint8_t prerelease; + uint8_t hardwareRevision; + uint32_t raw; +}; + +/** A301 periodic status frame 0. */ +struct HAL_A301PeriodicStatus0 { + double appliedOutput; + double voltage; + double current; + int32_t motorTemperature; + HAL_Bool inverted; + HAL_Bool primaryHeartbeatLock; + HAL_A301GearboxRPM gearboxRPM; + uint64_t timestamp; +}; + +/** A301 periodic status frame 1. */ +struct HAL_A301PeriodicStatus1 { + HAL_Bool otherFault; + HAL_Bool motorTypeFault; + HAL_Bool sensorFault; + HAL_Bool canFault; + HAL_Bool temperatureFault; + HAL_Bool drvFault; + HAL_Bool escEepromFault; + HAL_Bool firmwareFault; + HAL_Bool motorStartupFault; + + HAL_Bool brownoutWarning; + HAL_Bool overcurrentWarning; + HAL_Bool escEepromWarning; + HAL_Bool extEepromWarning; + HAL_Bool sensorWarning; + HAL_Bool stallWarning; + HAL_Bool hasResetWarning; + HAL_Bool otherWarning; + HAL_Bool overvoltageWarning; + HAL_Bool motorLoopSpeedWarning; + + HAL_Bool otherStickyFault; + HAL_Bool motorTypeStickyFault; + HAL_Bool sensorStickyFault; + HAL_Bool canStickyFault; + HAL_Bool temperatureStickyFault; + HAL_Bool drvStickyFault; + HAL_Bool escEepromStickyFault; + HAL_Bool firmwareStickyFault; + HAL_Bool motorStartupStickyFault; + + HAL_Bool brownoutStickyWarning; + HAL_Bool overcurrentStickyWarning; + HAL_Bool escEepromStickyWarning; + HAL_Bool extEepromStickyWarning; + HAL_Bool sensorStickyWarning; + HAL_Bool stallStickyWarning; + HAL_Bool hasResetStickyWarning; + HAL_Bool otherStickyWarning; + HAL_Bool overvoltageStickyWarning; + HAL_Bool motorLoopSpeedStickyWarning; + + HAL_Bool isFollower; + uint64_t timestamp; +}; + +/** A301 periodic status frame 2. */ +struct HAL_A301PeriodicStatus2 { + double encoderVelocity; + double relativeEncoderPosition; + uint64_t timestamp; +}; + +/** A301 periodic status frame 3. */ +struct HAL_A301PeriodicStatus3 { + double absoluteEncoderPosition; + uint64_t timestamp; +}; + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Detects the device ID of an A301 attached to a CAN bus. + * + * @param busId the CAN bus ID + * @param status error status variable; 0 on success + * @return the detected device ID, or -1 on failure + */ +int32_t HAL_DetectA301DeviceId(int32_t busId, int32_t* status); + +/** + * Initializes an A301. + * + * On Motioncore CAN buses (IDs 5 and above), device ID detection is attempted + * and the detected ID is used when available. + * + * @param busId the CAN bus ID + * @param deviceId the requested device ID + * @param allocationLocation allocation stack trace + * @param status error status variable; 0 on success + * @return the created A301 handle + */ +HAL_A301Handle HAL_InitializeA301(int32_t busId, int32_t deviceId, + const char* allocationLocation, + int32_t* status); + +/** + * Frees an A301 handle. + * + * @param handle A301 handle + */ +void HAL_FreeA301(HAL_A301Handle handle); + +/** + * Returns whether an A301 device ID is valid. + * + * @param deviceId device ID to validate + * @return true when the device ID is valid + */ +HAL_Bool HAL_CheckA301DeviceId(int32_t deviceId); + +/** + * Gets the CAN bus ID associated with a handle. + * + * @param handle A301 handle + * @param status error status variable; 0 on success + * @return CAN bus ID + */ +int32_t HAL_GetA301BusId(HAL_A301Handle handle, int32_t* status); + +/** + * Gets the resolved device ID associated with a handle. + * + * @param handle A301 handle + * @param status error status variable; 0 on success + * @return resolved device ID + */ +int32_t HAL_GetA301DeviceId(HAL_A301Handle handle, int32_t* status); + +/** + * Gets the A301 firmware version. + * + * @param handle A301 handle + * @param version output firmware version + * @param status error status variable; 0 on success + */ +void HAL_GetA301FirmwareVersion(HAL_A301Handle handle, + struct HAL_A301FirmwareVersion* version, + int32_t* status); + +/** + * Gets periodic status frame 0. + * + * @param handle A301 handle + * @param frame output decoded status frame + * @param status error status variable; 0 on success + */ +void HAL_GetA301PeriodicStatus0(HAL_A301Handle handle, + struct HAL_A301PeriodicStatus0* frame, + int32_t* status); + +/** + * Gets periodic status frame 1. + * + * @param handle A301 handle + * @param frame output decoded status frame + * @param status error status variable; 0 on success + */ +void HAL_GetA301PeriodicStatus1(HAL_A301Handle handle, + struct HAL_A301PeriodicStatus1* frame, + int32_t* status); + +/** + * Gets periodic status frame 2. + * + * @param handle A301 handle + * @param frame output decoded status frame + * @param status error status variable; 0 on success + */ +void HAL_GetA301PeriodicStatus2(HAL_A301Handle handle, + struct HAL_A301PeriodicStatus2* frame, + int32_t* status); + +/** + * Gets periodic status frame 3. + * + * @param handle A301 handle + * @param frame output decoded status frame + * @param status error status variable; 0 on success + */ +void HAL_GetA301PeriodicStatus3(HAL_A301Handle handle, + struct HAL_A301PeriodicStatus3* frame, + int32_t* status); + +/** + * Sets the relative encoder position in rotations. + * + * @param handle A301 handle + * @param position position in motor rotations + * @param status error status variable; 0 on success + */ +void HAL_SetA301RelativeEncoderPosition(HAL_A301Handle handle, double position, + int32_t* status); + +/** + * Sets the absolute encoder position in rotations. + * + * @param handle A301 handle + * @param position position in rotations + * @param status error status variable; 0 on success + */ +void HAL_SetA301AbsoluteEncoderPosition(HAL_A301Handle handle, double position, + int32_t* status); + +/** + * Sets an A301 control setpoint. + * + * @param handle A301 handle + * @param value requested control value + * @param controlType controller control mode + * @param positionSpeed approach speed for position controls, or zero for + * maximum speed + * @param status error status variable; 0 on success + */ +void HAL_SetA301Setpoint(HAL_A301Handle handle, double value, + HAL_A301ControlType controlType, double positionSpeed, + int32_t* status); + +/** + * Sets the A301 idle mode. + * + * @param handle A301 handle + * @param idleMode idle mode + * @param status error status variable; 0 on success + */ +void HAL_SetA301IdleMode(HAL_A301Handle handle, HAL_A301IdleMode idleMode, + int32_t* status); + +/** + * Gets the A301 idle mode. + * + * @param handle A301 handle + * @param status error status variable; 0 on success + * @return configured idle mode + */ +HAL_A301IdleMode HAL_GetA301IdleMode(HAL_A301Handle handle, int32_t* status); + +/** + * Enables or disables continuous input for absolute position control. + * + * @param handle A301 handle + * @param enabled true to enable continuous input + * @param status error status variable; 0 on success + */ +void HAL_SetA301AbsolutePositionContinuousInput(HAL_A301Handle handle, + HAL_Bool enabled, + int32_t* status); + +/** + * Gets whether absolute position continuous input is enabled. + * + * @param handle A301 handle + * @param status error status variable; 0 on success + * @return true when continuous input is enabled + */ +HAL_Bool HAL_GetA301AbsolutePositionContinuousInput(HAL_A301Handle handle, + int32_t* status); + +/** + * Sets the absolute encoder range offset in rotations. + * + * @param handle A301 handle + * @param offset offset in rotations, from -0.5 to 0.5 + * @param status error status variable; 0 on success + */ +void HAL_SetA301AbsoluteEncoderRangeOffset(HAL_A301Handle handle, double offset, + int32_t* status); + +/** + * Gets the absolute encoder range offset in rotations. + * + * @param handle A301 handle + * @param status error status variable; 0 on success + * @return range offset in rotations + */ +double HAL_GetA301AbsoluteEncoderRangeOffset(HAL_A301Handle handle, + int32_t* status); + +/** + * Sets whether non-position A301 control output is inverted. + * + * @param handle A301 handle + * @param inverted true to invert the output + * @param status error status variable; 0 on success + */ +void HAL_SetA301Inverted(HAL_A301Handle handle, HAL_Bool inverted, + int32_t* status); + +/** + * Gets whether non-position A301 control output is inverted. + * + * @param handle A301 handle + * @param status error status variable; 0 on success + * @return true when the output is inverted + */ +HAL_Bool HAL_GetA301Inverted(HAL_A301Handle handle, int32_t* status); + +/** + * Clears active and sticky faults and warnings. + * + * @param handle A301 handle + * @param status error status variable; 0 on success + */ +void HAL_ClearA301Faults(HAL_A301Handle handle, int32_t* status); + +/** + * Sets an A301 periodic status frame period in milliseconds. + * + * @param handle A301 handle + * @param frame periodic status frame + * @param periodMs requested period in milliseconds, from 0 to 1000 + * @param status error status variable; 0 on success + */ +void HAL_SetA301StatusFramePeriod(HAL_A301Handle handle, + HAL_A301PeriodicFrame frame, int32_t periodMs, + int32_t* status); + +/** + * Gets an A301 periodic status frame period in milliseconds. + * + * @param handle A301 handle + * @param frame periodic status frame + * @param status error status variable; 0 on success + * @return effective period in milliseconds + */ +int32_t HAL_GetA301StatusFramePeriod(HAL_A301Handle handle, + HAL_A301PeriodicFrame frame, + int32_t* status); + +#ifdef __cplusplus +} // extern "C" +#endif + +/** @} */ diff --git a/hal/src/main/native/include/wpi/hal/Types.h b/hal/src/main/native/include/wpi/hal/Types.h index 3b3a6855b9c..c403776efd4 100644 --- a/hal/src/main/native/include/wpi/hal/Types.h +++ b/hal/src/main/native/include/wpi/hal/Types.h @@ -62,6 +62,8 @@ typedef HAL_Handle HAL_REVPHHandle; typedef HAL_Handle HAL_CANStreamHandle; +typedef HAL_Handle HAL_A301Handle; + typedef int32_t HAL_Bool; typedef int32_t HAL_Status; diff --git a/hal/src/main/native/include/wpi/hal/handles/HandlesInternal.hpp b/hal/src/main/native/include/wpi/hal/handles/HandlesInternal.hpp index e54aefbafd4..654d62cec30 100644 --- a/hal/src/main/native/include/wpi/hal/handles/HandlesInternal.hpp +++ b/hal/src/main/native/include/wpi/hal/handles/HandlesInternal.hpp @@ -77,6 +77,7 @@ enum class HAL_HandleEnum { REV_PH = 21, CAN_STREAM = 22, ALERT = 23, + A301 = 24, }; /** diff --git a/hal/src/main/native/sim/HAL.cpp b/hal/src/main/native/sim/HAL.cpp index 673d1b1ed59..7927b5d53dc 100644 --- a/hal/src/main/native/sim/HAL.cpp +++ b/hal/src/main/native/sim/HAL.cpp @@ -86,6 +86,7 @@ void InitializeHAL() { InitializeAnalogInput(); InitializeAnalogInternal(); InitializeCAN(); + InitializeA301(); InitializeCounter(); InitializeDigitalInternal(); InitializeDIO(); diff --git a/hal/src/main/native/sim/HALInitializer.hpp b/hal/src/main/native/sim/HALInitializer.hpp index 2789fb52850..d9a508a99a4 100644 --- a/hal/src/main/native/sim/HALInitializer.hpp +++ b/hal/src/main/native/sim/HALInitializer.hpp @@ -4,19 +4,11 @@ #pragma once -#include +#include "../cpp/HALInitializer.hpp" namespace wpi::hal::init { -extern std::atomic_bool HAL_IsInitialized; -extern void RunInitialize(); -inline void CheckInit() { - if (HAL_IsInitialized.load(std::memory_order_relaxed)) { - return; - } - RunInitialize(); -} - extern void InitializeAddressableLEDData(); +extern void InitializeA301(); extern void InitializeAnalogInData(); extern void InitializeCanData(); extern void InitializeCANAPI(); diff --git a/hal/src/main/native/systemcore/HAL.cpp b/hal/src/main/native/systemcore/HAL.cpp index 56b65cff54a..d14dcf6cb29 100644 --- a/hal/src/main/native/systemcore/HAL.cpp +++ b/hal/src/main/native/systemcore/HAL.cpp @@ -43,6 +43,7 @@ void InitializeHAL() { InitializeAnalogInput(); InitializeCAN(); InitializeCANAPI(); + InitializeA301(); InitializeCounter(); InitializeDIO(); InitializeDutyCycle(); diff --git a/hal/src/main/native/systemcore/HALInitializer.hpp b/hal/src/main/native/systemcore/HALInitializer.hpp index 095cd559be3..32581906c0a 100644 --- a/hal/src/main/native/systemcore/HALInitializer.hpp +++ b/hal/src/main/native/systemcore/HALInitializer.hpp @@ -4,19 +4,11 @@ #pragma once -#include +#include "../cpp/HALInitializer.hpp" namespace wpi::hal::init { -extern std::atomic_bool HAL_IsInitialized; -extern void RunInitialize(); -inline void CheckInit() { - if (HAL_IsInitialized.load(std::memory_order_relaxed)) { - return; - } - RunInitialize(); -} - extern void InitializeCTREPCM(); +extern void InitializeA301(); extern void InitializeREVPH(); extern void InitializeAddressableLED(); extern void InitializeAnalogInput(); diff --git a/hal/src/test/native/cpp/can/A301Test.cpp b/hal/src/test/native/cpp/can/A301Test.cpp new file mode 100644 index 00000000000..61d964da4ed --- /dev/null +++ b/hal/src/test/native/cpp/can/A301Test.cpp @@ -0,0 +1,486 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "wpi/hal/A301.h" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "wpi/hal/CAN.h" +#include "wpi/hal/CANAPITypes.h" +#include "wpi/hal/Errors.h" +#include "wpi/hal/HAL.h" +#include "wpi/hal/handles/HandlesInternal.hpp" +#include "wpi/hal/simulation/CanData.h" +#include "wpi/util/timestamp.hpp" + +namespace { + +struct SentFrame { + int32_t busId; + uint32_t messageId; + HAL_CANMessage message; + int32_t periodMs; +}; + +struct CallbackHandle { + int32_t handle; + void (*cancel)(int32_t); + + ~CallbackHandle() { cancel(handle); } +}; + +struct A301Handle { + HAL_A301Handle handle; + + ~A301Handle() { HAL_FreeA301(handle); } +}; + +void StoreU32LE(uint8_t* data, uint32_t value) { + data[0] = value; + data[1] = value >> 8; + data[2] = value >> 16; + data[3] = value >> 24; +} + +void StoreFloatLE(uint8_t* data, float value) { + StoreU32LE(data, std::bit_cast(value)); +} + +float LoadFloatLE(const uint8_t* data) { + uint32_t value = data[0] | (static_cast(data[1]) << 8) | + (static_cast(data[2]) << 16) | + (static_cast(data[3]) << 24); + return std::bit_cast(value); +} + +TEST_CASE("A301 setpoint frames", "[hal][a301]") { + HAL_Initialize(); + std::vector sentFrames; + struct SendData { + std::vector* sentFrames; + bool failStops = false; + } sendData{&sentFrames}; + CallbackHandle sendCallback{ + HALSIM_RegisterCanSendMessageCallback( + [](const char*, void* param, int32_t busId, uint32_t messageId, + const HAL_CANMessage* message, int32_t periodMs, int32_t* status) { + auto data = static_cast(param); + data->sentFrames->push_back({busId, messageId, *message, periodMs}); + if (data->failStops && + periodMs == HAL_CAN_SEND_PERIOD_STOP_REPEATING) { + *status = HAL_CAN_BUFFER_OVERRUN; + } + }, + &sendData), + HALSIM_CancelCanSendMessageCallback}; + + int32_t status = 0; + A301Handle a301{HAL_InitializeA301(0, 3, "A301Test", &status)}; + REQUIRE(status == 0); + REQUIRE(a301.handle != HAL_INVALID_HANDLE); + sentFrames.clear(); + + HAL_SetA301Setpoint(a301.handle, 0.25, HAL_A301_CONTROL_TYPE_DUTY_CYCLE, 0.0, + &status); + REQUIRE(status == 0); + REQUIRE(sentFrames.size() == 1); + CHECK(sentFrames[0].busId == 0); + CHECK(sentFrames[0].messageId == (0x02030080u | 3)); + CHECK(sentFrames[0].periodMs == 20); + CHECK(sentFrames[0].message.dataSize == 8); + CHECK(LoadFloatLE(sentFrames[0].message.data) == 0.25f); + CHECK(LoadFloatLE(sentFrames[0].message.data + 4) == 0.0f); + + sentFrames.clear(); + HAL_SetA301Setpoint(a301.handle, 125.0, HAL_A301_CONTROL_TYPE_VELOCITY, 0.0, + &status); + REQUIRE(status == 0); + REQUIRE(sentFrames.size() == 2); + CHECK(sentFrames[0].messageId == (0x02030080u | 3)); + CHECK(sentFrames[0].periodMs == HAL_CAN_SEND_PERIOD_STOP_REPEATING); + CHECK(sentFrames[1].messageId == (0x02030000u | 3)); + CHECK(sentFrames[1].periodMs == 20); + CHECK(LoadFloatLE(sentFrames[1].message.data) == 125.0f); + + sendData.failStops = true; + sentFrames.clear(); + HAL_SetA301Setpoint(a301.handle, 0.25, + HAL_A301_CONTROL_TYPE_ABSOLUTE_POSITION, 0.0, &status); + CHECK(status == HAL_CAN_BUFFER_OVERRUN); + REQUIRE(sentFrames.size() == 1); + CHECK(sentFrames[0].messageId == (0x02030000u | 3)); + CHECK(sentFrames[0].periodMs == HAL_CAN_SEND_PERIOD_STOP_REPEATING); + + sendData.failStops = false; + sentFrames.clear(); + status = 0; + HAL_SetA301Setpoint(a301.handle, 0.25, + HAL_A301_CONTROL_TYPE_ABSOLUTE_POSITION, 0.0, &status); + REQUIRE(status == 0); + REQUIRE(sentFrames.size() == 2); + CHECK(sentFrames[0].messageId == (0x02030000u | 3)); + CHECK(sentFrames[0].periodMs == HAL_CAN_SEND_PERIOD_STOP_REPEATING); + CHECK(sentFrames[1].messageId == (0x02030200u | 3)); + CHECK(sentFrames[1].periodMs == 20); + + sentFrames.clear(); + HAL_SetA301Inverted(a301.handle, true, &status); + HAL_SetA301Setpoint(a301.handle, 6.0, HAL_A301_CONTROL_TYPE_VOLTAGE, 0.0, + &status); + REQUIRE(status == 0); + REQUIRE_FALSE(sentFrames.empty()); + CHECK(LoadFloatLE(sentFrames.back().message.data) == -0.5f); + + sentFrames.clear(); + HAL_SetA301Setpoint(a301.handle, 3.0, HAL_A301_CONTROL_TYPE_RELATIVE_POSITION, + 0.0, &status); + REQUIRE(status == 0); + REQUIRE(sentFrames.size() == 2); + CHECK(sentFrames.back().messageId == (0x02030100u | 3)); + CHECK(LoadFloatLE(sentFrames.back().message.data) == 3.0f); + + sentFrames.clear(); + HAL_SetA301Setpoint(a301.handle, 0.25, + HAL_A301_CONTROL_TYPE_ABSOLUTE_POSITION, 0.0, &status); + REQUIRE(status == 0); + REQUIRE(sentFrames.size() == 2); + CHECK(sentFrames.back().messageId == (0x02030200u | 3)); + CHECK(LoadFloatLE(sentFrames.back().message.data) == 0.25f); + + sentFrames.clear(); + HAL_SetA301RelativeEncoderPosition(a301.handle, 3.0, &status); + REQUIRE(status == 0); + REQUIRE(sentFrames.size() == 1); + CHECK(sentFrames[0].messageId == (0x02032800u | 3)); + CHECK(LoadFloatLE(sentFrames[0].message.data) == 3.0f); + + sentFrames.clear(); + HAL_SetA301AbsoluteEncoderPosition(a301.handle, 0.25, &status); + REQUIRE(status == 0); + REQUIRE(sentFrames.size() == 1); + CHECK(sentFrames[0].messageId == (0x02032880u | 3)); + CHECK(LoadFloatLE(sentFrames[0].message.data) == 0.25f); +} + +TEST_CASE("A301 periodic status decoding", "[hal][a301]") { + HAL_Initialize(); + std::array status0{}; + int16_t appliedOutput = 16222; + status0[0] = appliedOutput; + status0[1] = appliedOutput >> 8; + uint16_t voltage = 1638; + uint16_t current = 273; + status0[2] = voltage; + status0[3] = (voltage >> 8) | ((current & 0x0f) << 4); + status0[4] = current >> 4; + status0[5] = 42; + status0[6] = 1 | (HAL_A301_GEARBOX_RPM_500 << 1); + + std::array status1{0x81, 0xc1, 0x81, 0x42, + 0x41, 0x42, 0x01, 0x00}; + std::array status2{}; + StoreFloatLE(status2.data(), 123.5f); + StoreFloatLE(status2.data() + 4, 4.25f); + std::array status3{}; + StoreFloatLE(status3.data() + 4, -0.25f); + + struct ReceiveData { + std::array* status0; + std::array* status1; + std::array* status2; + std::array* status3; + } receiveData{&status0, &status1, &status2, &status3}; + + CallbackHandle receiveCallback{ + HALSIM_RegisterCanReceiveMessageCallback( + [](const char*, void* param, int32_t, uint32_t messageId, + HAL_CANReceiveMessage* message, int32_t* status) { + auto data = static_cast(param); + std::array* source = nullptr; + switch (messageId & ~0x3fu) { + case 0x0203b800: + source = data->status0; + break; + case 0x0203b840: + source = data->status1; + break; + case 0x0203b880: + source = data->status2; + break; + case 0x0203b8c0: + source = data->status3; + break; + default: + return; + } + message->timeStamp = wpi::util::Now(); + message->message.dataSize = source->size(); + std::memcpy(message->message.data, source->data(), source->size()); + *status = 0; + }, + &receiveData), + HALSIM_CancelCanReceiveMessageCallback}; + + int32_t status = 0; + A301Handle a301{HAL_InitializeA301(0, 4, "A301Test", &status)}; + REQUIRE(status == 0); + + HAL_A301PeriodicStatus0 frame0{}; + HAL_GetA301PeriodicStatus0(a301.handle, &frame0, &status); + REQUIRE(status == 0); + CHECK(frame0.appliedOutput == Catch::Approx(0.5).margin(0.001)); + CHECK(frame0.voltage == Catch::Approx(12.0).margin(0.01)); + CHECK(frame0.current == Catch::Approx(10.0).margin(0.02)); + CHECK(frame0.motorTemperature == 42); + CHECK(frame0.primaryHeartbeatLock); + CHECK(frame0.gearboxRPM == HAL_A301_GEARBOX_RPM_500); + + HAL_A301PeriodicStatus1 frame1{}; + HAL_GetA301PeriodicStatus1(a301.handle, &frame1, &status); + REQUIRE(status == 0); + CHECK(frame1.otherFault); + CHECK(frame1.firmwareFault); + CHECK(frame1.motorStartupFault); + CHECK(frame1.overvoltageWarning); + CHECK(frame1.motorLoopSpeedWarning); + CHECK(frame1.brownoutWarning); + CHECK(frame1.otherWarning); + CHECK(frame1.motorTypeStickyFault); + CHECK(frame1.overvoltageStickyWarning); + CHECK(frame1.overcurrentStickyWarning); + CHECK(frame1.hasResetStickyWarning); + CHECK(frame1.isFollower); + + HAL_SetA301Inverted(a301.handle, true, &status); + HAL_A301PeriodicStatus2 frame2{}; + HAL_GetA301PeriodicStatus2(a301.handle, &frame2, &status); + REQUIRE(status == 0); + CHECK(frame2.encoderVelocity == -123.5); + CHECK(frame2.relativeEncoderPosition == 4.25); + + HAL_A301PeriodicStatus3 frame3{}; + HAL_GetA301PeriodicStatus3(a301.handle, &frame3, &status); + REQUIRE(status == 0); + CHECK(frame3.absoluteEncoderPosition == -0.25); +} + +TEST_CASE("A301 configuration request and response frames", "[hal][a301]") { + HAL_Initialize(); + std::vector sentFrames; + float rangeOffset = 0.125f; + CallbackHandle sendCallback{ + HALSIM_RegisterCanSendMessageCallback( + [](const char*, void* param, int32_t busId, uint32_t messageId, + const HAL_CANMessage* message, int32_t periodMs, int32_t*) { + static_cast*>(param)->push_back( + {busId, messageId, *message, periodMs}); + }, + &sentFrames), + HALSIM_CancelCanSendMessageCallback}; + CallbackHandle receiveCallback{ + HALSIM_RegisterCanReceiveMessageCallback( + [](const char*, void* param, int32_t, uint32_t messageId, + HAL_CANReceiveMessage* message, int32_t* status) { + std::array data{}; + uint8_t dataSize = 0; + uint64_t timestamp = wpi::util::Now(); + switch (messageId & ~0x3fu) { + case 0x02032600: + data = {27, 1, 0x12, 0x34, 11, 2, 0, 0}; + dataSize = 8; + break; + case 0x02035140: + data[0] = HAL_A301_IDLE_MODE_BRAKE; + dataSize = 1; + break; + case 0x020351c0: + data[0] = 1; + dataSize = 1; + break; + case 0x02035240: + StoreFloatLE(data.data(), *static_cast(param)); + dataSize = 4; + break; + case 0x02034440: + data[0] = 0; + data[1] = HAL_A301_STATUS_2; + StoreU32LE(data.data() + 2, 1000); + dataSize = 6; + break; + case 0x02036040: + data[0] = HAL_A301_STATUS_2; + StoreU32LE(data.data() + 1, 125); + dataSize = 5; + break; + case 0x0203b880: + dataSize = 8; + timestamp -= 600'000; + break; + default: + return; + } + message->timeStamp = timestamp; + message->message.dataSize = dataSize; + std::memcpy(message->message.data, data.data(), dataSize); + *status = 0; + }, + &rangeOffset), + HALSIM_CancelCanReceiveMessageCallback}; + + int32_t status = 0; + A301Handle a301{HAL_InitializeA301(0, 5, "A301Test", &status)}; + REQUIRE(status == 0); + sentFrames.clear(); + + HAL_A301FirmwareVersion version{}; + HAL_GetA301FirmwareVersion(a301.handle, &version, &status); + REQUIRE(status == 0); + CHECK(version.major == 27); + CHECK(version.minor == 1); + CHECK(version.patch == 0x1234); + CHECK(version.prerelease == 11); + CHECK(version.hardwareRevision == 2); + CHECK(version.raw == 0x1b011234); + REQUIRE(sentFrames.size() == 1); + CHECK(sentFrames[0].messageId == + ((0x02032600u | 5) | HAL_CAN_IS_FRAME_REMOTE)); + CHECK(sentFrames[0].message.dataSize == 8); + + sentFrames.clear(); + CHECK(HAL_GetA301IdleMode(a301.handle, &status) == HAL_A301_IDLE_MODE_BRAKE); + REQUIRE(status == 0); + CHECK(HAL_GetA301AbsolutePositionContinuousInput(a301.handle, &status)); + REQUIRE(status == 0); + CHECK(HAL_GetA301AbsoluteEncoderRangeOffset(a301.handle, &status) == 0.125); + REQUIRE(status == 0); + REQUIRE(sentFrames.size() == 3); + CHECK(sentFrames[0].messageId == (0x02035100u | 5)); + CHECK(sentFrames[1].messageId == (0x02035180u | 5)); + CHECK(sentFrames[2].messageId == (0x02035200u | 5)); + + HAL_SetA301Inverted(a301.handle, true, &status); + REQUIRE(status == 0); + sentFrames.clear(); + HAL_SetA301AbsoluteEncoderRangeOffset(a301.handle, 0.25, &status); + REQUIRE(status == 0); + REQUIRE(sentFrames.size() == 1); + CHECK(sentFrames[0].messageId == (0x02032980u | 5)); + rangeOffset = LoadFloatLE(sentFrames[0].message.data); + CHECK(rangeOffset == -0.25f); + CHECK(HAL_GetA301AbsoluteEncoderRangeOffset(a301.handle, &status) == 0.25); + REQUIRE(status == 0); + + sentFrames.clear(); + HAL_SetA301StatusFramePeriod(a301.handle, HAL_A301_STATUS_2, 250, &status); + REQUIRE(status == 0); + REQUIRE(sentFrames.size() == 1); + CHECK(sentFrames[0].messageId == (0x02034400u | 5)); + CHECK(sentFrames[0].message.dataSize == 5); + CHECK(sentFrames[0].message.data[0] == HAL_A301_STATUS_2); + CHECK(sentFrames[0].message.data[1] == 250); + + HAL_A301PeriodicStatus2 frame{}; + HAL_GetA301PeriodicStatus2(a301.handle, &frame, &status); + CHECK(status == 0); + + sentFrames.clear(); + CHECK(HAL_GetA301StatusFramePeriod(a301.handle, HAL_A301_STATUS_2, &status) == + 125); + REQUIRE(status == 0); + REQUIRE(sentFrames.size() == 1); + CHECK(sentFrames[0].messageId == (0x02036000u | 5)); + CHECK(sentFrames[0].message.dataSize == 1); + CHECK(sentFrames[0].message.data[0] == HAL_A301_STATUS_2); +} + +TEST_CASE("A301 device ID detection", "[hal][a301]") { + HAL_Initialize(); + struct DetectionData { + int32_t busId = -1; + uint32_t messageId = 0; + uint32_t messageIdMask = 0; + } detection; + CallbackHandle openCallback{ + HALSIM_RegisterCanOpenStreamCallback( + [](const char*, void* param, HAL_CANStreamHandle* streamHandle, + int32_t busId, uint32_t messageId, uint32_t messageIdMask, + uint32_t, int32_t* status) { + auto data = static_cast(param); + data->busId = busId; + data->messageId = messageId; + data->messageIdMask = messageIdMask; + *streamHandle = 301; + *status = 0; + }, + &detection), + HALSIM_CancelCanOpenStreamCallback}; + CallbackHandle readCallback{ + HALSIM_RegisterCanReadStreamCallback( + [](const char*, void*, HAL_CANStreamHandle streamHandle, + HAL_CANStreamMessage* messages, uint32_t, uint32_t* messagesRead, + int32_t* status) { + CHECK(streamHandle == 301); + messages[0].messageId = 0x0203b800u | 17; + *messagesRead = 1; + *status = HAL_ERR_CANSessionMux_SessionOverrun; + }, + nullptr), + HALSIM_CancelCanReadStreamCallback}; + + int32_t status = 0; + CHECK(HAL_DetectA301DeviceId(5, &status) == 17); + REQUIRE(status == 0); + CHECK(detection.busId == 5); + CHECK(detection.messageId == 0x0203b800); + CHECK(detection.messageIdMask == 0x1fffffc0); +} + +TEST_CASE("A301 allocation is released by a global handle reset", + "[hal][a301]") { + HAL_Initialize(); + std::vector sentFrames; + CallbackHandle sendCallback{ + HALSIM_RegisterCanSendMessageCallback( + [](const char*, void* param, int32_t busId, uint32_t messageId, + const HAL_CANMessage* message, int32_t periodMs, int32_t*) { + static_cast*>(param)->push_back( + {busId, messageId, *message, periodMs}); + }, + &sentFrames), + HALSIM_CancelCanSendMessageCallback}; + + int32_t status = 0; + HAL_A301Handle first = HAL_InitializeA301(0, 6, "A301Test", &status); + REQUIRE(status == 0); + REQUIRE(first != HAL_INVALID_HANDLE); + + sentFrames.clear(); + wpi::hal::HandleBase::ResetGlobalHandles(); + constexpr std::array expectedStopIds{ + 0x02030080u | 6, 0x02030000u | 6, 0x02030100u | 6, 0x02030180u | 6, + 0x02030200u | 6}; + REQUIRE(sentFrames.size() == expectedStopIds.size()); + for (uint32_t expectedId : expectedStopIds) { + CHECK(std::count_if(sentFrames.begin(), sentFrames.end(), + [expectedId](const SentFrame& frame) { + return frame.messageId == expectedId && + frame.periodMs == + HAL_CAN_SEND_PERIOD_STOP_REPEATING; + }) == 1); + } + + A301Handle second{HAL_InitializeA301(0, 6, "A301Test", &status)}; + REQUIRE(status == 0); + CHECK(second.handle != HAL_INVALID_HANDLE); +} + +} // namespace