Skip to content

Commit

Permalink
Shift RollerMode to Roller
Browse files Browse the repository at this point in the history 
was changeable but in constants, now moved to Roller subsystem

=Also remove get/set of RollerMode.speed in favor of just making it public (if you both get and set a variable publicly without doing anything special, don't make it private)
  • Loading branch information
@FriedLongJohns
FriedLongJohns committed Oct 1, 2023
1 parent f510639 commit eec4b4a
Show file tree
Hide file tree
Showing 4 changed files with 58 additions and 67 deletions.
36 changes: 0 additions & 36 deletions src/main/java/org/carlmontrobotics/robotcode2023/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -380,42 +380,6 @@ public static final class Roller {
//#endregion

//#region Command Constants
// TODO: Determine actual speeds/timings for roller
public static class RollerMode {
public static RollerMode INTAKE_CONE = new RollerMode(-0.5, .5, GameObject.CONE, conePickupColor);
public static RollerMode INTAKE_CUBE = new RollerMode(0.4, .25, GameObject.CUBE, cubePickupColor);
// The obj indicates which game object the roller is trying to intake
// if obj == NONE, that means it is trying to outtake rather than intake
public static RollerMode OUTTAKE_CONE = new RollerMode(0.5, .5, GameObject.NONE, defaultColor);
public static RollerMode OUTTAKE_CUBE = new RollerMode(-0.5, .5, GameObject.NONE, defaultColor);
public static RollerMode STOP = new RollerMode(0, .1, GameObject.NONE, defaultColor);
private double speed;
public double time;
public GameObject obj;
public Color ledColor;

/**
* @param speed A number between -1 and 1
* @param time Amount of time in seconds to keep the motor running after
* distance sensor has detected an object
* @param intake Whether the roller is outtaking or intaking
*/
public RollerMode(double speed, double time, GameObject obj, Color ledColor) {
this.speed = speed;
this.time = time;
this.obj = obj;
this.ledColor = ledColor;
}

public double getSpeed() {
return speed;
}

public void setSpeed(double speed) {
this.speed = speed;
}
}

//#endregion

public static enum GameObject {
Expand Down
25 changes: 12 additions & 13 deletions src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@
import org.carlmontrobotics.robotcode2023.Constants.GoalPos;
import org.carlmontrobotics.robotcode2023.Constants.OI.Driver;
import org.carlmontrobotics.robotcode2023.Constants.OI.Manipulator;
import org.carlmontrobotics.robotcode2023.Constants.Roller.RollerMode;
import org.carlmontrobotics.robotcode2023.commands.AlignChargingStation;
import org.carlmontrobotics.robotcode2023.commands.ArmTeleop;
import org.carlmontrobotics.robotcode2023.commands.DriveOverChargeStation;
Expand Down Expand Up @@ -76,7 +75,7 @@ public RobotContainer() {
eventMap.put("Cone High Pos.", new SetArmWristGoalPreset(GoalPos.HIGH, () -> false, () -> false, arm));
// Command fakeArmCommand = new InstantCommand(() -> System.err.println("==============Store================="), arm);
// eventMap.put("Stored Pos.", new SequentialCommandGroup(fakeArmCommand, new WaitCommand(2)));
eventMap.put("Run Cube Intake", new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, () -> false, arm), new RunRoller(roller, RollerMode.INTAKE_CUBE)));
eventMap.put("Run Cube Intake", new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, () -> false, arm), new RunRoller(roller, Roller.RollerMode.INTAKE_CUBE)));
eventMap.put("DriveOverChargeStation", new DriveOverChargeStation(drivetrain));
eventMap.put("Extend Arm High Cube", new SetArmWristGoalPreset(GoalPos.HIGH, () -> true, () -> false, arm));
eventMap.put("Extend Arm Mid Cube", new SetArmWristGoalPreset(GoalPos.MID, () -> true, () -> false, arm));
Expand All @@ -87,9 +86,9 @@ public RobotContainer() {
new SetArmWristGoalPreset(GoalPos.HIGH, () -> true, () -> false, arm),
new PrintCommand("================================Cube High Pos. Ended==================================")
));
eventMap.put("Run Cube Outtake", new RunRoller(roller, RollerMode.OUTTAKE_CUBE));
eventMap.put("Run Cone Intake", new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, () -> false, arm), new RunRoller(roller, RollerMode.INTAKE_CONE)));
eventMap.put("Run Cone Outtake", new RunRoller(roller, RollerMode.OUTTAKE_CONE));
eventMap.put("Run Cube Outtake", new RunRoller(roller, Roller.RollerMode.OUTTAKE_CUBE));
eventMap.put("Run Cone Intake", new SequentialCommandGroup(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, () -> false, arm), new RunRoller(roller, Roller.RollerMode.INTAKE_CONE)));
eventMap.put("Run Cone Outtake", new RunRoller(roller, Roller.RollerMode.OUTTAKE_CONE));
//eventMap.put("Move Arm Back", new SetArmWristPositionV3((-5*Math.PI)/8, Constants.Arm.WRIST_STOW_POS_RAD, arm));
eventMap.put("Cone Intake Pos.", new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, () -> false, arm));
eventMap.put("Cube Intake Pos.", new SequentialCommandGroup(
Expand Down Expand Up @@ -171,23 +170,23 @@ private void configureButtonBindingsManipulator() {
new POVButton(manipulatorController, Manipulator.substationPickupPOV).onTrue(new SetArmWristGoalPreset(GoalPos.STORED, isCube, isFront, arm));

new POVButton(manipulatorController, Manipulator.intakeCubePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, isFront, arm));
new POVButton(manipulatorController, Manipulator.intakeConePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, isFront, arm).andThen(new RunRoller(roller, RollerMode.INTAKE_CONE)));
new POVButton(manipulatorController, Manipulator.substationPickupPOV).onTrue(new SetArmWristGoalPreset(GoalPos.SUBSTATION, isCube, isFront, arm).andThen(new RunRoller(roller, RollerMode.INTAKE_CONE)));
new POVButton(manipulatorController, Manipulator.intakeCubePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, isFront, arm).andThen(new RunRoller(roller, RollerMode.INTAKE_CUBE)));
new JoystickButton(manipulatorController, Manipulator.stopRollerButton).onTrue(new RunRoller(roller, RollerMode.STOP));
new POVButton(manipulatorController, Manipulator.intakeConePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> false, isFront, arm).andThen(new RunRoller(roller, Roller.RollerMode.INTAKE_CONE)));
new POVButton(manipulatorController, Manipulator.substationPickupPOV).onTrue(new SetArmWristGoalPreset(GoalPos.SUBSTATION, isCube, isFront, arm).andThen(new RunRoller(roller, Roller.RollerMode.INTAKE_CONE)));
new POVButton(manipulatorController, Manipulator.intakeCubePOV).onTrue(new SetArmWristGoalPreset(GoalPos.INTAKE, () -> true, isFront, arm).andThen(new RunRoller(roller, Roller.RollerMode.INTAKE_CUBE)));
new JoystickButton(manipulatorController, Manipulator.stopRollerButton).onTrue(new RunRoller(roller, Roller.RollerMode.STOP));

