Skip to content

Commit

Permalink
adding spotless
Browse files Browse the repository at this point in the history
  • Loading branch information
michaelx0281 committed Jan 22, 2024
1 parent 6046d80 commit a9f3a43
Show file tree
Hide file tree
Showing 5 changed files with 35 additions and 51 deletions.
13 changes: 7 additions & 6 deletions src/main/java/org/sciborgs1155/robot/shooter/Shooter.java
Original file line number Diff line number Diff line change
Expand Up @@ -50,11 +50,12 @@ public Command runFeederInverse(double voltage) {
public Command runFlywheel(DoubleSupplier velocity) {
PIDController pid = new PIDController(Flywheel.kP, Flywheel.kI, Flywheel.kD);
SimpleMotorFeedforward ff = new SimpleMotorFeedforward(Flywheel.kS, Flywheel.kV, Flywheel.kA);
return run(
() ->
return run(() ->
flywheel.setVoltage(
pid.calculate(flywheel.getVelocity(), velocity.getAsDouble())
+ ff.calculate(velocity.getAsDouble()))).finallyDo(() -> pid.close()).withName("running Flywheel");
+ ff.calculate(velocity.getAsDouble())))
.finallyDo(() -> pid.close())
.withName("running Flywheel");
}

public Command runPivot(double goalAngle) {
Expand All @@ -66,10 +67,10 @@ public Command runPivot(double goalAngle) {
new TrapezoidProfile.Constraints(Pivot.MAX_VELOCITY, Pivot.MAX_ACCEL));
ArmFeedforward ff = new ArmFeedforward(Pivot.kS, Pivot.kG, Pivot.kV);

return run(
() ->
return run(() ->
pivot.setVoltage(
pid.calculate(pivot.getPosition(), goalAngle)
+ ff.calculate(goalAngle + Pivot.POSITION_OFFSET, pid.getSetpoint().velocity))).withName("running Pivot");
+ ff.calculate(goalAngle + Pivot.POSITION_OFFSET, pid.getSetpoint().velocity)))
.withName("running Pivot");
}
}
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
package org.sciborgs1155.robot.shooter;

import static edu.wpi.first.units.Units.*;

import edu.wpi.first.units.*;

public class ShooterConstants {
Expand All @@ -15,10 +16,9 @@ public static class Flywheel {
public static final double RADIUS = 1;
public static final Measure<Distance> CIRCUMFERENCE = Meters.of(2 * Math.PI * RADIUS);

public static final Measure<Angle> POSITION_FACTOR =
Rotations.of(GEARING).times(CIRCUMFERENCE.in(Meters));
public static final Measure<Velocity<Angle>> VELOCITY_FACTOR =
POSITION_FACTOR.per(Minute);
public static final Measure<Angle> POSITION_FACTOR =
Rotations.of(GEARING).times(CIRCUMFERENCE.in(Meters));
public static final Measure<Velocity<Angle>> VELOCITY_FACTOR = POSITION_FACTOR.per(Minute);

public static final double kP = 1;
public static final double kI = 0;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,18 +3,15 @@
import static org.sciborgs1155.robot.Ports.Shooter.Feeder.*;
import static org.sciborgs1155.robot.shooter.ShooterConstants.Feeder.*;

import java.util.Set;

import org.sciborgs1155.lib.SparkUtils;
import org.sciborgs1155.lib.SparkUtils.Data;
import org.sciborgs1155.lib.SparkUtils.Sensor;

import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkFlex;
import com.revrobotics.CANSparkLowLevel.MotorType;

import edu.wpi.first.units.Current;
import edu.wpi.first.units.Unit;
import java.util.Set;
import org.sciborgs1155.lib.SparkUtils;
import org.sciborgs1155.lib.SparkUtils.Data;
import org.sciborgs1155.lib.SparkUtils.Sensor;

public class RealFeeder implements FeederIO {

Expand All @@ -31,12 +28,11 @@ public RealFeeder() {

motor.burnFlash();

SparkUtils.configureFrameStrategy(
motor,
Set.of(Data.POSITION, Data.VELOCITY, Data.VOLTAGE),
Set.of(Sensor.INTEGRATED),
false
);
SparkUtils.configureFrameStrategy(
motor,
Set.of(Data.POSITION, Data.VELOCITY, Data.VOLTAGE),
Set.of(Sensor.INTEGRATED),
false);
}

@Override
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,16 +4,14 @@
import static org.sciborgs1155.robot.Ports.Shooter.Flywheel.*;
import static org.sciborgs1155.robot.shooter.ShooterConstants.Flywheel.*;

import java.util.Set;

import org.sciborgs1155.lib.SparkUtils;
import org.sciborgs1155.lib.SparkUtils.Data;
import org.sciborgs1155.lib.SparkUtils.Sensor;

import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkFlex;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.RelativeEncoder;
import java.util.Set;
import org.sciborgs1155.lib.SparkUtils;
import org.sciborgs1155.lib.SparkUtils.Data;
import org.sciborgs1155.lib.SparkUtils.Sensor;

public class RealFlywheel implements FlywheelIO {
private final CANSparkFlex leftMotor;
Expand All @@ -39,14 +37,11 @@ public RealFlywheel() {
encoder.setPositionConversionFactor(POSITION_CONVERSION);

SparkUtils.configureFrameStrategy(
leftMotor,
Set.of(Data.POSITION, Data.VELOCITY, Data.VOLTAGE),
Set.of(Sensor.INTEGRATED),
true
);
SparkUtils.configureFollowerFrameStrategy(
rightMotor
);
leftMotor,
Set.of(Data.POSITION, Data.VELOCITY, Data.VOLTAGE),
Set.of(Sensor.INTEGRATED),
true);
SparkUtils.configureFollowerFrameStrategy(rightMotor);

rightMotor.follow(leftMotor);

Expand Down
20 changes: 6 additions & 14 deletions src/main/java/org/sciborgs1155/robot/shooter/pivot/RealPivot.java
Original file line number Diff line number Diff line change
Expand Up @@ -4,16 +4,14 @@
import static org.sciborgs1155.robot.Ports.Shooter.Pivot.*;
import static org.sciborgs1155.robot.shooter.ShooterConstants.Pivot.*;

import java.util.Set;

import org.sciborgs1155.lib.SparkUtils;
import org.sciborgs1155.lib.SparkUtils.Data;
import org.sciborgs1155.lib.SparkUtils.Sensor;

import com.revrobotics.CANSparkBase.IdleMode;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
import edu.wpi.first.wpilibj.DutyCycleEncoder;
import java.util.Set;
import org.sciborgs1155.lib.SparkUtils;
import org.sciborgs1155.lib.SparkUtils.Data;
import org.sciborgs1155.lib.SparkUtils.Sensor;

public class RealPivot implements PivotIO {
private final CANSparkMax lead;
Expand All @@ -39,15 +37,9 @@ public RealPivot() {
encoder.setDistancePerRotation(CONVERSION);

SparkUtils.configureFrameStrategy(
lead,
Set.of(Data.POSITION, Data.VELOCITY, Data.VOLTAGE),
Set.of(Sensor.INTEGRATED),
true
);
lead, Set.of(Data.POSITION, Data.VELOCITY, Data.VOLTAGE), Set.of(Sensor.INTEGRATED), true);

SparkUtils.configureFollowerFrameStrategy(
follow
);
SparkUtils.configureFollowerFrameStrategy(follow);

follow.follow(lead);

Expand Down

0 comments on commit a9f3a43

Please sign in to comment.