diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..2fb8b04 --- /dev/null +++ b/simgui.json @@ -0,0 +1,44 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Autonomous": "String Chooser", + "/SmartDashboard/algae": "Mechanism2d" + }, + "windows": { + "/SmartDashboard/algae": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables": { + "transitory": { + "FMSInfo": { + "open": true + }, + "LiveWindow": { + "open": true + }, + "Shuffleboard": { + "open": true + }, + "SmartDashboard": { + "algae": { + "open": true + }, + "open": true + } + } + }, + "NetworkTables Info": { + "Clients": { + "open": true + }, + "Server": { + "open": true + }, + "visible": true + } +} diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 5b55349..9bd6488 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -5,8 +5,21 @@ package com.stuypulse.robot; +import javax.management.openmbean.OpenType; + +// Algae Commands +import com.stuypulse.robot.commands.algae.AlgaeGroundPickup; +import com.stuypulse.robot.commands.algae.AlgaeL2; +import com.stuypulse.robot.commands.algae.AlgaeL3; +import com.stuypulse.robot.commands.algae.AlgaeProcessorScore; +import com.stuypulse.robot.commands.algae.AlgaeReefKnockoff; +import com.stuypulse.robot.commands.algae.AlgaeStopRoller; +import com.stuypulse.robot.commands.algae.AlgaeStow; + import com.stuypulse.robot.commands.auton.DoNothingAuton; import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.subsystems.algae.Algae; + import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.input.gamepads.AutoGamepad; @@ -14,6 +27,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; + public class RobotContainer { // Gamepads @@ -21,6 +35,7 @@ public class RobotContainer { public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR); // Subsystem + public final Algae algae = Algae.getInstance(); // Autons private static SendableChooser autonChooser = new SendableChooser<>(); @@ -43,7 +58,37 @@ private void configureDefaultCommands() {} /*** BUTTONS ***/ /***************/ - private void configureButtonBindings() {} + + private void configureButtonBindings() { + configureDriverBindings(); + configureOperatorBindings(); + } + + private void configureDriverBindings() {} + + private void configureOperatorBindings() { + + operator.getBottomButton() + .onTrue(new AlgaeStow()); + + operator.getTopButton() + .onTrue(new AlgaeGroundPickup()); + + operator.getLeftButton() + .onTrue(new AlgaeL2()); + + operator.getRightButton() + .onTrue(new AlgaeL3()); + + operator.getLeftBumper() + .onTrue(new AlgaeProcessorScore()); + + operator.getRightBumper() + .onTrue(new AlgaeReefKnockoff()); + + operator.getLeftMenuButton() + .onTrue(new AlgaeStopRoller()); + } /**************/ /*** AUTONS ***/ diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeGroundPickup.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeGroundPickup.java new file mode 100644 index 0000000..f8888f8 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeGroundPickup.java @@ -0,0 +1,15 @@ +package com.stuypulse.robot.commands.algae; + +import com.stuypulse.robot.constants.Settings; + +public class AlgaeGroundPickup extends AlgaeSetPivot { + + public AlgaeGroundPickup(){ + super(Settings.Algae.GROUND_PICKUP_ANGLE); + algae.acquireUnder(); + } + public void initialize(){ + super.initialize(); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeL2.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeL2.java new file mode 100644 index 0000000..fd48511 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeL2.java @@ -0,0 +1,15 @@ +package com.stuypulse.robot.commands.algae; + +import com.stuypulse.robot.constants.Settings; + +public class AlgaeL2 extends AlgaeSetPivot { + + public AlgaeL2(){ + super(Settings.Algae.L2_ANGLE); + algae.acquireUnder(); + } + + public void initialize(){ + super.initialize(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeL3.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeL3.java new file mode 100644 index 0000000..d20a537 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeL3.java @@ -0,0 +1,15 @@ +package com.stuypulse.robot.commands.algae; + +import com.stuypulse.robot.constants.Settings; + +public class AlgaeL3 extends AlgaeSetPivot { + + public AlgaeL3(){ + super(Settings.Algae.L3_ANGLE); + algae.acquireOver(); + } + + public void initialize(){ + super.initialize(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeProcessorScore.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeProcessorScore.java new file mode 100644 index 0000000..b92b30f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeProcessorScore.java @@ -0,0 +1,18 @@ +package com.stuypulse.robot.commands.algae; + +import com.stuypulse.robot.constants.Settings; + + +public class AlgaeProcessorScore extends AlgaeSetPivot { + + public AlgaeProcessorScore(){ + super(Settings.Algae.PROCESSOR_ANGLE); + + algae.deacquireUnder(); + } + + public void initialize(){ + super.initialize(); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeReefKnockoff.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeReefKnockoff.java new file mode 100644 index 0000000..6029966 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeReefKnockoff.java @@ -0,0 +1,14 @@ +package com.stuypulse.robot.commands.algae; + +import com.stuypulse.robot.constants.Settings; + +public class AlgaeReefKnockoff extends AlgaeSetPivot { + + public AlgaeReefKnockoff(){ + super(Settings.Algae.REEF_KNOCKOFF_ANGLE); + } + + public void initialize(){ + super.initialize(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetPivot.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetPivot.java new file mode 100644 index 0000000..aad7354 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeSetPivot.java @@ -0,0 +1,24 @@ +package com.stuypulse.robot.commands.algae; + +import com.stuypulse.robot.subsystems.algae.*; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class AlgaeSetPivot extends InstantCommand { + + protected final Algae algae; + private final double angle; //target angle + + public AlgaeSetPivot(double angle) { + algae = Algae.getInstance(); + + this.angle = angle; + addRequirements(algae); + } + + @Override + public void initialize() { + algae.setTargetAngle(angle); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeStopRoller.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeStopRoller.java new file mode 100644 index 0000000..812e414 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeStopRoller.java @@ -0,0 +1,22 @@ +package com.stuypulse.robot.commands.algae; + +import com.stuypulse.robot.subsystems.algae.*; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class AlgaeStopRoller extends InstantCommand { + + private final Algae algae; + + public AlgaeStopRoller() { + algae = Algae.getInstance(); + + addRequirements(algae); + } + + @Override + public void initialize() { + algae.stopRollers(); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/algae/AlgaeStow.java b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeStow.java new file mode 100644 index 0000000..49919f0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/algae/AlgaeStow.java @@ -0,0 +1,10 @@ +package com.stuypulse.robot.commands.algae; + +import com.stuypulse.robot.constants.Settings; + +public class AlgaeStow extends AlgaeSetPivot { + + public AlgaeStow() { + super(Settings.Algae.STOW_ANGLE); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 1a2106d..fa3873f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -12,4 +12,9 @@ public interface Gamepad { int OPERATOR = 1; int DEBUGGER = 2; } -} + + public interface Algae { + int ROLLER_ID = 0; // update ports later -- def won't be 0 + int PIVOT_ID = 1; // update ports later + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 570bbaa..238ff2a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -14,4 +14,34 @@ * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable * values that we can edit on Shuffleboard. */ -public interface Settings {} +public interface Settings { + + public interface Algae { + double RAISED_ANGLE = 0.0; // CHANGE + double PROCESSOR_ANGLE = 0.0; // CHANGE + double REEF_KNOCKOFF_ANGLE = 0.0; // CHANGE + double GROUND_PICKUP_ANGLE = 0.0; // CHANGE + double L2_ANGLE = 0.0; // CHANGE + double L3_ANGLE = 0.0; // CHANGE + double STOW_ANGLE = 0.0; // CHANGE + double ACQUIRE_SPEED = 0.0; // CHANGE + double DEACQUIRE_SPEED = 0.0; // CHANGE + + public interface PID { + double kP = 0.0; + double kI = 0.0; + double kD = 0.0; + double MAX_VELOCITY = 0.0; + double MAX_ACCELERATION = 0.0; + } + + + public interface FF{ + double kS = 0.0; + double kV = 0.0; + double kA = 0.0; + double kG = 0.0; + } + } +} + diff --git a/src/main/java/com/stuypulse/robot/subsystems/algae/Algae.java b/src/main/java/com/stuypulse/robot/subsystems/algae/Algae.java new file mode 100644 index 0000000..f0240c1 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/algae/Algae.java @@ -0,0 +1,40 @@ +package com.stuypulse.robot.subsystems.algae; + +import com.revrobotics.spark.SparkMax; +import com.stuypulse.robot.Robot; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class Algae extends SubsystemBase { + + public static final Algae instance; + + static { + instance = new AlgaeImpl(); + } + + public Algae() { + + } + + public static Algae getInstance() { + return instance; + } + + public abstract double getTargetAngle(); + + public abstract double getCurrentAngle(); + + public abstract void acquireUnder(); + + public abstract void acquireOver(); + + public abstract void deacquireUnder(); + + public abstract void deacquireOver(); + + public abstract void stopRollers(); + + public abstract void setTargetAngle(double targetAngle); +} + diff --git a/src/main/java/com/stuypulse/robot/subsystems/algae/AlgaeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/algae/AlgaeImpl.java new file mode 100644 index 0000000..6c3e647 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/algae/AlgaeImpl.java @@ -0,0 +1,130 @@ +package com.stuypulse.robot.subsystems.algae; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; + +public class AlgaeImpl extends Algae { + + // variable declaration + + + private SparkMax pivotMotor; + private SparkMax rollerMotor; + + private RelativeEncoder pivotEncoder; + + private ProfiledPIDController pivotPIDController; + private ArmFeedforward pivotFFController; + + private final AlgaeVisualizer visualizer; + + private double targetAngle; + + private Constraints constraints; + + //private int rollerState; // -1 is deacquire, 1 is acquire/intake, 0 is not moving + + // constructor + + public AlgaeImpl() { + pivotMotor = new SparkMax(com.stuypulse.robot.constants.Ports.Algae.PIVOT_ID, MotorType.kBrushless); + rollerMotor = new SparkMax(com.stuypulse.robot.constants.Ports.Algae.ROLLER_ID, MotorType.kBrushless); + + + pivotEncoder = pivotMotor.getEncoder(); + + constraints = new Constraints(Settings.Algae.PID.MAX_VELOCITY, Settings.Algae.PID.MAX_ACCELERATION); + + pivotPIDController = new ProfiledPIDController(Settings.Algae.PID.kP, Settings.Algae.PID.kI, Settings.Algae.PID.kD, constraints); + pivotFFController = new ArmFeedforward(Settings.Algae.FF.kS, Settings.Algae.FF.kG, Settings.Algae.FF.kV, Settings.Algae.FF.kA); + targetAngle = 0; + + visualizer = new AlgaeVisualizer(); + } + + // pivot + public double getTargetAngle() { + return targetAngle; + } + + public double getCurrentAngle() { + // return Units.rotationsToDegrees(pivotEncoder.getPosition()); + return 135; + } + + // setters + + // rollers + public void acquireUnder() { // only rollers - GROUND AND L2 + rollerMotor.set(Settings.Algae.ACQUIRE_SPEED); + } + + public void acquireOver() { // only rollers - L3 + rollerMotor.set(-Settings.Algae.ACQUIRE_SPEED); + // not sure if this should be negative or positive yet + // but it shoudl be opposite of Under + // Do we rename acquire speed to smth else + // Should probably rename aquire speed to smth else + } + + public void deacquireUnder() { // only rollers - GROUND AND L2 + rollerMotor.set(Settings.Algae.DEACQUIRE_SPEED); + } + + public void deacquireOver() { // L3 + rollerMotor.set(-Settings.Algae.DEACQUIRE_SPEED); + // not sure if this should be negative or positive yet + // but it should be opposite of Under + } + + public void stopRollers() { + rollerMotor.set(0); + } + + // pivot + + public void setTargetAngle(double targetAngle) { + this.targetAngle = targetAngle; + } + + private double getCurrentPivotVelocity(){ + return pivotEncoder.getVelocity(); + } + + // periodic + + @Override + public void periodic() { + + pivotMotor.setVoltage( + pivotPIDController.calculate( + getCurrentAngle(), + getTargetAngle() + ) + + + pivotFFController.calculate( + Math.toRadians(getTargetAngle()), + getCurrentPivotVelocity() + ) + ); + + + // visualizer updating stuff + + visualizer.update(); + visualizer.updateBarAngle(); + visualizer.updatePivotAngle(); + + + + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/algae/AlgaeVisualizer.java b/src/main/java/com/stuypulse/robot/subsystems/algae/AlgaeVisualizer.java new file mode 100644 index 0000000..1c395b5 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/algae/AlgaeVisualizer.java @@ -0,0 +1,94 @@ +package com.stuypulse.robot.subsystems.algae; + +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class AlgaeVisualizer { + + public static AlgaeVisualizer instance = new AlgaeVisualizer(); + + private Mechanism2d algaeVis; + private Algae algae; + + // base declarations + private MechanismRoot2d baseRoot; + private MechanismLigament2d baseLigament; + + // pivot declarations + private MechanismRoot2d pivotRoot; + private MechanismLigament2d pivotLigament; + + // bar declarations + private MechanismRoot2d barRoot; + private MechanismLigament2d barLigament; + + // roller declarations (2 rollers) - 4 declarations in this section + private MechanismRoot2d leftRollerRoot; + private MechanismLigament2d leftRollerLigament; + /* */ + private MechanismRoot2d rightRollerRoot; + private MechanismLigament2d rightRollerLigament; + + public AlgaeVisualizer() { + algae = Algae.getInstance(); + + algaeVis = new Mechanism2d(100, 100); + baseRoot = algaeVis.getRoot("base", 50, 50); + baseLigament = new MechanismLigament2d("baseLigament", 10, 90); + + pivotRoot = algaeVis.getRoot("pivot", 50, 60); + pivotLigament = new MechanismLigament2d("pivotLigament", 10, 135); + + barRoot = algaeVis.getRoot("bar", 42.93, 67.07); + barLigament = new MechanismLigament2d("barLigament", 10, 180); + + leftRollerRoot = algaeVis.getRoot("leftRoller", 34.5, 67.07); + leftRollerLigament = new MechanismLigament2d("leftRollerLigament", 1, 260); + + rightRollerRoot = algaeVis.getRoot("rightRoller", 40.5, 67.07); + rightRollerLigament = new MechanismLigament2d("rightRollerLigament", 1, 280); + + baseRoot.append(baseLigament); + pivotRoot.append(pivotLigament); + barRoot.append(barLigament); + rightRollerRoot.append(rightRollerLigament); + leftRollerRoot.append(leftRollerLigament); + } + + public void spinVisualizerRollersIntake() { + rightRollerLigament.setAngle(rightRollerLigament.getAngle() - 5); + leftRollerLigament.setAngle(leftRollerLigament.getAngle() + 5); + } + + public void spinVisualizerRollersOutake() { + rightRollerLigament.setAngle(rightRollerLigament.getAngle() + 5); + leftRollerLigament.setAngle(leftRollerLigament.getAngle() - 5); + } + + public void updatePivotAngle() { + pivotLigament.setAngle(Algae.getInstance().getCurrentAngle()); + } + + public void updateBarAngle(){ + barLigament.setAngle(Algae.getInstance().getCurrentAngle() + 45); + barRoot.setPosition( + Math.cos(barLigament.getAngle()) * pivotLigament.getLength() + 50, + Math.sin(barLigament.getAngle()) * pivotLigament.getLength() + 67.07 + ); + } + + public void updateRollerPositions() { + + } + + + public void update() { + SmartDashboard.putData("algae", algaeVis); + leftRollerLigament.setAngle(leftRollerLigament.getAngle()+30); + rightRollerLigament.setAngle(rightRollerLigament.getAngle()-30); + } +} \ No newline at end of file