// axisTrigger(manipulatorController, Manipulator.rollerIntakeConeButton)
// .onTrue(new RunRoller(roller, RollerMode.INTAKE_CONE, Constants.Roller.conePickupColor));
axisTrigger(manipulatorController, Manipulator.rollerIntakeCubeButton)
.onTrue(new ConditionalCommand(
new RunRoller(roller, RollerMode.INTAKE_CUBE),
new RunRoller(roller, RollerMode.OUTTAKE_CUBE),
new RunRoller(roller, Roller.RollerMode.INTAKE_CUBE),
new RunRoller(roller, Roller.RollerMode.OUTTAKE_CUBE),
isIntake
));
axisTrigger(manipulatorController, Manipulator.rollerIntakeConeButton)
.onTrue(new ConditionalCommand(
new RunRoller(roller, RollerMode.INTAKE_CONE),
new RunRoller(roller, RollerMode.OUTTAKE_CONE),
new RunRoller(roller, Roller.RollerMode.INTAKE_CONE),
new RunRoller(roller, Roller.RollerMode.OUTTAKE_CONE),
isIntake
));

Expand Down
17 changes: 9 additions & 8 deletions src/main/java/org/carlmontrobotics/robotcode2023/commands/RunRoller.java
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,7 @@
package org.carlmontrobotics.robotcode2023.commands;

