diff --git a/wpilibc/src/main/native/cpp/Servo.cpp b/wpilibc/src/main/native/cpp/Servo.cpp deleted file mode 100644 index abc6c73ca1..0000000000 --- a/wpilibc/src/main/native/cpp/Servo.cpp +++ /dev/null @@ -1,84 +0,0 @@ -// 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 "frc/Servo.h" - -#include -#include -#include - -using namespace frc; - -Servo::Servo(int channel) : m_pwm(channel, false) { - wpi::SendableRegistry::Add(this, "Servo", channel); - - // Assign defaults for period multiplier for the servo PWM control signal - m_pwm.SetOutputPeriod(PWM::kOutputPeriod_20Ms); - - HAL_ReportUsage("IO", channel, "Servo"); - wpi::SendableRegistry::SetName(this, "Servo", channel); - - m_simDevice = hal::SimDevice{"Servo", channel}; - if (m_simDevice) { - m_simPosition = m_simDevice.CreateDouble("Position", true, 0.0); - m_pwm.SetSimDevice(m_simDevice); - } -} - -void Servo::Set(double value) { - value = std::clamp(value, 0.0, 1.0); - if (m_simPosition) { - m_simPosition.Set(value); - } - - units::microsecond_t rawValue = - (value * GetFullRangeScaleFactor()) + m_minPwm; - - m_pwm.SetPulseTime(rawValue); -} - -double Servo::Get() const { - units::microsecond_t rawValue = m_pwm.GetPulseTime(); - - if (rawValue < m_minPwm) { - return 0.0; - } else if (rawValue > m_maxPwm) { - return 1.0; - } - return (rawValue - m_minPwm).to() / - GetFullRangeScaleFactor().to(); -} - -void Servo::SetAngle(double degrees) { - if (degrees < kMinServoAngle) { - degrees = kMinServoAngle; - } else if (degrees > kMaxServoAngle) { - degrees = kMaxServoAngle; - } - - Set((degrees - kMinServoAngle) / GetServoAngleRange()); -} - -double Servo::GetAngle() const { - return Get() * GetServoAngleRange() + kMinServoAngle; -} - -void Servo::InitSendable(wpi::SendableBuilder& builder) { - builder.SetSmartDashboardType("Servo"); - builder.AddDoubleProperty( - "Value", [=, this] { return Get(); }, - [=, this](double value) { Set(value); }); -} - -double Servo::GetServoAngleRange() { - return kMaxServoAngle - kMinServoAngle; -} - -units::microsecond_t Servo::GetFullRangeScaleFactor() const { - return m_maxPwm - m_minPwm; -} - -int Servo::GetChannel() const { - return m_pwm.GetChannel(); -} diff --git a/wpilibc/src/main/native/cpp/simulation/ServoSim.cpp b/wpilibc/src/main/native/cpp/simulation/ServoSim.cpp deleted file mode 100644 index a6af4733de..0000000000 --- a/wpilibc/src/main/native/cpp/simulation/ServoSim.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// 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 "frc/simulation/ServoSim.h" - -#include -#include - -#include "frc/simulation/SimDeviceSim.h" - -using namespace frc; -using namespace frc::sim; - -ServoSim::ServoSim(const Servo& servo) : ServoSim(servo.GetChannel()) {} - -ServoSim::ServoSim(int channel) { - frc::sim::SimDeviceSim deviceSim{"Servo", channel}; - m_simPosition = deviceSim.GetDouble("Position"); -} - -double ServoSim::GetPosition() const { - return m_simPosition.Get(); -} - -double ServoSim::GetAngle() const { - return GetPosition() * Servo::GetServoAngleRange() + Servo::kMinServoAngle; -} diff --git a/wpilibc/src/main/native/include/frc/Servo.h b/wpilibc/src/main/native/include/frc/Servo.h deleted file mode 100644 index 244f8a2425..0000000000 --- a/wpilibc/src/main/native/include/frc/Servo.h +++ /dev/null @@ -1,112 +0,0 @@ -// 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 "frc/PWM.h" - -namespace frc { - -namespace sim { -class ServoSim; -} // namespace sim - -/** - * Standard hobby style servo. - * - * The range parameters default to the appropriate values for the Hitec HS-322HD - * servo provided in the FIRST Kit of Parts in 2008. - */ -class Servo : public wpi::Sendable, public wpi::SendableHelper { - public: - friend class frc::sim::ServoSim; - - /** - * Constructor. - * - * By default, 2.4 ms is used as the max PWM value and 0.6 ms is used as the - * min PWM value. - * - * @param channel The PWM channel to which the servo is attached. 0-9 are - * on-board, 10-19 are on the MXP port - */ - explicit Servo(int channel); - - Servo(Servo&&) = default; - Servo& operator=(Servo&&) = default; - - /** - * Set the servo position. - * - * Servo values range from 0.0 to 1.0 corresponding to the range of full left - * to full right. - * - * @param value Position from 0.0 to 1.0. - */ - void Set(double value); - - /** - * Get the servo position. - * - * Servo values range from 0.0 to 1.0 corresponding to the range of full left - * to full right. This returns the commanded position, not the position that - * the servo is actually at, as the servo does not report its own position. - * - * @return Position from 0.0 to 1.0. - */ - double Get() const; - - /** - * Set the servo angle. - * - * The angles are based on the HS-322HD Servo, and have a range of 0 to 180 - * degrees. - * - * Servo angles that are out of the supported range of the servo simply - * "saturate" in that direction. In other words, if the servo has a range of - * (X degrees to Y degrees) than angles of less than X result in an angle of - * X being set and angles of more than Y degrees result in an angle of Y being - * set. - * - * @param angle The angle in degrees to set the servo. - */ - void SetAngle(double angle); - - /** - * Get the servo angle. - * - * This returns the commanded angle, not the angle that the servo is actually - * at, as the servo does not report its own angle. - * - * @return The angle in degrees to which the servo is set. - */ - double GetAngle() const; - - int GetChannel() const; - - void InitSendable(wpi::SendableBuilder& builder) override; - - private: - static double GetServoAngleRange(); - units::microsecond_t GetFullRangeScaleFactor() const; - - static constexpr double kMaxServoAngle = 180.0; - static constexpr double kMinServoAngle = 0.0; - - static constexpr units::millisecond_t kDefaultMaxServoPWM = 2.4_ms; - static constexpr units::millisecond_t kDefaultMinServoPWM = 0.6_ms; - - units::millisecond_t m_maxPwm = kDefaultMaxServoPWM; - units::millisecond_t m_minPwm = kDefaultMinServoPWM; - - hal::SimDevice m_simDevice; - hal::SimDouble m_simPosition; - - PWM m_pwm; -}; - -} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/simulation/ServoSim.h b/wpilibc/src/main/native/include/frc/simulation/ServoSim.h deleted file mode 100644 index 15c3c07de0..0000000000 --- a/wpilibc/src/main/native/include/frc/simulation/ServoSim.h +++ /dev/null @@ -1,31 +0,0 @@ -// 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 "frc/Servo.h" - -namespace frc { - -class Servo; - -namespace sim { -class ServoSim { - public: - explicit ServoSim(const Servo& servo); - - explicit ServoSim(int channel); - - double GetPosition() const; - - double GetAngle() const; - - private: - hal::SimDouble m_simPosition; -}; -} // namespace sim -} // namespace frc diff --git a/wpilibc/src/test/native/cpp/simulation/ServoSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ServoSimTest.cpp deleted file mode 100644 index 9018b58036..0000000000 --- a/wpilibc/src/test/native/cpp/simulation/ServoSimTest.cpp +++ /dev/null @@ -1,32 +0,0 @@ -// 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 "frc/simulation/ServoSim.h" // NOLINT(build/include_order) - -#include -#include - -#include "frc/Servo.h" - -namespace frc::sim { -TEST(ServoSimTest, TestServo) { - frc::Servo servo{0}; - frc::sim::ServoSim sim{servo}; - - servo.Set(0); - EXPECT_EQ(0, sim.GetPosition()); - - servo.Set(0.354); - EXPECT_EQ(0.354, sim.GetPosition()); - - servo.SetAngle(10); - EXPECT_EQ(10, sim.GetAngle()); - - servo.SetAngle(90); - EXPECT_EQ(90, sim.GetAngle()); - - servo.SetAngle(170); - EXPECT_EQ(170, sim.GetAngle()); -} -} // namespace frc::sim diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java deleted file mode 100644 index f271ecc369..0000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java +++ /dev/null @@ -1,179 +0,0 @@ -// 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 edu.wpi.first.wpilibj; - -import edu.wpi.first.hal.HAL; -import edu.wpi.first.hal.SimDevice; -import edu.wpi.first.hal.SimDevice.Direction; -import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.util.sendable.SendableRegistry; -import edu.wpi.first.wpilibj.PWM.OutputPeriod; - -/** - * Standard hobby style servo. - * - *

