Skip to content
This repository has been archived by the owner on Feb 13, 2023. It is now read-only.

Commit

Permalink
Complete comp bot 2019 code (bag day)
Browse files Browse the repository at this point in the history
  • Loading branch information
@FinnitoProductions
FinnitoProductions committed Feb 20, 2019
1 parent c66a4dc commit 2fee5fb
Show file tree
Hide file tree
Showing 10 changed files with 88 additions and 124 deletions.
73 changes: 17 additions & 56 deletions src/main/java/frc/robot/OI.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ public int getId() {
private static final int OPERATOR_PORT = 1;

public static final double DRIVER_DEADBAND = 0.15;
public static final double DRIVER_DEADBAND_TRIGGER = 0.15;
public static final double DRIVER_DEADBAND_TRIGGER = 0.16;
public static final double OPERATOR_DEADBAND_JOYSTICK = 0.1;
public static final double OPERATOR_DEADBAND_TRIGGER = 0.1;

Expand All @@ -68,11 +68,6 @@ public int getId() {
public static final int DPAD_RIGHT_ANGLE = 90;
public static final int DPAD_DOWN_ANGLE = 180;

private int driverControlScheme = 0;
private static final int NUM_DRIVERS = 2;
private static final int CHRIS_CONTROL_SCHEME = 0;
private static final int PRANAV_CONTROL_SCHEME = 1;
private static final int ANGELA_CONTROL_SCHEME = 0;
private static Driver driver;

private static boolean cargoBayToggleMode;
Expand Down Expand Up @@ -108,57 +103,23 @@ public void initBindings() {
// System.out.println("START");
// }

//});

if(driverControlScheme % NUM_DRIVERS == CHRIS_CONTROL_SCHEME) {
driver = Driver.CHRIS;
driverGamepad = new XboxGamepad(DRIVER_PORT);

driverGamepad.getButtonBumperLeft().whenPressed(new ToggleArmPosition());
driverGamepad.getButtonB().whenPressed(new ToggleFlowerState());
driverGamepad.getButtonA().whenPressed(new ToggleExtenderState());
//driverGamepad.getButtonBumperRight().whenPressed(new StowHatchIntake());


} else if(driverControlScheme % NUM_DRIVERS == PRANAV_CONTROL_SCHEME) {
driver = Driver.PRANAV;
driverGamepad = new XboxGamepad(DRIVER_PORT);

//driverGamepad.getButtonBumperRight().whileActive(new AlignWithLimelightDrive(Limelight.TX_SETPOINT));

driverGamepad.getButtonBumperRight().whileActive(new InstantCommand() {
@Override
public void initialize() {
driverGamepad.setRumble(RumbleType.kRightRumble, 1.0);
}
});

driverGamepad.getButtonBumperRight().whenReleased(new InstantCommand() {
@Override
driverGamepad.getButtonBumperRight().whenPressed(new InstantCommand() {
@Override
public void initialize() {
driverGamepad.setRumble(RumbleType.kRightRumble, 0.0);
}
});

driverGamepad.getButtonB().whileActive(new SpinIntakeIndefinite(Intake.DEFAULT_INTAKE_MAGNITUDE, IntakeDirection.IN));

driverGamepad.getButtonA().whenPressed(new ToggleArmPosition());

driverGamepad.getButtonX().whenPressed(new ToggleFlowerState());
driverGamepad.getButtonY().whenPressed(new ToggleExtenderState());

} else if(driverControlScheme % NUM_DRIVERS == ANGELA_CONTROL_SCHEME) {
// driver = Driver.ANGELA;
// driverGamepad = new XboxGamepad(DRIVER_PORT);
// driverGamepad.getButtonA().whenPressed(new ToggleFlowerState());
// driverGamepad.getButtonBumperLeft().whenPressed(new ToggleExtenderState());
// driverGamepad.getButtonBumperRight().whenPressed(new AlignWithLimelightDrive(Limelight.TX_SETPOINT));
// driverGamepad.getDownDPadButton().whenPressed(new SetArmPosition(ArmDirection.UP));
// driverGamepad.getUpDPadButton().whenPressed(new SetArmPosition(ArmDirection.DOWN));

// driverGamepad.getButtonB().whenPressed(new SpinIntakeIndefinite(Intake.DEFAULT_INTAKE_MAGNITUDE, IntakeDirection.IN));

}
cargoBayToggleMode = !cargoBayToggleMode;
if(cargoBayToggleMode) {
customOperatorGamepad.getForwardOneButton().whenPressed(new SetScoringPosition(Location.CARGO_SHIP_FRONT));
customOperatorGamepad.getBackwardOneButton().whenPressed(new SetScoringPosition(Location.CARGO_SHIP_BACK));
} else {
customOperatorGamepad.getForwardOneButton().whenPressed(new SetScoringPosition(Location.F1));
customOperatorGamepad.getBackwardOneButton().whenPressed(new SetScoringPosition(Location.B1));
}}});

driverGamepad.getButtonBumperLeft().whenPressed(new ToggleArmPosition());
driverGamepad.getButtonB().whenPressed(new ToggleFlowerState());
driverGamepad.getButtonA().whenPressed(new ToggleExtenderState());
//driverGamepad.getButtonBumperRight().whenPressed(new StowHatchIntake());

customOperatorGamepad.getForwardOneButton().whenPressed(new SetScoringPosition(Location.F1));
customOperatorGamepad.getForwardTwoButton().whenPressed(new SetScoringPosition(Location.F2));
Expand All @@ -171,7 +132,7 @@ public void initialize() {
customOperatorGamepad.getOuttakeButton().whenPressed(new SetScoringPosition(Location.CARGO_INTAKE));
customOperatorGamepad.getZeroButton().whilePressed(new ZeroWrist());
customOperatorGamepad.getIntakeHatchButton().whilePressed(new ZeroElevator());
customOperatorGamepad.getStowButton().whenPressed(new SetScoringPosition(Location.B1, true));
customOperatorGamepad.getStowButton().whenPressed(new SetScoringPosition(Location.B1, () -> true));

// customOperatorGamepad.getOuttakeButton().whenPressed(new InstantCommand() {
// @Override
Expand Down
3 changes: 3 additions & 0 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,6 +115,8 @@ public void autonomousPeriodic() {
public void teleopInit() {
wrapper.start();
startTime = Timer.getFPGATimestamp();
Elevator.getInstance().setElevator(ControlMode.Disabled, 0);
Wrist.getInstance().setWrist(ControlMode.Disabled, 0);
// Elevator.getInstance().getMaster().configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute);
//Wrist.getInstance().getMasterTalon().configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute);
}
Expand Down Expand Up @@ -146,6 +148,7 @@ public void robotPeriodic() {
SmartDashboard.putNumber("Wrist Position", Wrist.getInstance().getCurrentAngleDegrees());
SmartDashboard.putNumber("Encoder Position", Elevator.getInstance().getMasterTalon().getSelectedSensorPosition());
SmartDashboard.putNumber("LEFT Y", OI.getInstance().getDriverGamepad().getLeftY());
SmartDashboard.putNumber("el current", Elevator.getInstance().getMasterTalon().getOutputCurrent());
// //System.out.println(limelight.getCamtranData());
// SmartDashboard.putNumber("right y", OI.getInstance().getDriverGamepad().getRightY());
// SmartDashboard.putNumber("right x", OI.getInstance().getDriverGamepad().getRightX());
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/elevator/MoveElevatorManual.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ public void initialize() {
@Override
public void execute() {
double desiredSpeed = MathUtil.mapJoystickOutput(OI.getInstance().getDriverGamepad().getRightY(), OI.DRIVER_DEADBAND);
Elevator.getInstance().getMasterTalon().set(ControlMode.PercentOutput, desiredSpeed);
Elevator.getInstance().setElevator(ControlMode.PercentOutput, desiredSpeed);
// if(Math.abs(desiredSpeed) > 0)
// {
// boolean isDown = desiredSpeed < 0;
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/commands/elevator/MoveElevatorMotionMagic.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ protected void initialize() {
*/
@Override
protected void execute() {
Elevator.getInstance().getMasterTalon().set(ControlMode.MotionMagic, setpoint);
Elevator.getInstance().setElevator(ControlMode.MotionMagic, setpoint);
SmartDashboard.putNumber("el error", Elevator.getInstance().getMasterTalon().getClosedLoopError()) ;

}
Expand Down
17 changes: 3 additions & 14 deletions src/main/java/frc/robot/commands/elevator/ZeroElevator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
*/
public class ZeroElevator extends Command {

private static final double ZERO_SPEED = -0.26;
private static final double ZERO_SPEED = -0.34;
private static final double CURRENT_SPIKE = 2.8;
private static final double TIMEOUT = 5000;

Expand All @@ -40,7 +40,7 @@ public void initialize() {

@Override
public void execute() {
Elevator.getInstance().getMasterTalon().set(ControlMode.PercentOutput, ZERO_SPEED);
Elevator.getInstance().setElevator(ControlMode.PercentOutput, ZERO_SPEED);
}

/**
Expand All @@ -52,18 +52,7 @@ protected boolean isFinished() {
return true;
}

if ((Robot.getTime() - startTime) > Elevator.PEAK_TIME) {
System.out.println(Elevator.getInstance().getMasterTalon().getOutputCurrent());
currentVals.add(Elevator.getInstance().getMasterTalon().getOutputCurrent());
if (currentVals.size() >= VALUES_TO_SAMPLE) {
currentVals.remove(0);

boolean isAllValid = true;
for (double currentVal : currentVals) {isAllValid = isAllValid && currentVal > CURRENT_SPIKE;}
return isAllValid;
}
}
return false;
return Elevator.getInstance().getMasterTalon().getSensorCollection().isRevLimitSwitchClosed();
}

protected void end () {
Expand Down
64 changes: 26 additions & 38 deletions src/main/java/frc/robot/commands/groups/SetScoringPosition.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,13 +10,16 @@
import frc.robot.Robot.Side;
import frc.robot.commands.arm.SetArmPosition;
import frc.robot.commands.elevator.MoveElevatorMotionMagic;
import frc.robot.commands.hatchpanelintake.SetExtenderManual;
import frc.robot.commands.wrist.MoveWristMotionMagic;
import frc.robot.subsystems.Arm.ArmDirection;
import frc.robot.subsystems.HatchLatcher.ExtenderDirection;
import frc.robot.subsystems.Elevator;
import frc.robot.subsystems.HatchLatcher;
import frc.robot.subsystems.Wrist;
import frc.robot.util.ConditionalCommand;
import frc.robot.util.Pair;
import harkerrobolib.commands.CallMethodCommand;

/**
* Passes through and moves to a desired height.
Expand All @@ -29,14 +32,14 @@
*/
public class SetScoringPosition extends CommandGroup {
public enum Location {
F1(Elevator.LOW_ROCKET_SCORING_POSITION_CARGO, Wrist.SCORING_POSITION_FRONT_CARGO, Elevator.LOW_SCORING_POSITION_HATCH, Wrist.SCORING_POSITION_FRONT_HATCH),
F2(Elevator.MEDIUM_ROCKET_SCORING_POSITION_CARGO, Wrist.SCORING_POSITION_FRONT_CARGO, Elevator.MEDIUM_SCORING_POSITION_HATCH, Wrist.SCORING_POSITION_FRONT_HATCH),
F1(Elevator.LOW_ROCKET_SCORING_POSITION_CARGO, Wrist.SCORING_POSITION_FRONT_CARGO_2, Elevator.LOW_SCORING_POSITION_HATCH, Wrist.SCORING_POSITION_FRONT_HATCH),
F2(Elevator.HIGH_ROCKET_SCORING_POSITION_CARGO, Wrist.SCORING_POSITION_BACK_CARGO, Elevator.MEDIUM_SCORING_POSITION_HATCH, Wrist.SCORING_POSITION_FRONT_HATCH),
F3(Elevator.HIGH_ROCKET_SCORING_POSITION_CARGO, Wrist.SCORING_POSITION_BACK_CARGO, Elevator.HIGH_SCORING_POSITION_HATCH, Wrist.SCORING_POSITION_BACK_HATCH),
B1(Elevator.LOW_ROCKET_SCORING_POSITION_CARGO, Wrist.SCORING_POSITION_BACK_CARGO, Elevator.LOW_SCORING_POSITION_HATCH, Wrist.SCORING_POSITION_BACK_HATCH),
B2(Elevator.MEDIUM_ROCKET_SCORING_POSITION_CARGO, Wrist.SCORING_POSITION_BACK_CARGO, Elevator.MEDIUM_SCORING_POSITION_HATCH, Wrist.SCORING_POSITION_BACK_HATCH),
B2(Elevator.HIGH_ROCKET_SCORING_POSITION_CARGO, Wrist.SCORING_POSITION_BACK_CARGO_2, Elevator.MEDIUM_SCORING_POSITION_HATCH, Wrist.SCORING_POSITION_BACK_HATCH),
B3(Elevator.HIGH_ROCKET_SCORING_POSITION_CARGO, Wrist.SCORING_POSITION_BACK_CARGO, Elevator.HIGH_SCORING_POSITION_HATCH, Wrist.SCORING_POSITION_BACK_HATCH),
CARGO_SHIP_FRONT(Elevator.CARGO_SHIP_SCORING_POSITION_CARGO, Wrist.SCORING_POSITION_FRONT_CARGO, Elevator.CARGO_SHIP_SCORING_POSITION_HATCH, Wrist.SCORING_POSITION_FRONT_HATCH),
CARGO_SHIP_BACK(Elevator.CARGO_SHIP_SCORING_POSITION_CARGO, Wrist.SCORING_POSITION_BACK_CARGO, Elevator.CARGO_SHIP_SCORING_POSITION_HATCH, Wrist.SCORING_POSITION_BACK_HATCH),
CARGO_SHIP_FRONT(Elevator.CARGO_SHIP_SCORING_POSITION_CARGO_FRONT, Wrist.SCORING_POSITION_FRONT_CARGO_SHIP, Elevator.LOW_SCORING_POSITION_HATCH, Wrist.SCORING_POSITION_FRONT_HATCH),
CARGO_SHIP_BACK(Elevator.CARGO_SHIP_SCORING_POSITION_CARGO_FRONT, Wrist.SCORING_POSITION_BACK_CARGO_SHIP, Elevator.LOW_SCORING_POSITION_HATCH, Wrist.SCORING_POSITION_BACK_HATCH),

ZERO(0, 0, 0, 0),
HATCH_INTAKE(Integer.MAX_VALUE, Integer.MAX_VALUE, Elevator.HATCH_INTAKING_POSITION, Wrist.HATCH_INTAKING_POSITION),
Expand Down Expand Up @@ -83,44 +86,18 @@ public SetScoringPosition () {
}

public SetScoringPosition(Location desiredLocation) {
this.desiredLocation = desiredLocation;

Supplier<Integer> getDesiredHeight = () -> desiredLocation.getDesiredHeightAndAngle().getFirst();
Supplier<Integer> getDesiredAngle = () -> desiredLocation.getDesiredHeightAndAngle().getSecond();
Supplier<Side> getDesiredSide = () -> Wrist.getInstance().getSide(getDesiredAngle.get());
Supplier<Integer> getSafePassthroughHeight = () -> HatchLatcher.getInstance().hasHatch() ? Elevator.SAFE_LOW_PASSTHROUGH_POSITION_HATCH :
Elevator.SAFE_LOW_PASSTHROUGH_POSITION_CARGO;
BooleanSupplier mustPassthroughLow = () -> Wrist.getInstance().getCurrentSide() != getDesiredSide.get();
BooleanSupplier mustPassthroughHigh = () -> Elevator.getInstance().isAbove(Elevator.RAIL_POSITION) &&
(Wrist.getInstance().getCurrentSide() == Side.FRONT ||
Wrist.getInstance().getCurrentSide() == Side.AMBIGUOUS);
this(desiredLocation, () -> HatchLatcher.getInstance().hasHatch());
}

addSequential(new InstantCommand() {
@Override
public void initialize() {
Elevator.getInstance().getMasterTalon().set(ControlMode.Disabled, 0.0);
Wrist.getInstance().getMasterTalon().set(ControlMode.Disabled, 0.0);
}
});

addSequential(new SetArmPosition(ArmDirection.DOWN));
addSequential (new ConditionalCommand(mustPassthroughHigh, new MoveWristMotionMagic(Wrist.BACK_LOW_PASSTHROUGH_ANGLE)));
addSequential (new ConditionalCommand(() -> mustPassthroughLow.getAsBoolean() && Elevator.getInstance().isAbove(getSafePassthroughHeight.get()), // needs to pass through robot and lower to max passthrough
new MoveElevatorMotionMagic(getSafePassthroughHeight)));
addSequential(new MoveWristMotionMagic(() -> (mustPassthroughLow.getAsBoolean() ?
(getDesiredSide.get() == Side.FRONT ? Wrist.FRONT_LOW_PASSTHROUGH_ANGLE : Wrist.BACK_LOW_PASSTHROUGH_ANGLE)
: getDesiredAngle.get()))); // perform the passthrough OR simply move to desired angle
addSequential (new MoveElevatorMotionMagic(getDesiredHeight));
addSequential (new MoveWristMotionMagic( (desiredLocation == Location.F3) ? (() -> (HatchLatcher.getInstance().hasHatch() ? Wrist.FRONT_HIGH_PASSTHROUGH_HATCH : Wrist.FRONT_HIGH_PASSTHROUGH_CARGO)) : getDesiredAngle));
}
public SetScoringPosition(Location desiredLocation, Supplier<Boolean> doHatch) {

public SetScoringPosition(Location desiredLocation, boolean doHatch) {
this.desiredLocation = desiredLocation;

Supplier<Integer> getDesiredHeight = () -> doHatch ? desiredLocation.getHatchHeight() : desiredLocation.getCargoHeight();
Supplier<Integer> getDesiredAngle = () -> doHatch ? desiredLocation.getHatchAngle() : desiredLocation.getCargoAngle();
Supplier<Integer> getDesiredHeight = () -> doHatch.get() ? desiredLocation.getHatchHeight() : desiredLocation.getCargoHeight();
Supplier<Integer> getDesiredAngle = () -> doHatch.get() ? desiredLocation.getHatchAngle() : desiredLocation.getCargoAngle();
Supplier<Side> getDesiredSide = () -> Wrist.getInstance().getSide(getDesiredAngle.get());
Supplier<Integer> getSafePassthroughHeight = () -> doHatch ? Elevator.SAFE_LOW_PASSTHROUGH_POSITION_HATCH :
Supplier<Integer> getSafePassthroughHeight = () -> doHatch.get() ? Elevator.SAFE_LOW_PASSTHROUGH_POSITION_HATCH :
Elevator.SAFE_LOW_PASSTHROUGH_POSITION_CARGO;
BooleanSupplier mustPassthroughLow = () -> Wrist.getInstance().getCurrentSide() != getDesiredSide.get();
BooleanSupplier mustPassthroughHigh = () -> Elevator.getInstance().isAbove(Elevator.RAIL_POSITION) &&
Expand All @@ -135,14 +112,25 @@ public void initialize() {
}
});

addSequential(new CallMethodCommand(() -> System.out.println(doHatch.get() + " " + getDesiredAngle.get())));
addSequential(new SetArmPosition(ArmDirection.DOWN));
addSequential(new SetExtenderManual(ExtenderDirection.IN));
addSequential (new ConditionalCommand(mustPassthroughHigh, new MoveWristMotionMagic(Wrist.BACK_HIGH_PASSTHROUGH_ANGLE)));
addSequential (new ConditionalCommand(() -> mustPassthroughLow.getAsBoolean() && Elevator.getInstance().isAbove(getSafePassthroughHeight.get()), // needs to pass through robot and lower to max passthrough
new MoveElevatorMotionMagic(getSafePassthroughHeight)));
addSequential(new MoveWristMotionMagic(() -> (mustPassthroughLow.getAsBoolean() ?
(getDesiredSide.get() == Side.FRONT ? Wrist.FRONT_LOW_PASSTHROUGH_ANGLE : Wrist.BACK_LOW_PASSTHROUGH_ANGLE)
: getDesiredAngle.get()))); // perform the passthrough OR simply move to desired angle
addSequential (new MoveElevatorMotionMagic(getDesiredHeight));
addSequential (new MoveWristMotionMagic( (desiredLocation == Location.F3) ? (() -> (doHatch ? Wrist.FRONT_HIGH_PASSTHROUGH_HATCH : Wrist.FRONT_HIGH_PASSTHROUGH_CARGO)) : getDesiredAngle));
if (desiredLocation == Location.F3) {
addSequential (new MoveWristMotionMagic(() -> (doHatch.get() ? Wrist.FRONT_HIGH_PASSTHROUGH_HATCH : Wrist.SCORING_POSITION_FRONT_CARGO_3)));
}
else if (desiredLocation == Location.F2) {
addSequential (new MoveWristMotionMagic(() -> (doHatch.get() ? Wrist.SCORING_POSITION_FRONT_HATCH : Wrist.SCORING_POSITION_FRONT_CARGO_2)));
}
else {
addSequential (new MoveWristMotionMagic(getDesiredAngle));
}

}
}
16 changes: 15 additions & 1 deletion src/main/java/frc/robot/commands/wrist/MoveWristManual.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.OI;
import frc.robot.RobotMap.Global;
import frc.robot.subsystems.Wrist;
Expand All @@ -17,6 +18,8 @@
* @since 1/10/19
*/
public class MoveWristManual extends IndefiniteCommand {
private boolean isHolding;
private double lastPos;
public MoveWristManual () {
requires (Wrist.getInstance());
}
Expand All @@ -26,7 +29,10 @@ public MoveWristManual () {
*/
@Override
public void initialize () {
isHolding = false;
lastPos = Integer.MIN_VALUE;
Wrist.getInstance().getMasterTalon().configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, Global.PID_PRIMARY);
Wrist.getInstance().setupMotionMagic();
}

/**
Expand All @@ -41,6 +47,7 @@ public void execute() {
WristDirection direction;
//double currentPosition = Wrist.getInstance().getMasterTalon().getSelectedSensorPosition(Global.PID_PRIMARY);
if (leftOperatorTrigger > OI.DRIVER_DEADBAND_TRIGGER || rightOperatorTrigger > OI.DRIVER_DEADBAND_TRIGGER) {
isHolding = false;;
if (leftOperatorTrigger > rightOperatorTrigger) {
/*double distFromBack = Math.abs(currentPosition - Wrist.MAX_BACKWARD_POSITION);
Expand All @@ -63,7 +70,14 @@ public void execute() {
Wrist.getInstance().setWristPercentOutput(magnitude, direction);
}
else {
//Wrist.getInstance().setWrist(ControlMode.MotionMagic, Wrist.getInstance().getCurrentAngleEncoder());
if (!isHolding) {lastPos = Wrist.getInstance().getCurrentAngleEncoder();}
isHolding = true;
}

if (isHolding) {
if (lastPos != Integer.MIN_VALUE)
Wrist.getInstance().setWrist(ControlMode.MotionMagic, lastPos);
SmartDashboard.putNumber("Wrist Error", Wrist.getInstance().getMasterTalon().getClosedLoopError());
}
}

Expand Down
Loading

0 comments on commit 2fee5fb

Please sign in to comment.