Skip to content

Commit

Permalink
Merge pull request #68 from chopshop-166/Champs
Browse files Browse the repository at this point in the history 
Champs
  • Loading branch information
@bot190
bot190 committed May 10, 2023
2 parents 6bed4d0 + cd8badb commit adb7131
Show file tree
Hide file tree
Showing 5 changed files with 83 additions and 18 deletions.
56 changes: 52 additions & 4 deletions src/main/java/frc/robot/Auto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,10 +11,12 @@

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StringSubscriber;
import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
import frc.robot.auto.AutoPath;
import frc.robot.auto.ConeStation;
Expand Down Expand Up @@ -68,23 +70,55 @@ public CommandBase leaveCommunityAndPickUP() {

private CommandBase armScore(ArmPresets aboveLevel, ArmPresets scoreLevel) {
return sequence(
armRotate.moveTo(aboveLevel), armExtend.moveTo(aboveLevel),
armRotate.moveTo(scoreLevel), armExtend.moveTo(ArmPresets.ARM_STOWED));
armRotate.moveTo(aboveLevel, new Constraints(150,
1000)),
armExtend.moveTo(aboveLevel),
armRotate.moveTo(scoreLevel, new Constraints(150,
100)),
armExtend.retract(0.4));
}

private boolean wasInterrupted = false;

public CommandBase scoreWheninterupted() {
return runOnce(() -> {
wasInterrupted = false;
}).andThen(armScore(ArmPresets.HIGH_SCORE, ArmPresets.HIGH_SCORE_ACTUAL).finallyDo(Interrupted -> {
wasInterrupted = true;
}).withTimeout(10), Commands.either(raiseArm(), runOnce(() -> {
}), () -> wasInterrupted));
}

public CommandBase raiseArm() {
return armRotate.moveTo(ArmPresets.HIGH_SCORE);
}

// THE ONE THAT ACTUALLY WORKS
public CommandBase scoreConeWhile(CommandBase commandWhileStow) {
return sequence(
drive.setGyro180(),
// backUp(1.5, 0.2),
armRotate.moveTo(ArmPresets.HIGH_SCORE).withTimeout(1.5),
armRotate.moveTo(ArmPresets.HIGH_SCORE, new Constraints(150,
250)).withTimeout(1.5),
// backUp(-1.5, 0.2),
drive.driveRelative(new Translation2d(-Units.inchesToMeters(4), 0), 180, 2),
armScore(ArmPresets.HIGH_SCORE, ArmPresets.HIGH_SCORE_ACTUAL),
moveFor(1.0, 0.3),
parallel(stowArmCloseIntake(),
commandWhileStow).withTimeout(5));
}

public CommandBase scoreConeRotateWhile(CommandBase commandWhileStow) {
return sequence(
drive.setGyro180(),
// backUp(1.5, 0.2),
armRotate.moveTo(ArmPresets.HIGH_SCORE, new Constraints(150,
250)).withTimeout(1.5),
// backUp(-1.5, 0.2),
drive.driveRelative(new Translation2d(-Units.inchesToMeters(4), 0), 180, 2),
armScore(ArmPresets.HIGH_SCORE, ArmPresets.HIGH_SCORE_ACTUAL),
drive.driveRelative(new Translation2d(Units.inchesToMeters(18), 0), Rotation2d.fromDegrees(0), 5),
parallel(stowArmCloseIntake(),
commandWhileStow).withTimeout(5));
}

// Score cone and back up onto charge station (from pos 1) and then balance
Expand All @@ -102,6 +136,20 @@ public CommandBase scoreConeBalance() {
.withName("Score Cone Balance");
}

public CommandBase scoreConeBalanceRotate() {
return sequence(
// armRotate.zeroVelocityCheck(),
balanceArm.pushDown(),
scoreConeRotateWhile(drive.driveUntilTipped(true)),
led.balancing(),
drive.balance(),
led.starPower(),
balanceArm.pushUp()

)
.withName("(TEST) Score Cone Rotate Balance");
}

