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.simulation;
006
007import edu.wpi.first.hal.SimBoolean;
008import edu.wpi.first.hal.SimDouble;
009import edu.wpi.first.wpilibj.DutyCycleEncoder;
010
011/** Class to control a simulated duty cycle encoder. */
012public class DutyCycleEncoderSim {
013  private final SimDouble m_simPosition;
014  private final SimDouble m_simDistancePerRotation;
015  private final SimDouble m_simAbsolutePosition;
016  private final SimBoolean m_simIsConnected;
017
018  /**
019   * Constructs from an DutyCycleEncoder object.
020   *
021   * @param encoder DutyCycleEncoder to simulate
022   */
023  public DutyCycleEncoderSim(DutyCycleEncoder encoder) {
024    this(encoder.getSourceChannel());
025  }
026
027  /**
028   * Constructs from a digital input channel.
029   *
030   * @param channel digital input channel.
031   */
032  public DutyCycleEncoderSim(int channel) {
033    SimDeviceSim wrappedSimDevice = new SimDeviceSim("DutyCycle:DutyCycleEncoder", channel);
034    m_simPosition = wrappedSimDevice.getDouble("position");
035    m_simDistancePerRotation = wrappedSimDevice.getDouble("distance_per_rot");
036    m_simAbsolutePosition = wrappedSimDevice.getDouble("absPosition");
037    m_simIsConnected = wrappedSimDevice.getBoolean("connected");
038  }
039
040  /**
041   * Get the position in turns.
042   *
043   * @return The position.
044   */
045  public double get() {
046    return m_simPosition.get();
047  }
048
049  /**
050   * Set the position in turns.
051   *
052   * @param turns The position.
053   */
054  public void set(double turns) {
055    m_simPosition.set(turns);
056  }
057
058  /**
059   * Get the distance.
060   *
061   * @return The distance.
062   */
063  public double getDistance() {
064    return m_simPosition.get() * m_simDistancePerRotation.get();
065  }
066
067  /**
068   * Set the distance.
069   *
070   * @param distance The distance.
071   */
072  public void setDistance(double distance) {
073    m_simPosition.set(distance / m_simDistancePerRotation.get());
074  }
075
076  /**
077   * Get the absolute position.
078   *
079   * @return The absolute position
080   */
081  public double getAbsolutePosition() {
082    return m_simAbsolutePosition.get();
083  }
084
085  /**
086   * Set the absolute position.
087   *
088   * @param position The absolute position
089   */
090  public void setAbsolutePosition(double position) {
091    m_simAbsolutePosition.set(position);
092  }
093
094  /**
095   * Get the distance per rotation for this encoder.
096   *
097   * @return The scale factor that will be used to convert rotation to useful units.
098   */
099  public double getDistancePerRotation() {
100    return m_simDistancePerRotation.get();
101  }
102
103  /**
104   * Get if the encoder is connected.
105   *
106   * @return true if the encoder is connected.
107   */
108  public boolean getConnected() {
109    return m_simIsConnected.get();
110  }
111
112  /**
113   * Set if the encoder is connected.
114   *
115   * @param isConnected Whether or not the sensor is connected.
116   */
117  public void setConnected(boolean isConnected) {
118    m_simIsConnected.set(isConnected);
119  }
120}