import org.carlmontrobotics.robotcode2023.Constants.Roller.GameObject;
import org.carlmontrobotics.robotcode2023.Constants.Roller.RollerMode;
import org.carlmontrobotics.robotcode2023.subsystems.Roller;
import org.carlmontrobotics.robotcode2023.Constants.Roller.GameObject;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
Expand All @@ -12,9 +11,9 @@ public class RunRoller extends CommandBase {

private final Roller roller;
private final Timer timer = new Timer();
private final RollerMode mode;
private final Roller.RollerMode mode;

public RunRoller(Roller roller, RollerMode mode) {
public RunRoller(Roller roller, Roller.RollerMode mode) {
addRequirements(this.roller = roller);
this.mode = mode;
}
Expand All @@ -23,7 +22,7 @@ public RunRoller(Roller roller, RollerMode mode) {
public void initialize() {
System.err.println("=============================RunRoller is Started=============================");

if (mode.getSpeed() > 0) { // should not interrupt command to stop rollers
if (mode.speed > 0) { // should not interrupt command to stop rollers
if(roller.hasGamePiece() && isIntake()) cancel();
if(!roller.hasGamePiece() && !isIntake()) cancel();
}
Expand All @@ -38,8 +37,10 @@ public void execute() {}
@Override
public void end(boolean interrupted) {
// keep it running if its a cone
if (mode.obj != GameObject.CONE && !interrupted)
roller.setSpeed(0);
if (mode.obj != GameObject.CONE && !interrupted) {
mode.speed = 0;
roller.setRollerMode(mode);
}
timer.stop();
System.err.println("=============================RunRoller has ended==========================================================");

Expand All @@ -48,7 +49,7 @@ public void end(boolean interrupted) {
@Override
public boolean isFinished() {

if(mode.getSpeed() == 0) return true;
if(mode.speed == 0) return true;

double time = timer.get();

Expand Down
47 changes: 37 additions & 10 deletions src/main/java/org/carlmontrobotics/robotcode2023/subsystems/Roller.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,26 +89,26 @@ public double getGamePieceDistanceIn() {
}

public void setRollerMode(RollerMode mode) {
setSpeed(mode.getSpeed());
setSpeed(mode.speed);
setLedColor(mode.ledColor);
}

public void putRollerConstsOnSmartDashboard() {
SmartDashboard.putNumber("Intake Cone Speed", RollerMode.INTAKE_CONE.getSpeed());
SmartDashboard.putNumber("Outtake Cone Speed", RollerMode.OUTTAKE_CONE.getSpeed());
SmartDashboard.putNumber("Intake Cube Speed", RollerMode.INTAKE_CUBE.getSpeed());
SmartDashboard.putNumber("Outtake Cube Speed", RollerMode.OUTTAKE_CUBE.getSpeed());
SmartDashboard.putNumber("Intake Cone Speed", RollerMode.INTAKE_CONE.speed);
SmartDashboard.putNumber("Outtake Cone Speed", RollerMode.OUTTAKE_CONE.speed);
SmartDashboard.putNumber("Intake Cube Speed", RollerMode.INTAKE_CUBE.speed);
SmartDashboard.putNumber("Outtake Cube Speed", RollerMode.OUTTAKE_CUBE.speed);
SmartDashboard.putNumber("Intake Cone Time", RollerMode.INTAKE_CONE.time);
SmartDashboard.putNumber("Outtake Cone Time", RollerMode.OUTTAKE_CONE.time);
SmartDashboard.putNumber("Intake Cube Time", RollerMode.INTAKE_CUBE.time);
SmartDashboard.putNumber("Outtake Cube Time", RollerMode.OUTTAKE_CUBE.time);
}

public void getRollerConstsOnSmartDashboard() {
RollerMode.INTAKE_CONE.setSpeed(SmartDashboard.getNumber("Intake Cone Speed", RollerMode.INTAKE_CONE.getSpeed()));
RollerMode.OUTTAKE_CONE.setSpeed(SmartDashboard.getNumber("Outtake Cone Speed", RollerMode.OUTTAKE_CONE.getSpeed()));
RollerMode.INTAKE_CUBE.setSpeed(SmartDashboard.getNumber("Intake Cube Speed", RollerMode.INTAKE_CUBE.getSpeed()));
RollerMode.OUTTAKE_CUBE.setSpeed(SmartDashboard.getNumber("Outtake Cube Speed", RollerMode.OUTTAKE_CUBE.getSpeed()));
RollerMode.INTAKE_CONE.speed = SmartDashboard.getNumber("Intake Cone Speed", RollerMode.INTAKE_CONE.speed);
RollerMode.OUTTAKE_CONE.speed = SmartDashboard.getNumber("Outtake Cone Speed", RollerMode.OUTTAKE_CONE.speed);
RollerMode.INTAKE_CUBE.speed = SmartDashboard.getNumber("Intake Cube Speed", RollerMode.INTAKE_CUBE.speed);
RollerMode.OUTTAKE_CUBE.speed = SmartDashboard.getNumber("Outtake Cube Speed", RollerMode.OUTTAKE_CUBE.speed);
RollerMode.INTAKE_CONE.time = SmartDashboard.getNumber("Intake Cone Time", RollerMode.INTAKE_CONE.time);
RollerMode.OUTTAKE_CONE.time = SmartDashboard.getNumber("Outtake Cone Time", RollerMode.OUTTAKE_CONE.time);
RollerMode.INTAKE_CUBE.time = SmartDashboard.getNumber("Intake Cube Time", RollerMode.INTAKE_CUBE.time);
Expand All @@ -118,5 +118,32 @@ public void getRollerConstsOnSmartDashboard() {
public double getPosition() {
return motor.getEncoder().getPosition();
}


// TODO: Determine actual speeds/timings for roller
public static class RollerMode {
public static RollerMode INTAKE_CONE = new RollerMode(-0.5, .5, GameObject.CONE, conePickupColor);
public static RollerMode INTAKE_CUBE = new RollerMode(0.4, .25, GameObject.CUBE, cubePickupColor);
// The obj indicates which game object the roller is trying to intake
// if obj == NONE, that means it is trying to outtake rather than intake
public static RollerMode OUTTAKE_CONE = new RollerMode(0.5, .5, GameObject.NONE, defaultColor);
public static RollerMode OUTTAKE_CUBE = new RollerMode(-0.5, .5, GameObject.NONE, defaultColor);
public static RollerMode STOP = new RollerMode(0, .1, GameObject.NONE, defaultColor);
public double speed;
public double time;
public GameObject obj;
public Color ledColor;

/**
* @param speed A number between -1 and 1
* @param time Amount of time in seconds to keep the motor running after
* distance sensor has detected an object
* @param intake Whether the roller is outtaking or intaking
*/
public RollerMode(double speed, double time, GameObject obj, Color ledColor) {
this.speed = speed;
this.time = time;
this.obj = obj;
this.ledColor = ledColor;
}
}
}

0 comments on commit eec4b4a

Please sign in to comment.