Skip to content

Commit

Permalink
Add/copy missing docs, some cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
rzblue committed Sep 7, 2024
1 parent eaf8d64 commit e60d097
Show file tree
Hide file tree
Showing 9 changed files with 98 additions and 18 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,11 @@ public class OnBoardIO {
private static final double MESSAGE_INTERVAL = 1.0;
private double m_nextMessageTime;

/** Mode for Romi onboard IO channel */
public enum ChannelMode {
/** Input */
INPUT,
/** Output */
OUTPUT
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,12 @@
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;

/**
* Use a rate gyro to return the robots heading relative to a starting position.
*
* <p>This class is for the Romi onboard gyro, and will only work in simulation/Romi mode. Only one
* instance of a RomiGyro is supported.
*/
public class RomiGyro {
private final SimDevice m_simDevice;
private final SimDouble m_simRateX;
Expand Down Expand Up @@ -130,10 +136,27 @@ public void reset() {
}
}

/**
* Return the actual angle in degrees that the robot is currently facing.
*
* <p>The angle is based on integration of the returned rate form the gyro. The angle is
* continuous, that is, it will continue from 360->361 degrees. This allows algorithms that
* wouldn't want to see a discontinuity in the gyro output as it sweeps from 360 to 0 on the
* second time around.
*
* @return the current heading of the robot in degrees.
*/
public double getAngle() {
return getAngleZ();
}

/**
* Return the rate of rotation of the gyro
*
* <p>The rate is based on the most recent reading of the gyro.
*
* @return the current rate in degrees per second
*/
public double getRate() {
return getRateZ();
}
Expand Down
3 changes: 2 additions & 1 deletion romiVendordep/src/main/native/include/frc/romi/OnBoardIO.h
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,8 @@ namespace frc {
*/
class OnBoardIO {
public:
enum ChannelMode { INPUT, OUTPUT };
/** Mode for Romi onboard IO */
enum ChannelMode { /** Input */ INPUT, /** Output */ OUTPUT };
OnBoardIO(OnBoardIO::ChannelMode dio1, OnBoardIO::ChannelMode dio2);

static constexpr auto kMessageInterval = 1_s;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,11 @@ public class XRPGyro {
private double m_angleYOffset;
private double m_angleZOffset;

/** Create a new XRPGyro. */
/**
* Constructs an XRPGyro.
*
* <p>Only one instance of a XRPGyro is supported.
*/
public XRPGyro() {
m_simDevice = SimDevice.create("Gyro:XRPGyro");
if (m_simDevice != null) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,11 @@ private static void checkDeviceAllocation(int deviceNum) {
private final SimDouble m_simSpeed;
private final SimBoolean m_simInverted;

/** XRPMotor. */
/**
* Constructs an XRPMotor.
*
* @param deviceNum the motor channel
*/
public XRPMotor(int deviceNum) {
checkDeviceAllocation(deviceNum);

Expand Down
34 changes: 19 additions & 15 deletions xrpVendordep/src/main/java/edu/wpi/first/wpilibj/xrp/XRPServo.java
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,11 @@ private static void checkDeviceAllocation(int deviceNum) {

private final SimDouble m_simPosition;

/** XRPServo. */
/**
* Constructs an XRPServo.
*
* @param deviceNum the servo channel
*/
public XRPServo(int deviceNum) {
checkDeviceAllocation(deviceNum);

Expand All @@ -58,18 +62,18 @@ public XRPServo(int deviceNum) {
/**
* Set the servo angle.
*
* @param angle Desired angle in degrees
* @param angleDegrees Desired angle in degrees
*/
public void setAngle(double angle) {
if (angle < 0.0) {
angle = 0.0;
public void setAngle(double angleDegrees) {
if (angleDegrees < 0.0) {
angleDegrees = 0.0;
}

if (angle > 180.0) {
angle = 180.0;
if (angleDegrees > 180.0) {
angleDegrees = 180.0;
}

double pos = angle / 180.0;
double pos = angleDegrees / 180.0;

if (m_simPosition != null) {
m_simPosition.set(pos);
Expand All @@ -92,19 +96,19 @@ public double getAngle() {
/**
* Set the servo position.
*
* @param pos Desired position (Between 0.0 and 1.0)
* @param position Desired position (Between 0.0 and 1.0)
*/
public void setPosition(double pos) {
if (pos < 0.0) {
pos = 0.0;
public void setPosition(double position) {
if (position < 0.0) {
position = 0.0;
}

if (pos > 1.0) {
pos = 1.0;
if (position > 1.0) {
position = 1.0;
}

if (m_simPosition != null) {
m_simPosition.set(pos);
m_simPosition.set(position);
}
}

Expand Down
5 changes: 5 additions & 0 deletions xrpVendordep/src/main/native/include/frc/xrp/XRPGyro.h
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,11 @@ namespace frc {
*/
class XRPGyro {
public:
/**
* Constructs an XRPGyro.
*
* <p>Only one instance of a XRPGyro is supported.
*/
XRPGyro();

/**
Expand Down
5 changes: 5 additions & 0 deletions xrpVendordep/src/main/native/include/frc/xrp/XRPMotor.h
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,11 @@ WPI_IGNORE_DEPRECATED

class XRPMotor : public frc::MotorController, public frc::MotorSafety {
public:
/**
* Constructs an XRPMotor.
*
* @param deviceNum the motor channel
*/
explicit XRPMotor(int deviceNum);

void Set(double value) override;
Expand Down
31 changes: 31 additions & 0 deletions xrpVendordep/src/main/native/include/frc/xrp/XRPServo.h
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,45 @@

namespace frc {

/**
* XRPServo.
*
* <p>A SimDevice based servo
*/
class XRPServo {
public:
/**
* Constructs an XRPServo.
* @param deviceNum the servo channel
*/
explicit XRPServo(int deviceNum);

/**
* Set the servo angle.
*
* @param angleDegrees Desired angle in degrees
*/
void SetAngle(double angleDegrees);

/**
* Get the servo angle.
*
* @return Current servo angle
*/
double GetAngle() const;

/**
* Set the servo position.
*
* @param position Desired position (Between 0.0 and 1.0)
*/
void SetPosition(double position);

/**
* Get the servo position.
*
* @return Current servo position
*/
double GetPosition() const;

private:
Expand Down

0 comments on commit e60d097

Please sign in to comment.