diff --git a/src/main/java/training/Robot.java b/src/main/java/training/Robot.java index 3cee6f9..ed0c03c 100644 --- a/src/main/java/training/Robot.java +++ b/src/main/java/training/Robot.java @@ -1,7 +1,7 @@ package training; import training.subsystems.ArmSubsystems.elbow.Elbow; -import training.subsystems.ArmSubsystems.roller.Roller; +//import training.subsystems.ArmSubsystems.roller.Roller; import training.subsystems.ArmSubsystems.wrist.Wrist; public class Robot { @@ -11,14 +11,14 @@ public class Robot { public static final Robot.robotType ROBOT_TYPE = Robot.robotType.SIMULATION; private final Elbow elbow; private final Wrist wrist; - private final Roller roller; +// private final Roller roller; public Robot() { // Boot your subsystems... this.elbow = new Elbow(); this.wrist = new Wrist(); - this.roller = new Roller(); +// this.roller = new Roller(); } @@ -31,9 +31,9 @@ public Elbow getElbow() { return elbow; } - public Roller getRoller() { - return roller; - } +// public Roller getRoller() { +// return roller; +// } public Wrist getWrist() { return wrist; diff --git a/src/main/java/training/TrainingRobotManager.java b/src/main/java/training/TrainingRobotManager.java index 45d1ce5..24bf23d 100644 --- a/src/main/java/training/TrainingRobotManager.java +++ b/src/main/java/training/TrainingRobotManager.java @@ -3,8 +3,8 @@ import training.commands.ArmCommands.elbowCommands.ElbowStayInPlace; import training.commands.ArmCommands.elbowCommands.MoveElbow; import training.commands.ArmCommands.wristCommands.MoveWrist; -import training.commands.ArmCommands.rollerCommands.RollClockwise; -import training.commands.ArmCommands.rollerCommands.RollCounterClockwise; +//import training.commands.ArmCommands.rollerCommands.RollClockwise; +//import training.commands.ArmCommands.rollerCommands.RollCounterClockwise; import training.commands.ArmCommands.wristCommands.StopWrist; import training.subsystems.ArmSubsystems.elbow.ElbowConstants; import training.subsystems.ArmSubsystems.wrist.WristConstants; @@ -27,8 +27,8 @@ public void trainingInit() { smartJoystick.Y.onTrue(new MoveWrist(WristConstants.CLIMBING_POSITION, robot.getWrist())); smartJoystick.X.onTrue(new MoveWrist(WristConstants.STARTING_POSITION, robot.getWrist())); - smartJoystick.R1.whileTrue(new RollClockwise(robot.getRoller())); - smartJoystick.R2.whileTrue(new RollCounterClockwise(robot.getRoller())); +// smartJoystick.R1.whileTrue(new RollClockwise(robot.getRoller())); +// smartJoystick.R2.whileTrue(new RollCounterClockwise(robot.getRoller())); robot.getElbow().setDefaultCommand(new ElbowStayInPlace(robot.getElbow())); robot.getWrist().setDefaultCommand(new StopWrist(robot.getWrist())); diff --git a/src/main/java/training/commands/ArmCommands/elbowCommands/ElbowStayInPlace.java b/src/main/java/training/commands/ArmCommands/elbowCommands/ElbowStayInPlace.java index 2f40ee4..0a3cf84 100644 --- a/src/main/java/training/commands/ArmCommands/elbowCommands/ElbowStayInPlace.java +++ b/src/main/java/training/commands/ArmCommands/elbowCommands/ElbowStayInPlace.java @@ -4,7 +4,6 @@ import edu.wpi.first.wpilibj2.command.Command; import training.subsystems.ArmSubsystems.elbow.Elbow; -import java.lang.module.ModuleDescriptor; public class ElbowStayInPlace extends Command { diff --git a/src/main/java/training/commands/ArmCommands/rollerCommands/RollClockwise.java b/src/main/java/training/commands/ArmCommands/rollerCommands/RollClockwise.java index 69b4687..f2d98d1 100644 --- a/src/main/java/training/commands/ArmCommands/rollerCommands/RollClockwise.java +++ b/src/main/java/training/commands/ArmCommands/rollerCommands/RollClockwise.java @@ -10,7 +10,6 @@ public class RollClockwise extends Command { public RollClockwise(Roller roller) { this.roller = roller; addRequirements(roller); - } @Override diff --git a/src/main/java/training/commands/ArmCommands/wristCommands/MoveWrist.java b/src/main/java/training/commands/ArmCommands/wristCommands/MoveWrist.java index 21bcfdd..6c380c0 100644 --- a/src/main/java/training/commands/ArmCommands/wristCommands/MoveWrist.java +++ b/src/main/java/training/commands/ArmCommands/wristCommands/MoveWrist.java @@ -14,7 +14,6 @@ public MoveWrist(Rotation2d targetPosition, Wrist wrist) { this.wrist = wrist; this.targetPosition = targetPosition; addRequirements(wrist); - } @Override diff --git a/src/main/java/training/commands/ArmCommands/wristCommands/StopWrist.java b/src/main/java/training/commands/ArmCommands/wristCommands/StopWrist.java index fd414ec..82a3425 100644 --- a/src/main/java/training/commands/ArmCommands/wristCommands/StopWrist.java +++ b/src/main/java/training/commands/ArmCommands/wristCommands/StopWrist.java @@ -10,7 +10,6 @@ public class StopWrist extends Command { public StopWrist(Wrist wrist) { this.wrist = wrist; addRequirements(wrist); - } @Override diff --git a/src/main/java/training/subsystems/ArmSubsystems/elbow/Elbow.java b/src/main/java/training/subsystems/ArmSubsystems/elbow/Elbow.java index 51cc41b..b34cabd 100644 --- a/src/main/java/training/subsystems/ArmSubsystems/elbow/Elbow.java +++ b/src/main/java/training/subsystems/ArmSubsystems/elbow/Elbow.java @@ -34,7 +34,7 @@ public static Rotation2d angleDistance(Rotation2d currentAngle, Rotation2d targe public boolean isAtTargetPosition(Rotation2d targetAngle, Rotation2d positionTolerance, Rotation2d velocityTolerance) { return Math.abs(angleDistance(inputs.position, targetAngle).getDegrees()) <= positionTolerance.getDegrees() - && inputs.velocity.getRotations() <= velocityTolerance.getRotations(); + && inputs.position.getRotations() <= velocityTolerance.getRotations(); } public void goToPosition(Rotation2d targetPosition) { diff --git a/src/main/java/training/subsystems/ArmSubsystems/elbow/IElbow.java b/src/main/java/training/subsystems/ArmSubsystems/elbow/IElbow.java index 42f858e..2d0eef9 100644 --- a/src/main/java/training/subsystems/ArmSubsystems/elbow/IElbow.java +++ b/src/main/java/training/subsystems/ArmSubsystems/elbow/IElbow.java @@ -1,6 +1,7 @@ package training.subsystems.ArmSubsystems.elbow; import edu.wpi.first.math.geometry.Rotation2d; +import training.subsystems.ArmSubsystems.roller.RollerInputsAutoLogged; public interface IElbow { @@ -11,7 +12,4 @@ public interface IElbow { void setPower(double power); void updateInputs(ElbowInputsAutoLogged inputs); - - void stayAtPosition(); - } diff --git a/src/main/java/training/subsystems/ArmSubsystems/elbow/simulation/SimulationConstants.java b/src/main/java/training/subsystems/ArmSubsystems/elbow/simulation/SimulationConstants.java index 6654b1c..c1a5a33 100644 --- a/src/main/java/training/subsystems/ArmSubsystems/elbow/simulation/SimulationConstants.java +++ b/src/main/java/training/subsystems/ArmSubsystems/elbow/simulation/SimulationConstants.java @@ -8,7 +8,7 @@ public class SimulationConstants { public static final int ID = 0; - public static final double P_VALUE = 100; + public static final double P_VALUE = 150; public static final double I_VALUE = 0; public static final double D_VALUE = 0; public static final PIDController PID_CONTROLLER = new PIDController(P_VALUE, I_VALUE, D_VALUE); diff --git a/src/main/java/training/subsystems/ArmSubsystems/roller/IRoller.java b/src/main/java/training/subsystems/ArmSubsystems/roller/IRoller.java new file mode 100644 index 0000000..3a990da --- /dev/null +++ b/src/main/java/training/subsystems/ArmSubsystems/roller/IRoller.java @@ -0,0 +1,13 @@ +package training.subsystems.ArmSubsystems.roller; + +import training.subsystems.ArmSubsystems.elbow.ElbowInputsAutoLogged; + +public interface IRoller { + + public void setPower(double power); + + public void setVoltage(double voltage); + + public void updateInputs(RollerInputsAutoLogged inputs); + +} diff --git a/src/main/java/training/subsystems/ArmSubsystems/roller/NeoRoller/NeoConstants.java b/src/main/java/training/subsystems/ArmSubsystems/roller/NeoRoller/NeoConstants.java new file mode 100644 index 0000000..bfdba33 --- /dev/null +++ b/src/main/java/training/subsystems/ArmSubsystems/roller/NeoRoller/NeoConstants.java @@ -0,0 +1,8 @@ +package training.subsystems.ArmSubsystems.roller.NeoRoller; + + +public class NeoConstants { + + protected static final int ID = 22; + +} diff --git a/src/main/java/training/subsystems/ArmSubsystems/roller/NeoRoller/NeoRoller.java b/src/main/java/training/subsystems/ArmSubsystems/roller/NeoRoller/NeoRoller.java new file mode 100644 index 0000000..98f08f5 --- /dev/null +++ b/src/main/java/training/subsystems/ArmSubsystems/roller/NeoRoller/NeoRoller.java @@ -0,0 +1,31 @@ +package training.subsystems.ArmSubsystems.roller.NeoRoller; + +import com.revrobotics.CANSparkLowLevel; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.geometry.Rotation2d; +import training.subsystems.ArmSubsystems.roller.IRoller; +import training.subsystems.ArmSubsystems.roller.RollerInputsAutoLogged; + +public class NeoRoller implements IRoller { + + private final CANSparkMax motor; + + + public NeoRoller() { + this.motor = new CANSparkMax(NeoConstants.ID, CANSparkLowLevel.MotorType.kBrushless); + } + + public void setPower(double power) { + motor.set(power); + } + + public void setVoltage(double voltage) { + motor.setVoltage(voltage); + } + + public void updateInputs(RollerInputsAutoLogged inputs) { + inputs.position= Rotation2d.fromRotations(motor.getEncoder().getPosition()); + inputs.velocity=Rotation2d.fromRotations(motor.getEncoder().getVelocity()); + } + +} diff --git a/src/main/java/training/subsystems/ArmSubsystems/roller/Roller.java b/src/main/java/training/subsystems/ArmSubsystems/roller/Roller.java index d45152e..b0e10e1 100644 --- a/src/main/java/training/subsystems/ArmSubsystems/roller/Roller.java +++ b/src/main/java/training/subsystems/ArmSubsystems/roller/Roller.java @@ -1,16 +1,18 @@ package training.subsystems.ArmSubsystems.roller; -import com.revrobotics.CANSparkLowLevel; -import com.revrobotics.CANSparkMax; import edu.wpi.first.math.geometry.Rotation2d; +import training.subsystems.ArmSubsystems.elbow.ElbowInputsAutoLogged; +import training.subsystems.ArmSubsystems.elbow.IElbow; import utils.GBSubsystem; public class Roller extends GBSubsystem { - private final CANSparkMax motor; + private final IRoller motor; + private final RollerInputsAutoLogged inputs; public Roller() { - this.motor = new CANSparkMax(RollerConstants.ID, CANSparkLowLevel.MotorType.kBrushless); + this.motor=RollerFactory.create(); + this.inputs=new RollerInputsAutoLogged(); } @Override @@ -22,19 +24,22 @@ protected String getLogPath() { protected void subsystemPeriodic() {} public void rollClockwise() { - motor.set(RollerConstants.DEFAULT_CLOCKWISE_POWER); + motor.setPower(RollerConstants.DEFAULT_CLOCKWISE_POWER); } public void rollCounterClockwise() { - motor.set(RollerConstants.DEFAULT_COUNTER_CLOCKWISE_POWER); + motor.setPower(RollerConstants.DEFAULT_COUNTER_CLOCKWISE_POWER); } public void stop() { - motor.set(0); + motor.setPower(0); } public Rotation2d getVelocity() { - return Rotation2d.fromRotations(motor.getEncoder().getVelocity()); + return inputs.velocity; + } + public Rotation2d getPosition() { + return inputs.position; } } diff --git a/src/main/java/training/subsystems/ArmSubsystems/roller/RollerConstants.java b/src/main/java/training/subsystems/ArmSubsystems/roller/RollerConstants.java index a764b4e..1e1ee86 100644 --- a/src/main/java/training/subsystems/ArmSubsystems/roller/RollerConstants.java +++ b/src/main/java/training/subsystems/ArmSubsystems/roller/RollerConstants.java @@ -2,10 +2,8 @@ public class RollerConstants { - protected static final int ID = 22; protected static final double DEFAULT_CLOCKWISE_POWER = 0.3; - protected static final double DEFAULT_COUNTER_CLOCKWISE_POWER = -0.3; } diff --git a/src/main/java/training/subsystems/ArmSubsystems/roller/RollerFactory.java b/src/main/java/training/subsystems/ArmSubsystems/roller/RollerFactory.java new file mode 100644 index 0000000..f43c54e --- /dev/null +++ b/src/main/java/training/subsystems/ArmSubsystems/roller/RollerFactory.java @@ -0,0 +1,16 @@ +package training.subsystems.ArmSubsystems.roller; + +import training.Robot; +import training.subsystems.ArmSubsystems.roller.NeoRoller.NeoRoller; +import training.subsystems.ArmSubsystems.roller.simulationRoller.SimulationRoller; + +public class RollerFactory { + + public static IRoller create() { + return switch (Robot.ROBOT_TYPE) { + case SYNCOPA -> new NeoRoller(); + case SIMULATION -> new SimulationRoller(); + }; + } + +} diff --git a/src/main/java/training/subsystems/ArmSubsystems/roller/RollerInputs.java b/src/main/java/training/subsystems/ArmSubsystems/roller/RollerInputs.java new file mode 100644 index 0000000..8fc6a1a --- /dev/null +++ b/src/main/java/training/subsystems/ArmSubsystems/roller/RollerInputs.java @@ -0,0 +1,13 @@ +package training.subsystems.ArmSubsystems.roller; + +import edu.wpi.first.math.geometry.Rotation2d; +import org.littletonrobotics.junction.AutoLog; + +@AutoLog +public class RollerInputs { + + public Rotation2d position; + public Rotation2d velocity; + public double voltage; + +} diff --git a/src/main/java/training/subsystems/ArmSubsystems/roller/simulationRoller/SimulationConstants.java b/src/main/java/training/subsystems/ArmSubsystems/roller/simulationRoller/SimulationConstants.java new file mode 100644 index 0000000..398916b --- /dev/null +++ b/src/main/java/training/subsystems/ArmSubsystems/roller/simulationRoller/SimulationConstants.java @@ -0,0 +1,13 @@ +package training.subsystems.ArmSubsystems.roller.simulationRoller; + +public class SimulationConstants { + + protected static final int ID = 22; + public static final int NUMBER_OF_MOTORS = 1; + + public static final double GEAR_RATIO = 1; + + public static final double MOMENT_OF_INERTIA = 1; + + +} diff --git a/src/main/java/training/subsystems/ArmSubsystems/roller/simulationRoller/SimulationRoller.java b/src/main/java/training/subsystems/ArmSubsystems/roller/simulationRoller/SimulationRoller.java new file mode 100644 index 0000000..fe34942 --- /dev/null +++ b/src/main/java/training/subsystems/ArmSubsystems/roller/simulationRoller/SimulationRoller.java @@ -0,0 +1,39 @@ +package training.subsystems.ArmSubsystems.roller.simulationRoller; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import training.GlobalConstants; +import training.subsystems.ArmSubsystems.elbow.ElbowInputsAutoLogged; +import training.subsystems.ArmSubsystems.roller.IRoller; +import training.subsystems.ArmSubsystems.roller.RollerInputsAutoLogged; + +public class SimulationRoller implements IRoller { + + private final DCMotorSim motor; + + + public SimulationRoller() { + this.motor = new DCMotorSim( + DCMotor.getNEO(SimulationConstants.NUMBER_OF_MOTORS), + SimulationConstants.GEAR_RATIO, + SimulationConstants.MOMENT_OF_INERTIA + ); + } + + public void setPower(double power) { + setVoltage(power * GlobalConstants.MAX_BATTERY_VOLTAGE); + } + + public void setVoltage(double voltage) { + motor.setInputVoltage(voltage); + } + + @Override + public void updateInputs(RollerInputsAutoLogged inputs) { + inputs.position= Rotation2d.fromRotations(motor.getAngularPositionRotations()); + inputs.velocity=Rotation2d.fromRadians(motor.getAngularVelocityRadPerSec()); + } + + +}