blob: 4a292b9bd0377225c6e121eee8e3ad2f4c30fb10 [file] [log] [blame]
Austin Schuh812d0d12021-11-04 20:16:48 -07001// 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 Silverman8fce7482020-01-05 13:18:21 -08004
5#include "frc/Servo.h"
6
7#include <hal/FRCUsageReporting.h>
Austin Schuh812d0d12021-11-04 20:16:48 -07008#include <wpi/sendable/SendableBuilder.h>
9#include <wpi/sendable/SendableRegistry.h>
Brian Silverman8fce7482020-01-05 13:18:21 -080010
11using namespace frc;
12
13constexpr double Servo::kMaxServoAngle;
14constexpr double Servo::kMinServoAngle;
15
16constexpr double Servo::kDefaultMaxServoPWM;
17constexpr double Servo::kDefaultMinServoPWM;
18
19Servo::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 Schuh812d0d12021-11-04 20:16:48 -070027 wpi::SendableRegistry::SetName(this, "Servo", channel);
Brian Silverman8fce7482020-01-05 13:18:21 -080028}
29
Austin Schuh812d0d12021-11-04 20:16:48 -070030void Servo::Set(double value) {
31 SetPosition(value);
32}
Brian Silverman8fce7482020-01-05 13:18:21 -080033
Austin Schuh812d0d12021-11-04 20:16:48 -070034void Servo::SetOffline() {
35 SetRaw(0);
36}
Brian Silverman8fce7482020-01-05 13:18:21 -080037
Austin Schuh812d0d12021-11-04 20:16:48 -070038double Servo::Get() const {
39 return GetPosition();
40}
Brian Silverman8fce7482020-01-05 13:18:21 -080041
42void 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
52double Servo::GetAngle() const {
53 return GetPosition() * GetServoAngleRange() + kMinServoAngle;
54}
55
Austin Schuh812d0d12021-11-04 20:16:48 -070056double Servo::GetMaxAngle() const {
57 return kMaxServoAngle;
58}
Brian Silverman8fce7482020-01-05 13:18:21 -080059
Austin Schuh812d0d12021-11-04 20:16:48 -070060double Servo::GetMinAngle() const {
61 return kMinServoAngle;
62}
Brian Silverman8fce7482020-01-05 13:18:21 -080063
Austin Schuh812d0d12021-11-04 20:16:48 -070064void Servo::InitSendable(wpi::SendableBuilder& builder) {
Brian Silverman8fce7482020-01-05 13:18:21 -080065 builder.SetSmartDashboardType("Servo");
Austin Schuh1e69f942020-11-14 15:06:14 -080066 builder.AddDoubleProperty(
James Kuszmaulcf324122023-01-14 14:07:17 -080067 "Value", [=, this] { return Get(); },
68 [=, this](double value) { Set(value); });
Brian Silverman8fce7482020-01-05 13:18:21 -080069}
70
71double Servo::GetServoAngleRange() const {
72 return kMaxServoAngle - kMinServoAngle;
73}