The range parameters default to the appropriate values for the Hitec HS-322HD servo provided - * in the FIRST Kit of Parts in 2008. - */ -public class Servo implements Sendable, AutoCloseable { - private static final double kMaxServoAngle = 180.0; - private static final double kMinServoAngle = 0.0; - - private static final int kDefaultMaxServoPWM = 2400; - private static final int kDefaultMinServoPWM = 600; - - private final PWM m_pwm; - - private SimDevice m_simDevice; - private SimDouble m_simPosition; - - private static final int m_minPwm = kDefaultMinServoPWM; - private static final int m_maxPwm = kDefaultMaxServoPWM; - - /** - * Constructor. - * - *

By default, {@value #kDefaultMaxServoPWM} ms is used as the max PWM value and {@value - * #kDefaultMinServoPWM} ms is used as the minPWM value. - * - * @param channel The PWM channel to which the servo is attached. 0-9 are on-board, 10-19 are on - * the MXP port - */ - @SuppressWarnings("this-escape") - public Servo(final int channel) { - m_pwm = new PWM(channel, false); - SendableRegistry.add(this, "Servo", channel); - - m_pwm.setOutputPeriod(OutputPeriod.k20Ms); - - HAL.reportUsage("IO", channel, "Servo"); - - m_simDevice = SimDevice.create("Servo", channel); - if (m_simDevice != null) { - m_simPosition = m_simDevice.createDouble("Position", Direction.kOutput, 0.0); - m_pwm.setSimDevice(m_simDevice); - } - } - - /** Free the resource associated with the PWM channel and set the value to 0. */ - @Override - public void close() { - SendableRegistry.remove(this); - m_pwm.close(); - - if (m_simDevice != null) { - m_simDevice.close(); - m_simDevice = null; - m_simPosition = null; - } - } - - /** - * Set the servo position. - * - *

Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right. - * - * @param value Position from 0.0 to 1.0. - */ - public void set(double value) { - value = Math.clamp(value, 0.0, 1.0); - - if (m_simPosition != null) { - m_simPosition.set(value); - } - - int rawValue = (int) ((value * getFullRangeScaleFactor()) + m_minPwm); - - m_pwm.setPulseTimeMicroseconds(rawValue); - } - - /** - * Get the servo position. - * - *

Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right. - * This returns the commanded position, not the position that the servo is actually at, as the - * servo does not report its own position. - * - * @return Position from 0.0 to 1.0. - */ - public double get() { - int rawValue = m_pwm.getPulseTimeMicroseconds(); - - if (rawValue < m_minPwm) { - return 0.0; - } else if (rawValue > m_maxPwm) { - return 1.0; - } - return (rawValue - m_minPwm) / getFullRangeScaleFactor(); - } - - /** - * Set the servo angle. - * - *

The angles are based on the HS-322HD Servo, and have a range of 0 to 180 degrees. - * - *

Servo angles that are out of the supported range of the servo simply "saturate" in that - * direction In other words, if the servo has a range of (X degrees to Y degrees) than angles of - * less than X result in an angle of X being set and angles of more than Y degrees result in an - * angle of Y being set. - * - * @param degrees The angle in degrees to set the servo. - */ - public void setAngle(double degrees) { - if (degrees < kMinServoAngle) { - degrees = kMinServoAngle; - } else if (degrees > kMaxServoAngle) { - degrees = kMaxServoAngle; - } - - set((degrees - kMinServoAngle) / getServoAngleRange()); - } - - /** - * Get the servo angle. - * - *

This returns the commanded angle, not the angle that the servo is actually at, as the servo - * does not report its own angle. - * - * @return The angle in degrees to which the servo is set. - */ - public double getAngle() { - return get() * getServoAngleRange() + kMinServoAngle; - } - - /** - * Gets the backing PWM handle. - * - * @return The pwm handle. - */ - public int getPwmHandle() { - return m_pwm.getHandle(); - } - - /** - * Gets the PWM channel number. - * - * @return The channel number. - */ - public int getChannel() { - return m_pwm.getChannel(); - } - - private double getFullRangeScaleFactor() { - return m_maxPwm - m_minPwm; - } - - private double getServoAngleRange() { - return kMaxServoAngle - kMinServoAngle; - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("Servo"); - builder.addDoubleProperty("Value", this::get, this::set); - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ServoSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ServoSim.java deleted file mode 100644 index aefbdff772..0000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ServoSim.java +++ /dev/null @@ -1,50 +0,0 @@ -// 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 edu.wpi.first.wpilibj.simulation; - -import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.wpilibj.Servo; - -/** Class to control a simulated Servo. */ -public class ServoSim { - private final SimDouble m_simPosition; - - /** - * Constructor. - * - * @param servo The device to simulate - */ - public ServoSim(Servo servo) { - this(servo.getChannel()); - } - - /** - * Constructor. - * - * @param channel The channel the servo is attached to. - */ - public ServoSim(int channel) { - SimDeviceSim simDevice = new SimDeviceSim("Servo", channel); - m_simPosition = simDevice.getDouble("Position"); - } - - /** - * Gets the position set. - * - * @return Position - */ - public double getPosition() { - return m_simPosition.get(); - } - - /** - * Gets the angle set. - * - * @return Angle - */ - public double getAngle() { - return getPosition() * 180.0; - } -} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ServoSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ServoSimTest.java deleted file mode 100644 index 97931768c0..0000000000 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ServoSimTest.java +++ /dev/null @@ -1,37 +0,0 @@ -// 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 edu.wpi.first.wpilibj.simulation; - -import static org.junit.jupiter.api.Assertions.assertEquals; - -import edu.wpi.first.hal.HAL; -import edu.wpi.first.wpilibj.Servo; -import org.junit.jupiter.api.Test; - -class ServoSimTest { - @Test - void testServo() { - HAL.initialize(500, 0); - - try (Servo servo = new Servo(0)) { - ServoSim sim = new ServoSim(servo); - - servo.set(0); - assertEquals(0, sim.getPosition()); - - servo.set(0.354); - assertEquals(0.354, sim.getPosition()); - - servo.setAngle(10); - assertEquals(10, sim.getAngle()); - - servo.setAngle(90); - assertEquals(90, sim.getAngle()); - - servo.setAngle(170); - assertEquals(170, sim.getAngle()); - } - } -}