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 Jan 9, 2024
1 parent 0d39dab commit 72df664
Show file tree
Hide file tree
Showing 13 changed files with 332 additions and 319 deletions.
23 changes: 11 additions & 12 deletions src/main/java/frc/robot/swerve/AzimuthEncoderIO.java
Original file line number Diff line number Diff line change
Expand Up @@ -3,17 +3,16 @@
/** Azimuth encoder hardware interface. */
public interface AzimuthEncoderIO {

/** Values for the azimuth encoder hardware interface. */
public static class AzimuthEncoderIOValues {
/** Position of the azimuth encoder in rotations. */
public double angleRotations = 0.0;
}
/** Values for the azimuth encoder hardware interface. */
public static class AzimuthEncoderIOValues {
/** Position of the azimuth encoder in rotations. */
public double angleRotations = 0.0;
}

/**
* Updates the azimuth encoder's values.
*
* @param values
*/
public void update(AzimuthEncoderIOValues values);

/**
* Updates the azimuth encoder's values.
*
* @param values
*/
public void update(AzimuthEncoderIOValues values);
}
9 changes: 4 additions & 5 deletions src/main/java/frc/robot/swerve/AzimuthEncoderIOSim.java
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,8 @@
/** Simulated azimuth encoder. */
public class AzimuthEncoderIOSim implements AzimuthEncoderIO {

@Override
public void update(AzimuthEncoderIOValues values) {
values.angleRotations = 0.0;
}

@Override
public void update(AzimuthEncoderIOValues values) {
values.angleRotations = 0.0;
}
}
50 changes: 25 additions & 25 deletions src/main/java/frc/robot/swerve/DriveMotorIO.java
Original file line number Diff line number Diff line change
Expand Up @@ -3,33 +3,33 @@
/** Drive motor hardware interface. */
public interface DriveMotorIO {

/** Values for the drive motor hardware interface. */
public static class DriveMotorIOValues {
/** Position of the drive motor in meters. */
public double positionMeters = 0.0;
/** Values for the drive motor hardware interface. */
public static class DriveMotorIOValues {
/** Position of the drive motor in meters. */
public double positionMeters = 0.0;

/** Velocity of the drive motor in meters per second. */
public double velocityMetersPerSecond = 0.0;
}
/** Velocity of the drive motor in meters per second. */
public double velocityMetersPerSecond = 0.0;
}

/**
* Updates the drive motor's values.
*
* @param values
*/
public void update(DriveMotorIOValues values);
/**
* Updates the drive motor's values.
*
* @param values
*/
public void update(DriveMotorIOValues values);

/**
* Sets the drive motor's position.
*
* @param positionMeters the drive motor's position.
*/
public void setPosition(double positionMeters);
/**
* Sets the drive motor's position.
*
* @param positionMeters the drive motor's position.
*/
public void setPosition(double positionMeters);

/**
* Sets the drive motor's setpoint.
*
* @param velocityMetersPerSecond the drive motor's setpoint.
*/
public void setSetpoint(double velocityMetersPerSecond);
/**
* Sets the drive motor's setpoint.
*
* @param velocityMetersPerSecond the drive motor's setpoint.
*/
public void setSetpoint(double velocityMetersPerSecond);
}
37 changes: 18 additions & 19 deletions src/main/java/frc/robot/swerve/DriveMotorIOSim.java
Original file line number Diff line number Diff line change
Expand Up @@ -5,28 +5,27 @@
/** Simulated drive motor. */
public class DriveMotorIOSim implements DriveMotorIO {

/** Represents the position of the drive motor. */
private double positionMeters;
/** Represents the position of the drive motor. */
private double positionMeters;

/** Represents the velocity of the drive motor. */
private double velocityMetersPerSecond;
/** Represents the velocity of the drive motor. */
private double velocityMetersPerSecond;

@Override
public void update(DriveMotorIOValues values) {
this.positionMeters += velocityMetersPerSecond * RobotConstants.TICK_PERIOD;
@Override
public void update(DriveMotorIOValues values) {
this.positionMeters += velocityMetersPerSecond * RobotConstants.TICK_PERIOD;

values.positionMeters = positionMeters;
values.velocityMetersPerSecond = velocityMetersPerSecond;
}
values.positionMeters = positionMeters;
values.velocityMetersPerSecond = velocityMetersPerSecond;
}

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

@Override
public void setSetpoint(double velocityMetersPerSecond) {
this.velocityMetersPerSecond = velocityMetersPerSecond;
}
@Override
public void setPosition(double positionMeters) {
this.positionMeters = positionMeters;
}

@Override
public void setSetpoint(double velocityMetersPerSecond) {
this.velocityMetersPerSecond = velocityMetersPerSecond;
}
}
52 changes: 26 additions & 26 deletions src/main/java/frc/robot/swerve/SteerMotorIO.java
Original file line number Diff line number Diff line change
Expand Up @@ -3,33 +3,33 @@
/** Steer motor hardware interface. */
public interface SteerMotorIO {

/** Values for the steer motor hardware interface. */
public static class SteerMotorIOValues {
/** Position of the steer motor in rotations. */
public double angleRotations = 0.0;
/** Velocity of the steer motor in rotations per second. */
public double omegaRotationsPerSecond = 0.0;
}
/** Values for the steer motor hardware interface. */
public static class SteerMotorIOValues {
/** Position of the steer motor in rotations. */
public double angleRotations = 0.0;

/**
* Updates the steer motor's values.
*
* @param values
*/
public void update(SteerMotorIOValues values);
/** Velocity of the steer motor in rotations per second. */
public double omegaRotationsPerSecond = 0.0;
}

/**
* Sets the steer motor's position.
*
* @param angleRotations the steer motor's position.
*/
public void setPosition(double angleRotations);
/**
* Updates the steer motor's values.
*
* @param values
*/
public void update(SteerMotorIOValues values);

/**
* Sets the steer motor's setpoint.
*
* @param angleRotations the steer motor's setpoint.
*/
public void setSetpoint(double angleRotations);

/**
* Sets the steer motor's position.
*
* @param angleRotations the steer motor's position.
*/
public void setPosition(double angleRotations);

/**
* Sets the steer motor's setpoint.
*
* @param angleRotations the steer motor's setpoint.
*/
public void setSetpoint(double angleRotations);
}
64 changes: 31 additions & 33 deletions src/main/java/frc/robot/swerve/SteerMotorIOSim.java
Original file line number Diff line number Diff line change
Expand Up @@ -9,48 +9,46 @@
/** Simulated steer motor. */
public class SteerMotorIOSim implements SteerMotorIO {

/** Represents the motor used to steer the wheel. */
private final DCMotor motorSim = DCMotor.getFalcon500(1); // TODO
/** Represents the motor used to steer the wheel. */
private final DCMotor motorSim = DCMotor.getFalcon500(1); // TODO

/** Represents the wheel steered by the motor. */
private final FlywheelSim wheelSim = new FlywheelSim(motorSim, MK4iConstants.STEER_GEARING, MK4iConstants.STEER_MOI);
/** Represents the wheel steered by the motor. */
private final FlywheelSim wheelSim =
new FlywheelSim(motorSim, MK4iConstants.STEER_GEARING, MK4iConstants.STEER_MOI);

/** Represents the angle of the steer motor. */
private double angleRotations;
/** Represents the angle of the steer motor. */
private double angleRotations;

/** Feedback controller for the angle. */
private final PIDController angleController = new PIDController(1.0, 0, 0);
/** Feedback controller for the angle. */
private final PIDController angleController = new PIDController(1.0, 0, 0);

public SteerMotorIOSim() {
angleRotations = 0.0;
public SteerMotorIOSim() {
angleRotations = 0.0;

angleController.enableContinuousInput(0, 1);
}
angleController.enableContinuousInput(0, 1);
}

@Override
public void update(SteerMotorIOValues values) {
double voltage = angleController.calculate(angleRotations);
@Override
public void update(SteerMotorIOValues values) {
double voltage = angleController.calculate(angleRotations);

wheelSim.setInputVoltage(voltage);
wheelSim.update(RobotConstants.TICK_PERIOD);
wheelSim.setInputVoltage(voltage);
wheelSim.update(RobotConstants.TICK_PERIOD);

double omegaRotationsPerSecond = wheelSim.getAngularVelocityRPM() / 60.0;
angleRotations += omegaRotationsPerSecond;
double omegaRotationsPerSecond = wheelSim.getAngularVelocityRPM() / 60.0;
angleRotations += omegaRotationsPerSecond;

values.angleRotations = angleRotations;
values.omegaRotationsPerSecond = omegaRotationsPerSecond;
}

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

@Override
public void setSetpoint(double angleRotations) {
angleController.setSetpoint(angleRotations);
}

values.angleRotations = angleRotations;
values.omegaRotationsPerSecond = omegaRotationsPerSecond;
}

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

@Override
public void setSetpoint(double angleRotations) {
angleController.setSetpoint(angleRotations);
}
}
Loading

0 comments on commit 72df664

Please sign in to comment.