Skip to content
This repository has been archived by the owner on May 19, 2024. It is now read-only.

Commit

Permalink
chore: Format.
Browse files Browse the repository at this point in the history
  • Loading branch information
haydenheroux committed Apr 9, 2024
1 parent edf8abf commit 8e56beb
Show file tree
Hide file tree
Showing 24 changed files with 546 additions and 500 deletions.
163 changes: 84 additions & 79 deletions src/main/java/frc/lib/controller/PositionControllerIO.java
Original file line number Diff line number Diff line change
Expand Up @@ -7,85 +7,90 @@
/** Position controller interface. */
public interface PositionControllerIO {

/** Position controller constants. */
public static class PositionControllerIOConstants {
/** Interpret counterclockwise rotation on the motor face as having positive velocity, if set to true. */
public boolean ccwPositive = true;

/** Use the motor to brake the controlled mechanism on neutral output, if set to true. */
public boolean neutralBrake = false;

/** Maximum amount of current, in amps, to provide to the motor. */
public double currentLimitAmps = 40.0;

/** Ratio between the position sensor and the controlled mechanism. */
public double sensorToMechanismRatio = 1.0;

/** Offset between absolute encoder reading and controlled mechanism position in rotations. */
public double absoluteEncoderOffsetRotations = 0.0;
}

/** Position controller values. */
public static class PositionControllerIOValues {
/** Position in rotations. */
public double positionRotations = 0.0;

/** Velocity in rotations per second. */
public double velocityRotationsPerSecond = 0.0;

/** Acceleration in rotations per second per second. */
public double accelerationRotationsPerSecondPerSecond = 0.0;

/** Motor voltage in volts. */
public double motorVolts = 0.0;

/** Motor current in amps. */
public double motorAmps = 0.0;
}

/**
* Adds position controller values to Shuffleboard.
*
* @param tab
* @param name
* @param values
*/
public static void addToShuffleboard(ShuffleboardTab tab, String name, PositionControllerIOValues values) {
ShuffleboardLayout positionController = Telemetry.addColumn(tab, name);

positionController.addDouble("Position (rot)", () -> values.positionRotations);
positionController.addDouble("Velocity (rps)", () -> values.velocityRotationsPerSecond);
positionController.addDouble("Acceleration (rpsps)", () -> values.accelerationRotationsPerSecondPerSecond);
positionController.addDouble("Voltage (V)", () -> values.motorVolts);
positionController.addDouble("Current (A)", () -> values.motorAmps);
}

/**
* Configures the position controller.
*
* @param constants
*/
public void configure(PositionControllerIOConstants constants);

/**
* Updates the position controller's values.
*
* @param values
*/
public void update(PositionControllerIOValues values);

/**
* Sets the mechanism position.
*
* @param positionRotations
*/
public void setPosition(double positionRotations);

/** Position controller constants. */
public static class PositionControllerIOConstants {
/**
* Sets the position setpoint.
*
* @param positionRotations
* @param velocityRotationsPerSecond
* Interpret counterclockwise rotation on the motor face as having positive velocity, if set to
* true.
*/
public void setSetpoint(double positionRotations, double velocityRotationsPerSecond);
public boolean ccwPositive = true;

/** Use the motor to brake the controlled mechanism on neutral output, if set to true. */
public boolean neutralBrake = false;

/** Maximum amount of current, in amps, to provide to the motor. */
public double currentLimitAmps = 40.0;

/** Ratio between the position sensor and the controlled mechanism. */
public double sensorToMechanismRatio = 1.0;

/** Offset between absolute encoder reading and controlled mechanism position in rotations. */
public double absoluteEncoderOffsetRotations = 0.0;
}

/** Position controller values. */
public static class PositionControllerIOValues {
/** Position in rotations. */
public double positionRotations = 0.0;

/** Velocity in rotations per second. */
public double velocityRotationsPerSecond = 0.0;

/** Acceleration in rotations per second per second. */
public double accelerationRotationsPerSecondPerSecond = 0.0;

/** Motor voltage in volts. */
public double motorVolts = 0.0;

/** Motor current in amps. */
public double motorAmps = 0.0;
}

/**
* Adds position controller values to Shuffleboard.
*
* @param tab
* @param name
* @param values
*/
public static void addToShuffleboard(
ShuffleboardTab tab, String name, PositionControllerIOValues values) {
ShuffleboardLayout positionController = Telemetry.addColumn(tab, name);

positionController.addDouble("Position (rot)", () -> values.positionRotations);
positionController.addDouble("Velocity (rps)", () -> values.velocityRotationsPerSecond);
positionController.addDouble(
"Acceleration (rpsps)", () -> values.accelerationRotationsPerSecondPerSecond);
positionController.addDouble("Voltage (V)", () -> values.motorVolts);
positionController.addDouble("Current (A)", () -> values.motorAmps);
}

/**
* Configures the position controller.
*
* @param constants
*/
public void configure(PositionControllerIOConstants constants);

/**
* Updates the position controller's values.
*
* @param values
*/
public void update(PositionControllerIOValues values);

/**
* Sets the mechanism position.
*
* @param positionRotations
*/
public void setPosition(double positionRotations);

/**
* Sets the position setpoint.
*
* @param positionRotations
* @param velocityRotationsPerSecond
*/
public void setSetpoint(double positionRotations, double velocityRotationsPerSecond);
}
39 changes: 19 additions & 20 deletions src/main/java/frc/lib/controller/PositionControllerIOSim.java
Original file line number Diff line number Diff line change
Expand Up @@ -3,28 +3,27 @@
/** Simulated position controller. */
public class PositionControllerIOSim implements PositionControllerIO {

private double positionRotations = 0.0;

private double velocityRotationsPerSecond = 0.0;
private double positionRotations = 0.0;

@Override
public void configure(PositionControllerIOConstants constants) {}
private double velocityRotationsPerSecond = 0.0;

@Override
public void update(PositionControllerIOValues values) {
values.positionRotations = positionRotations;
values.velocityRotationsPerSecond = velocityRotationsPerSecond;
}
@Override
public void configure(PositionControllerIOConstants constants) {}

@Override
public void setPosition(double positionRotations) {
this.positionRotations = positionRotations;
}
@Override
public void update(PositionControllerIOValues values) {
values.positionRotations = positionRotations;
values.velocityRotationsPerSecond = velocityRotationsPerSecond;
}

@Override
public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) {
this.positionRotations = positionRotations;
this.velocityRotationsPerSecond = velocityRotationsPerSecond;
}

@Override
public void setPosition(double positionRotations) {
this.positionRotations = positionRotations;
}

@Override
public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) {
this.positionRotations = positionRotations;
this.velocityRotationsPerSecond = velocityRotationsPerSecond;
}
}
Loading

0 comments on commit 8e56beb

Please sign in to comment.