001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.wpilibj; 006 007import edu.wpi.first.hal.HAL; 008import edu.wpi.first.hal.SimDevice; 009import edu.wpi.first.hal.SimDevice.Direction; 010import edu.wpi.first.hal.SimDouble; 011import edu.wpi.first.math.MathUtil; 012import edu.wpi.first.util.sendable.Sendable; 013import edu.wpi.first.util.sendable.SendableBuilder; 014import edu.wpi.first.util.sendable.SendableRegistry; 015import edu.wpi.first.wpilibj.PWM.OutputPeriod; 016 017/** 018 * Standard hobby style servo. 019 * 020 * <p>The range parameters default to the appropriate values for the Hitec HS-322HD servo provided 021 * in the FIRST Kit of Parts in 2008. 022 */ 023public class Servo implements Sendable, AutoCloseable { 024 private static final double kMaxServoAngle = 180.0; 025 private static final double kMinServoAngle = 0.0; 026 027 private static final int kDefaultMaxServoPWM = 2400; 028 private static final int kDefaultMinServoPWM = 600; 029 030 private final PWM m_pwm; 031 032 private SimDevice m_simDevice; 033 private SimDouble m_simPosition; 034 035 private static final int m_minPwm = kDefaultMinServoPWM; 036 private static final int m_maxPwm = kDefaultMaxServoPWM; 037 038 /** 039 * Constructor. 040 * 041 * <p>By default, {@value #kDefaultMaxServoPWM} ms is used as the max PWM value and {@value 042 * #kDefaultMinServoPWM} ms is used as the minPWM value. 043 * 044 * @param channel The PWM channel to which the servo is attached. 0-9 are on-board, 10-19 are on 045 * the MXP port 046 */ 047 @SuppressWarnings("this-escape") 048 public Servo(final int channel) { 049 m_pwm = new PWM(channel, false); 050 SendableRegistry.add(this, "Servo", channel); 051 052 m_pwm.setOutputPeriod(OutputPeriod.k20Ms); 053 054 HAL.reportUsage("IO", channel, "Servo"); 055 056 m_simDevice = SimDevice.create("Servo", channel); 057 if (m_simDevice != null) { 058 m_simPosition = m_simDevice.createDouble("Position", Direction.kOutput, 0.0); 059 m_pwm.setSimDevice(m_simDevice); 060 } 061 } 062 063 /** Free the resource associated with the PWM channel and set the value to 0. */ 064 @Override 065 public void close() { 066 SendableRegistry.remove(this); 067 m_pwm.close(); 068 069 if (m_simDevice != null) { 070 m_simDevice.close(); 071 m_simDevice = null; 072 m_simPosition = null; 073 } 074 } 075 076 /** 077 * Set the servo position. 078 * 079 * <p>Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right. 080 * 081 * @param value Position from 0.0 to 1.0. 082 */ 083 public void set(double value) { 084 value = MathUtil.clamp(value, 0.0, 1.0); 085 086 if (m_simPosition != null) { 087 m_simPosition.set(value); 088 } 089 090 int rawValue = (int) ((value * getFullRangeScaleFactor()) + m_minPwm); 091 092 m_pwm.setPulseTimeMicroseconds(rawValue); 093 } 094 095 /** 096 * Get the servo position. 097 * 098 * <p>Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right. 099 * This returns the commanded position, not the position that the servo is actually at, as the 100 * servo does not report its own position. 101 * 102 * @return Position from 0.0 to 1.0. 103 */ 104 public double get() { 105 int rawValue = m_pwm.getPulseTimeMicroseconds(); 106 107 if (rawValue < m_minPwm) { 108 return 0.0; 109 } else if (rawValue > m_maxPwm) { 110 return 1.0; 111 } 112 return (rawValue - m_minPwm) / getFullRangeScaleFactor(); 113 } 114 115 /** 116 * Set the servo angle. 117 * 118 * <p>The angles are based on the HS-322HD Servo, and have a range of 0 to 180 degrees. 119 * 120 * <p>Servo angles that are out of the supported range of the servo simply "saturate" in that 121 * direction In other words, if the servo has a range of (X degrees to Y degrees) than angles of 122 * less than X result in an angle of X being set and angles of more than Y degrees result in an 123 * angle of Y being set. 124 * 125 * @param degrees The angle in degrees to set the servo. 126 */ 127 public void setAngle(double degrees) { 128 if (degrees < kMinServoAngle) { 129 degrees = kMinServoAngle; 130 } else if (degrees > kMaxServoAngle) { 131 degrees = kMaxServoAngle; 132 } 133 134 set((degrees - kMinServoAngle) / getServoAngleRange()); 135 } 136 137 /** 138 * Get the servo angle. 139 * 140 * <p>This returns the commanded angle, not the angle that the servo is actually at, as the servo 141 * does not report its own angle. 142 * 143 * @return The angle in degrees to which the servo is set. 144 */ 145 public double getAngle() { 146 return get() * getServoAngleRange() + kMinServoAngle; 147 } 148 149 /** 150 * Gets the backing PWM handle. 151 * 152 * @return The pwm handle. 153 */ 154 public int getPwmHandle() { 155 return m_pwm.getHandle(); 156 } 157 158 /** 159 * Gets the PWM channel number. 160 * 161 * @return The channel number. 162 */ 163 public int getChannel() { 164 return m_pwm.getChannel(); 165 } 166 167 private double getFullRangeScaleFactor() { 168 return m_maxPwm - m_minPwm; 169 } 170 171 private double getServoAngleRange() { 172 return kMaxServoAngle - kMinServoAngle; 173 } 174 175 @Override 176 public void initSendable(SendableBuilder builder) { 177 builder.setSmartDashboardType("Servo"); 178 builder.addDoubleProperty("Value", this::get, this::set); 179 } 180}