Austin Schuh | 812d0d1 | 2021-11-04 20:16:48 -0700 | [diff] [blame^] | 1 | // Copyright (c) FIRST and other WPILib contributors. |
| 2 | // Open Source Software; you can modify and/or share it under the terms of |
| 3 | // the WPILib BSD license file in the root directory of this project. |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 4 | |
| 5 | #include "frc/Servo.h" |
| 6 | |
| 7 | #include <hal/FRCUsageReporting.h> |
Austin Schuh | 812d0d1 | 2021-11-04 20:16:48 -0700 | [diff] [blame^] | 8 | #include <wpi/sendable/SendableBuilder.h> |
| 9 | #include <wpi/sendable/SendableRegistry.h> |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 10 | |
| 11 | using namespace frc; |
| 12 | |
| 13 | constexpr double Servo::kMaxServoAngle; |
| 14 | constexpr double Servo::kMinServoAngle; |
| 15 | |
| 16 | constexpr double Servo::kDefaultMaxServoPWM; |
| 17 | constexpr double Servo::kDefaultMinServoPWM; |
| 18 | |
| 19 | Servo::Servo(int channel) : PWM(channel) { |
| 20 | // Set minimum and maximum PWM values supported by the servo |
| 21 | SetBounds(kDefaultMaxServoPWM, 0.0, 0.0, 0.0, kDefaultMinServoPWM); |
| 22 | |
| 23 | // Assign defaults for period multiplier for the servo PWM control signal |
| 24 | SetPeriodMultiplier(kPeriodMultiplier_4X); |
| 25 | |
| 26 | HAL_Report(HALUsageReporting::kResourceType_Servo, channel + 1); |
Austin Schuh | 812d0d1 | 2021-11-04 20:16:48 -0700 | [diff] [blame^] | 27 | wpi::SendableRegistry::SetName(this, "Servo", channel); |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 28 | } |
| 29 | |
Austin Schuh | 812d0d1 | 2021-11-04 20:16:48 -0700 | [diff] [blame^] | 30 | void Servo::Set(double value) { |
| 31 | SetPosition(value); |
| 32 | } |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 33 | |
Austin Schuh | 812d0d1 | 2021-11-04 20:16:48 -0700 | [diff] [blame^] | 34 | void Servo::SetOffline() { |
| 35 | SetRaw(0); |
| 36 | } |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 37 | |
Austin Schuh | 812d0d1 | 2021-11-04 20:16:48 -0700 | [diff] [blame^] | 38 | double Servo::Get() const { |
| 39 | return GetPosition(); |
| 40 | } |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 41 | |
| 42 | void Servo::SetAngle(double degrees) { |
| 43 | if (degrees < kMinServoAngle) { |
| 44 | degrees = kMinServoAngle; |
| 45 | } else if (degrees > kMaxServoAngle) { |
| 46 | degrees = kMaxServoAngle; |
| 47 | } |
| 48 | |
| 49 | SetPosition((degrees - kMinServoAngle) / GetServoAngleRange()); |
| 50 | } |
| 51 | |
| 52 | double Servo::GetAngle() const { |
| 53 | return GetPosition() * GetServoAngleRange() + kMinServoAngle; |
| 54 | } |
| 55 | |
Austin Schuh | 812d0d1 | 2021-11-04 20:16:48 -0700 | [diff] [blame^] | 56 | double Servo::GetMaxAngle() const { |
| 57 | return kMaxServoAngle; |
| 58 | } |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 59 | |
Austin Schuh | 812d0d1 | 2021-11-04 20:16:48 -0700 | [diff] [blame^] | 60 | double Servo::GetMinAngle() const { |
| 61 | return kMinServoAngle; |
| 62 | } |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 63 | |
Austin Schuh | 812d0d1 | 2021-11-04 20:16:48 -0700 | [diff] [blame^] | 64 | void Servo::InitSendable(wpi::SendableBuilder& builder) { |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 65 | builder.SetSmartDashboardType("Servo"); |
Austin Schuh | 1e69f94 | 2020-11-14 15:06:14 -0800 | [diff] [blame] | 66 | builder.AddDoubleProperty( |
Austin Schuh | 812d0d1 | 2021-11-04 20:16:48 -0700 | [diff] [blame^] | 67 | "Value", [=] { return Get(); }, [=](double value) { Set(value); }); |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 68 | } |
| 69 | |
| 70 | double Servo::GetServoAngleRange() const { |
| 71 | return kMaxServoAngle - kMinServoAngle; |
| 72 | } |