public CommandBase scoreConeLeaveAndBalance() {
return sequence(
// armRotate.zeroVelocityCheck(),
Expand Down
26 changes: 18 additions & 8 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
import com.chopshop166.chopshoplib.controls.ButtonXboxController;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StringPublisher;
import edu.wpi.first.networktables.StringSubscriber;
Expand Down Expand Up @@ -69,8 +70,11 @@ public class Robot extends CommandRobot {
public CommandBase noAuto = runOnce(() -> {
});

// @Autonomous(name = "Score then balance")
// public CommandBase scoreBalance = auto.scoreConeBalance();
@Autonomous(name = "Score then balance")
public CommandBase scoreBalance = auto.scoreConeBalance();

@Autonomous(name = "(TEST) Score then rotate then balance")
public CommandBase scoreRotBalance = auto.scoreConeBalanceRotate();

@Autonomous(name = "Just Score")
public CommandBase scorewhile = auto.scoreConeWhile(runOnce(() -> {
Expand Down Expand Up @@ -100,15 +104,19 @@ public class Robot extends CommandRobot {
armExtend.moveTo(ArmPresets.ARM_STOWED));

public CommandBase scoreMidNode = sequence(
armRotate.moveTo(ArmPresets.MEDIUM_SCORE),
armRotate.moveTo(ArmPresets.MEDIUM_SCORE, new Constraints(150,
1000)),
armExtend.moveTo(ArmPresets.MEDIUM_SCORE),
armRotate.moveTo(ArmPresets.MEDIUM_SCORE_ACTUAL),
armRotate.moveTo(ArmPresets.MEDIUM_SCORE_ACTUAL, new Constraints(150,
100)),
armExtend.moveTo(ArmPresets.ARM_STOWED));

public CommandBase scoreHighNode = sequence(
armRotate.moveTo(ArmPresets.HIGH_SCORE),
armRotate.moveTo(ArmPresets.HIGH_SCORE, new Constraints(150,
1000)),
armExtend.moveTo(ArmPresets.HIGH_SCORE),
armRotate.moveTo(ArmPresets.HIGH_SCORE_ACTUAL),
armRotate.moveTo(ArmPresets.HIGH_SCORE_ACTUAL, new Constraints(150,
100)),
armExtend.moveTo(ArmPresets.ARM_STOWED));

public CommandBase grabCube() {
Expand Down Expand Up @@ -152,7 +160,8 @@ public CommandBase rumbleAndIntakeSpinningOff() {
armRotate.moveTo(ArmPresets.CONE_PICKUP), armExtend.moveTo(ArmPresets.CONE_PICKUP)),
sequence(
armRotate.moveTo(ArmPresets.CUBE_PICKUP), armExtend.moveTo(
ArmPresets.CUBE_PICKUP)),
ArmPresets.CUBE_PICKUP),
intake.coneRelease()),
() -> {
return gamePieceSub.get() == "Cone";
}));
Expand Down Expand Up @@ -236,7 +245,8 @@ public void configureButtonBindings() {
copilotController.rightBumper().onTrue(grabCone());
copilotController.leftBumper().onTrue(grabCube());
// will need buttons for the enums
copilotController.y().whileTrue(armRotate.moveTo(ArmPresets.HPS_PICKUP));
copilotController.y().whileTrue(armRotate.moveTo(ArmPresets.HPS_PICKUP, new Constraints(150,
250)));
copilotController.povUp()
.whileTrue(scoreHighNode);
copilotController.povRight()
Expand Down
5 changes: 3 additions & 2 deletions src/main/java/frc/robot/maps/FrostBiteMap.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -162,16 +162,17 @@ public ArmRotateMap getArmRotateMap() {
csmotor.getMotorController().setIdleMode(IdleMode.kCoast);
csmotor.getEncoder().setPositionScaleFactor(1.125);
csmotor.getEncoder().setVelocityScaleFactor(1.125 / 60);
ProfiledPIDController pid = new ProfiledPIDController(0.03, 0.002, 0.0, new Constraints(150, 200));
ProfiledPIDController pid = new ProfiledPIDController(0.045, 0.001, 0.0, new Constraints(150, 200));
pid.setTolerance(2);

CSEncoder encoder = new CSEncoder(2, 3, true);
encoder.setDistancePerPulse(360.0 / 2048.0);
CSDutyCycleEncoder absEncoder = new CSDutyCycleEncoder(4);
// rohdfshkfjhsdkjhfdskjhfdsjkf
absEncoder.setDutyCycleRange(1.0 / 1025.0, 1024.0 / 1025.0);
absEncoder.setDistancePerRotation(-360);
// Adjust this to move the encoder zero point to the retracted position
absEncoder.setPositionOffset(91.89780758483522);
absEncoder.setPositionOffset(91.89780758483522 + 3.75);

CSFusedEncoder fusedEncoder = new CSFusedEncoder(encoder, absEncoder);

Expand Down
12 changes: 9 additions & 3 deletions src/main/java/frc/robot/subsystems/ArmRotate.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,8 @@
import com.chopshop166.chopshoplib.commands.SmartSubsystemBase;

import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
import edu.wpi.first.networktables.BooleanSubscriber;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.DoubleSubscriber;
Expand Down Expand Up @@ -67,14 +69,14 @@ public boolean intakeBelowGround() {

}

public CommandBase moveToAngle(double angle) {
public CommandBase moveToAngle(double angle, Constraints rotateConstraints) {
// When executed the arm will move. The encoder will update until the desired
// value is reached, then the command will end.
PersistenceCheck setPointPersistenceCheck = new PersistenceCheck(20, pid::atGoal);
return cmd("Move To Set Angle").onInitialize(() -> {
pid.reset(getArmAngle(), data.rotatingAngleVelocity);
}).onExecute(() -> {
data.setPoint = pid.calculate(getArmAngle(), angle) + NO_FALL;
data.setPoint = pid.calculate(getArmAngle(), new State(angle, 0), rotateConstraints) + NO_FALL;
Logger.getInstance().recordOutput("getPositionErrors", pid.getPositionError());

}).runsUntil(setPointPersistenceCheck).onEnd(() -> {
Expand Down Expand Up @@ -109,8 +111,12 @@ public CommandBase resetZero() {
});
}

public CommandBase moveTo(ArmPresets level, Constraints rotateConstraints) {
return moveToAngle(level.getAngle(useAbsolute), rotateConstraints);
}

public CommandBase moveTo(ArmPresets level) {
return moveToAngle(level.getAngle(useAbsolute));
return moveToAngle(level.getAngle(useAbsolute), new Constraints(150, 200));
}

public CommandBase resetAngle() {
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ public class Drive extends SmartSubsystemBase {
private final double TILT_THRESHOLD = 4.0;
private final double TILT_MAX_STOPPING = 1;

private final double UNTIL_TIPPED_SPEED = 2.0;
private final double UNTIL_TIPPED_SPEED = 2;
private final double UNTIL_NOT_TIPPED_SPEED = 0.5;
private final double BALANCE_SPEED = 0.25;

Expand Down

0 comments on commit adb7131

Please sign in to comment.