Skip to content

Commit

Permalink
BatB Changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@ControlsNarwhal
ControlsNarwhal committed Oct 16, 2023
1 parent cae7f09 commit 506d54f
Show file tree
Hide file tree
Showing 8 changed files with 146 additions and 24 deletions.
8 changes: 4 additions & 4 deletions src/main/java/frc/team3128/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,9 +79,9 @@ public static class AutoConstants {
public static final Pose2d ClimbSetupOutsideBot = new Pose2d(5.6, 2.9, Rotation2d.fromDegrees(180));
public static final Pose2d ClimbSetupOutsideTop = new Pose2d(5.6, 3.3, Rotation2d.fromDegrees(180));

public static final double ANGLE_THRESHOLD = 9; //7, 9
public static final double VELOCITY_THRESHOLD = 3; //6, 3
public static final double RAMP_THRESHOLD = 10; //8, 10
public static final double ANGLE_THRESHOLD = 8; //7, 9
public static final double VELOCITY_THRESHOLD = 4; //6, 3
public static final double RAMP_THRESHOLD = 9; //8, 10
public static final double DRIVE_SPEED = Units.inchesToMeters(40); //30, 40
public static final double kP = 0.000000001;
public static final double kI = 0.0;
Expand Down Expand Up @@ -551,7 +551,7 @@ public static class IntakeConstants {
public static final double ROTATOR_GEAR_RATIO = 1.0 / 30.0;

public static final double ENCODER_CONVERSION_FACTOR_TO_DEGREES = 360;
public static final double ANGLE_OFFSET = 138+35;
public static final double ANGLE_OFFSET = 20;

public static final int ENCODER_DIO_ID = 8;

Expand Down
3 changes: 2 additions & 1 deletion src/main/java/frc/team3128/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,8 @@ public RobotContainer() {

private void configureButtonBindings() {
controller.getButton("A").onTrue(new InstantCommand(()-> Vision.AUTO_ENABLED = !Vision.AUTO_ENABLED));
controller.getButton("RightTrigger").onTrue(CmdPickup(ArmPosition.GROUND_CONE, true)).onFalse(new CmdMoveArm(ArmPosition.NEUTRAL).beforeStarting(CmdStopManip()));
controller.getButton("RightTrigger").onTrue(new InstantCommand(()-> Vision.MANUAL = !Vision.MANUAL));
// controller.getButton("RightTrigger").onTrue(CmdPickup(ArmPosition.GROUND_CONE, true)).onFalse(new CmdMoveArm(ArmPosition.NEUTRAL).beforeStarting(CmdStopManip()));
controller.getButton("LeftTrigger").onTrue(new InstantCommand(()-> Swerve.throttle = .25)).onFalse(new InstantCommand(()-> Swerve.throttle = 1));
controller.getButton("X").onTrue(new RunCommand(()-> swerve.xlock(), swerve)).onFalse(new InstantCommand(()-> swerve.stop(),swerve));
controller.getButton("B").onTrue(new InstantCommand(()-> swerve.resetEncoders()));
Expand Down
52 changes: 39 additions & 13 deletions src/main/java/frc/team3128/autonomous/AutoPrograms.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,10 +8,14 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc.team3128.Constants.SwerveConstants;
import frc.team3128.Constants.ArmConstants.ArmPosition;
import frc.team3128.commands.CmdAutoBalance;
import frc.team3128.commands.CmdAutoBalance2;
import frc.team3128.commands.CmdMoveArm;
import frc.team3128.common.narwhaldashboard.NarwhalDashboard;
import frc.team3128.common.utility.Log;
Expand Down Expand Up @@ -53,27 +57,49 @@ private void initAutoSelector() {
"r_mid_1Cone+1Cube+Climb",
//Hp
// "r_hp_1Cone+1Cube",
"r_cable_1Cone+2Cube"
"r_cable_1Cone+2Cube",
"ScuffedClimb"

};
NarwhalDashboard.addAutos(autoStrings);
}

public Command getAutonomousCommand() {
String selectedAutoName = NarwhalDashboard.getSelectedAutoName();
// String selectedAutoName = "b_cable_1Cone+1Cube"; //uncomment and change this for testing without opening Narwhal Dashboard
if (selectedAutoName == null || !Trajectories.contains(selectedAutoName)) {
return sequence(
new CmdMoveArm(ArmPosition.TOP_CONE),
runOnce(() -> Manipulator.getInstance().outtake()),
waitSeconds(.5),
runOnce(() -> Manipulator.getInstance().stopRoller()),
new CmdMoveArm(ArmPosition.NEUTRAL)
);
}
SmartDashboard.putString(selectedAutoName, selectedAutoName);
return sequence(
runOnce(()-> Swerve.getInstance().zeroGyro(DriverStation.getAlliance() == Alliance.Blue ? 0 : 180)),
new CmdMoveArm(ArmPosition.TOP_CONE),
runOnce(() -> Manipulator.getInstance().outtake()),
waitSeconds(.5),
runOnce(() -> Manipulator.getInstance().stopRoller()),
new CmdMoveArm(ArmPosition.NEUTRAL),
new CmdAutoBalance()
);

return Trajectories.get(selectedAutoName, selectedAutoName.contains("Climb"));
// if (selectedAutoName == "ScuffedClimb") {
// return sequence(
// runOnce(()-> Swerve.getInstance().zeroGyro(DriverStation.getAlliance() == Alliance.Blue ? 0 : 180)),
// new CmdMoveArm(ArmPosition.TOP_CONE),
// runOnce(() -> Manipulator.getInstance().outtake()),
// waitSeconds(.5),
// runOnce(() -> Manipulator.getInstance().stopRoller()),
// new CmdMoveArm(ArmPosition.NEUTRAL),
// new CmdAutoBalance()
// );
// }
// // String selectedAutoName = "b_cable_1Cone+1Cube"; //uncomment and change this for testing without opening Narwhal Dashboard
// if (selectedAutoName == null || !Trajectories.contains(selectedAutoName)) {
// return sequence(
// new CmdMoveArm(ArmPosition.TOP_CONE),
// runOnce(() -> Manipulator.getInstance().outtake()),
// waitSeconds(.5),
// runOnce(() -> Manipulator.getInstance().stopRoller()),
// new CmdMoveArm(ArmPosition.NEUTRAL)
// );
// }
// // SmartDashboard.putString(selectedAutoName, selectedAutoName);

// return Trajectories.get(selectedAutoName, selectedAutoName.contains("Climb"));
}

// /**
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/team3128/commands/CmdAutoBalance.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ public void execute() {
return;
}

swerve.drive(new Translation2d(onRamp ? DRIVE_SPEED * (advAngle > 0.0 ? 1.0 : -1.0) : DRIVE_SPEED, 0), 0, false);
swerve.drive(new Translation2d(onRamp ? DRIVE_SPEED * (advAngle > 0.0 ? 1.0 : -1.0) : DRIVE_SPEED * 2, 0), 0, false);
}

@Override
Expand Down
92 changes: 92 additions & 0 deletions src/main/java/frc/team3128/commands/CmdAutoBalance2.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,92 @@
package frc.team3128.commands;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.CommandBase;
import static frc.team3128.Constants.AutoConstants.*;

import frc.team3128.common.utility.NAR_Shuffleboard;
import frc.team3128.subsystems.Intake;
import frc.team3128.subsystems.Manipulator;
import frc.team3128.subsystems.Pivot;
import frc.team3128.subsystems.Swerve;

public class CmdAutoBalance2 extends CommandBase{
private final Swerve swerve;
private double prevAngle;
private double angleVelocity;
private int plateauCount;
private boolean onRamp;
private boolean shoot;

public CmdAutoBalance2() {
swerve = Swerve.getInstance();
addRequirements(Swerve.getInstance());
}

public CmdAutoBalance2(boolean shoot) {
this.shoot = shoot;
swerve = Swerve.getInstance();
addRequirements(Swerve.getInstance());
}

@Override
public void initialize() {
swerve.zeroAxis();
prevAngle = swerve.getPitch();
onRamp = false;
plateauCount = 0;
angleVelocity = 0;
}

public double getAngleVelocity(double currAngle) {
if (plateauCount < 5) {
plateauCount ++;
return angleVelocity;
}
plateauCount = 0;
angleVelocity = (currAngle - prevAngle) / 0.1;
prevAngle = currAngle;
return angleVelocity;
}

@Override
public void execute() {
final double advAngle = swerve.getPitch();
// final double angleVelocity = (advAngle - prevAngle) / 0.02;
final double angleVelocity = getAngleVelocity(advAngle);

if (advAngle > RAMP_THRESHOLD) onRamp = true;

if (Math.abs(advAngle) < ANGLE_THRESHOLD && onRamp) {
System.out.println("WHY");
swerve.xlock();
if (shoot) {
Intake.getInstance().outtake();
Intake.getInstance().startPID(0);
}
return;
}

NAR_Shuffleboard.addData("Drivetrain", "no", angleVelocity, 6, 2);
if (((advAngle < 0.0 && angleVelocity > VELOCITY_THRESHOLD) || (advAngle > 0.0 && angleVelocity < -VELOCITY_THRESHOLD)) && onRamp) {
swerve.xlock();
return;
}

swerve.drive(new Translation2d(onRamp ? -DRIVE_SPEED * (advAngle > 0.0 ? 1.0 : -1.0) : -DRIVE_SPEED, 0), 0, false);
}

@Override
public void end(boolean interrupted) {
swerve.xlock();
}

@Override
public boolean isFinished() {
return false;
}

}
2 changes: 1 addition & 1 deletion src/main/java/frc/team3128/commands/CmdManager.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ public static CommandBase CmdScore(boolean isReversed, ArmPosition position, int
new InstantCommand(() -> NarwhalDashboard.setGridCell(xpos,position.height)),
new InstantCommand(()-> Vision.AUTO_ENABLED = DriverStation.isAutonomous()),
new WaitUntilCommand(()-> Vision.AUTO_ENABLED),
new CmdTrajectory(xpos, position == ArmPosition.LOW_FLOOR),
// new CmdTrajectory(xpos, position == ArmPosition.LOW_FLOOR),
either(none(), CmdPivot(position), ()-> position == ArmPosition.LOW_FLOOR),
new InstantCommand(()-> Vision.AUTO_ENABLED = false),
vibrateController()
Expand Down
7 changes: 5 additions & 2 deletions src/main/java/frc/team3128/commands/CmdTrajectory.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,9 @@ private CommandBase generateAuto() {
public void initialize() {
trajCommand = generateAuto();
endPoint = allianceFlip(END_POINTS[index]);
trajCommand.schedule();
if (!Vision.MANUAL) {
trajCommand.schedule();
}
CmdSwerveDrive.enabled = false;
}

Expand All @@ -109,6 +111,7 @@ public void execute() {

@Override
public boolean isFinished(){
return swerve.getPose().minus(endPoint).getTranslation().getNorm() < 0.5;
return true;
// return swerve.getPose().minus(endPoint).getTranslation().getNorm() < 0.5;
}
}
4 changes: 2 additions & 2 deletions src/main/java/frc/team3128/subsystems/Intake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ public class Intake extends PIDSubsystem {
private static Intake instance;

public enum IntakeState {
DEPLOYED(-3),
DEPLOYED(0),
RETRACTED(100);

public double angle;
Expand Down Expand Up @@ -79,7 +79,7 @@ public void configEncoders() {
}

public double getAngle() {
return MathUtil.inputModulus(-m_encoder.get() * ENCODER_CONVERSION_FACTOR_TO_DEGREES - ANGLE_OFFSET,-180, 180);
return MathUtil.inputModulus(m_encoder.get() * ENCODER_CONVERSION_FACTOR_TO_DEGREES - ANGLE_OFFSET,-180, 180);
}

public void startPID(IntakeState desiredState) {
Expand Down

0 comments on commit 506d54f

Please sign in to comment.