001/*----------------------------------------------------------------------------*/
002/* Copyright (c) FIRST 2008-2014. All Rights Reserved.                        */
003/* Open Source Software - may be modified and shared by FRC teams. The code   */
004/* must be accompanied by the FIRST BSD license file in the root directory of */
005/* the project.                                                               */
006/*----------------------------------------------------------------------------*/
007
008package edu.wpi.first.wpilibj;
009
010import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType;
011import edu.wpi.first.wpilibj.communication.UsageReporting;
012import edu.wpi.first.wpilibj.livewindow.LiveWindow;
013import edu.wpi.first.wpilibj.tables.ITable;
014import edu.wpi.first.wpilibj.tables.ITableListener;
015
016/**
017 * Standard hobby style servo.
018 *
019 * The range parameters default to the appropriate values for the Hitec HS-322HD servo provided
020 * in the FIRST Kit of Parts in 2008.
021 */
022public class Servo extends PWM {
023
024    private static final double kMaxServoAngle = 180.0;
025    private static final double kMinServoAngle = 0.0;
026
027    protected static final double kDefaultMaxServoPWM = 2.4;
028    protected static final double kDefaultMinServoPWM = .6;
029
030    /**
031     * Common initialization code called by all constructors.
032     *
033     * InitServo() assigns defaults for the period multiplier for the servo PWM control signal, as
034     * well as the minimum and maximum PWM values supported by the servo.
035     *
036     */
037    private void initServo() {
038        setBounds(kDefaultMaxServoPWM, 0, 0, 0, kDefaultMinServoPWM);
039        setPeriodMultiplier(PeriodMultiplier.k4X);
040
041        LiveWindow.addActuator("Servo", getChannel(), this);
042        UsageReporting.report(tResourceType.kResourceType_Servo, getChannel());
043    }
044
045    /**
046     * Constructor.<br>
047     *
048     * By default {@value #kDefaultMaxServoPWM} ms is used as the maxPWM value<br>
049     * By default {@value #kDefaultMinServoPWM} ms is used as the minPWM value<br>
050     *
051     * @param channel The PWM channel to which the servo is attached. 0-9 are on-board, 10-19 are on the MXP port
052     */
053    public Servo(final int channel) {
054        super(channel);
055        initServo();
056    }
057
058
059        /**
060     * Set the servo position.
061     *
062     * Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right.
063     *
064     * @param value Position from 0.0 to 1.0.
065     */
066    public void set(double value) {
067        setPosition(value);
068    }
069
070    /**
071     * Get the servo position.
072     *
073     * Servo values range from 0.0 to 1.0 corresponding to the range of full left to full right.
074     *
075     * @return Position from 0.0 to 1.0.
076     */
077    public double get() {
078        return getPosition();
079    }
080
081    /**
082     * Set the servo angle.
083     *
084     * Assume that the servo angle is linear with respect to the PWM value (big assumption, need to test).
085     *
086     * Servo angles that are out of the supported range of the servo simply "saturate" in that direction
087     * In other words, if the servo has a range of (X degrees to Y degrees) than angles of less than X
088     * result in an angle of X being set and angles of more than Y degrees result in an angle of Y being set.
089     *
090     * @param degrees The angle in degrees to set the servo.
091     */
092    public void setAngle(double degrees) {
093        if (degrees < kMinServoAngle) {
094            degrees = kMinServoAngle;
095        } else if (degrees > kMaxServoAngle) {
096            degrees = kMaxServoAngle;
097        }
098
099        setPosition(((degrees - kMinServoAngle)) / getServoAngleRange());
100    }
101
102    /**
103     * Get the servo angle.
104     *
105     * Assume that the servo angle is linear with respect to the PWM value (big assumption, need to test).
106     * @return The angle in degrees to which the servo is set.
107     */
108    public double getAngle() {
109        return getPosition() * getServoAngleRange() + kMinServoAngle;
110    }
111
112    private double getServoAngleRange() {
113        return kMaxServoAngle - kMinServoAngle;
114    }
115
116    /*
117     * Live Window code, only does anything if live window is activated.
118     */
119    public String getSmartDashboardType() {
120        return "Servo";
121    }
122    private ITable m_table;
123    private ITableListener m_table_listener;
124
125    /**
126     * {@inheritDoc}
127     */
128    public void initTable(ITable subtable) {
129        m_table = subtable;
130        updateTable();
131    }
132
133    /**
134     * {@inheritDoc}
135     */
136    public void updateTable() {
137        if (m_table != null) {
138            m_table.putNumber("Value", get());
139        }
140    }
141
142    /**
143     * {@inheritDoc}
144     */
145    public void startLiveWindowMode() {
146        m_table_listener = new ITableListener() {
147            public void valueChanged(ITable itable, String key, Object value, boolean bln) {
148                set(((Double) value).doubleValue());
149            }
150        };
151        m_table.addTableListener("Value", m_table_listener, true);
152    }
153
154    /**
155     * {@inheritDoc}
156     */
157    public void stopLiveWindowMode() {
158        // TODO: Broken, should only remove the listener from "Value" only.
159        m_table.removeTableListener(m_table_listener);
160    }
161}