Skip to content

Commit

Permalink
Merge branch 'dev' of https://github.com/Team3128/3128-robot-2023-prebot
Browse files Browse the repository at this point in the history 
 into dev
  • Loading branch information
@leo-les
leo-les committed Oct 14, 2023
2 parents 17d35ac + 10bff79 commit aa801ae
Show file tree
Hide file tree
Showing 13 changed files with 108 additions and 63 deletions.
65 changes: 34 additions & 31 deletions src/main/java/frc/team3128/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,15 +53,15 @@ public static class TrajectoryConstants {
public static final double CONDITION_3 = 2.5;

public static final Pose2d[] END_POINTS = new Pose2d[]{
new Pose2d(1.75,0.5,Rotation2d.fromDegrees(0)),
new Pose2d(1.75,0.95,Rotation2d.fromDegrees(0)),
new Pose2d(1.75,1.55,Rotation2d.fromDegrees(0)),
new Pose2d(1.75,2.05,Rotation2d.fromDegrees(0)),
new Pose2d(1.75,2.65,Rotation2d.fromDegrees(0)),
new Pose2d(1.75,3.2,Rotation2d.fromDegrees(0)),
new Pose2d(1.75,3.75,Rotation2d.fromDegrees(0)),
new Pose2d(1.75,4.35,Rotation2d.fromDegrees(0)),
new Pose2d(1.75,4.79,Rotation2d.fromDegrees(0))
new Pose2d(1.75,0.5,Rotation2d.fromDegrees(180)),
new Pose2d(1.75,0.95,Rotation2d.fromDegrees(180)),
new Pose2d(1.75,1.55,Rotation2d.fromDegrees(180)),
new Pose2d(1.75,2.05,Rotation2d.fromDegrees(180)),
new Pose2d(1.75,2.65,Rotation2d.fromDegrees(180)),
new Pose2d(1.75,3.2,Rotation2d.fromDegrees(180)),
new Pose2d(1.75,3.75,Rotation2d.fromDegrees(180)),
new Pose2d(1.75,4.35,Rotation2d.fromDegrees(180)),
new Pose2d(1.75,4.79,Rotation2d.fromDegrees(180))
};
}

Expand Down Expand Up @@ -185,13 +185,11 @@ public static class SwerveConstants {
public static final double rotationKP = 2;
public static final double rotationKI = 0;
public static final double rotationKD = 0;
public static final double rotationKS = 0;

/* Turning PID Values */
public static final double turnKP = 0.1;
public static final double turnKP = 5;
public static final double turnKI = 0;
public static final double turnKD = 0;
public static final double turnKF = 0.1;

/* Angle Motor PID Values */
// switched 364 pid values to SDS pid values
Expand All @@ -218,7 +216,7 @@ public static class SwerveConstants {
// For safety, use less than theoretical and real values
public static final double maxSpeed = 4.5; //meters per second - 16.3 ft/sec
public static final double bumpSpeed = 2.5;
public static final double maxAcceleration = 2.3;
public static final double maxAcceleration = 2.5;
public static final double maxAngularVelocity = 5; //3; //11.5; // citrus: 10
public static final TrapezoidProfile.Constraints CONSTRAINTS = new TrapezoidProfile.Constraints(maxSpeed, maxAcceleration);

Expand Down Expand Up @@ -290,6 +288,11 @@ public static class VisionConstants {
new Translation2d(Units.inchesToMeters(5.43), Units.inchesToMeters(11.7)),
Rotation2d.fromDegrees(0)));

public static final Camera BACK_RIGHT = new Camera("BACK_RIGHT", true, 0, 0, 0,
new Transform2d(
new Translation2d(Units.inchesToMeters(6.57), Units.inchesToMeters(-11.7)),
Rotation2d.fromDegrees(180)));

public static final PIDController xController = new PIDController(1, 0, 0);
public static final PIDController yController = new PIDController(1, 0, 0);
public static final PIDController rController = new PIDController(1, 0, 0);
Expand All @@ -315,15 +318,15 @@ public static class VisionConstants {
public static final Matrix<N3,N1> SVR_VISION_MEASUREMENT_STD = VecBuilder.fill(1,1,Units.degreesToRadians(10));

public static final Pose2d[] SCORES = new Pose2d[]{
new Pose2d(1.90,0.5,Rotation2d.fromDegrees(180)),
new Pose2d(1.90,1.05,Rotation2d.fromDegrees(180)),
new Pose2d(1.90,1.65,Rotation2d.fromDegrees(180)),
new Pose2d(1.90,2.15,Rotation2d.fromDegrees(180)),
new Pose2d(1.90,2.75,Rotation2d.fromDegrees(180)),
new Pose2d(1.90,3.3,Rotation2d.fromDegrees(180)),
new Pose2d(1.90,3.85,Rotation2d.fromDegrees(180)),
new Pose2d(1.90,4.45,Rotation2d.fromDegrees(180)),
new Pose2d(1.90,4.89,Rotation2d.fromDegrees(180))
new Pose2d(1.90,0.55,Rotation2d.fromDegrees(180)), //0.5
new Pose2d(1.90,1.1,Rotation2d.fromDegrees(180)), //1.05
new Pose2d(1.90,1.7,Rotation2d.fromDegrees(180)), //1.65
new Pose2d(1.90,2.2,Rotation2d.fromDegrees(180)), //2.15
new Pose2d(1.90,2.8,Rotation2d.fromDegrees(180)), //2.75
new Pose2d(1.90,3.35,Rotation2d.fromDegrees(180)), //3.3
new Pose2d(1.90,3.90,Rotation2d.fromDegrees(180)), //3.85
new Pose2d(1.90,4.50,Rotation2d.fromDegrees(180)), //4.45
new Pose2d(1.90,4.94,Rotation2d.fromDegrees(180)) //4.89
};

public static final Pose2d[][] SCORES_GRID = new Pose2d[][] {
Expand Down Expand Up @@ -511,29 +514,29 @@ public static class ManipulatorConstants{
public static final int ROLLER_MOTOR_ID = 31;
public static final double ROLLER_POWER = 0.9;
public static final double STALL_POWER_CONE = 0.05;
public static final double STALL_POWER_CUBE = 0;
public static final double STALL_POWER_CUBE = 0.1;


public static final double CURRENT_THRESHOLD_CONE = 25;
public static final double CURRENT_THRESHOLD_CUBE = 15;
public static final double CURRENT_THRESHOLD_CONE = 30;
public static final double CURRENT_THRESHOLD_CUBE = 25;
}

public static class ElevatorConstants {
public static final int ELV1_ID = 11;
public static final int ELV2_ID = 12;

public static final double kP = 3;
public static final double kP = 1;
public static final double kI = 0;
public static final double kD = 0;

public static final double kS = 0.475;
public static final double kS = 0.975;
public static final double kV = 0;
public static final double kG = 0.625;
public static final double kG = 1.05;

public static final double MIN_DIST = 2; //Ask Charlie
public static final double MAX_DIST = 55; //Ask Charlie

public static final double GEAR_RATIO = 12.5;
public static final double GEAR_RATIO = 10;
public static final double SPOOL_CIRCUMFERENCE = 3 * Math.PI;
public static final double FRAME_LENGTH = 15;

Expand All @@ -552,12 +555,12 @@ public static class BalanceConstants{
}

public static class LedConstants{
public static final int CANDLE_ID = 37;
public static final int CANDLE_ID = 52;

public static final int WHITE_VALUE = 0; //leds used don't have a white value

public static final int STARTING_ID = 8;
public static final int PIVOT_COUNT = 100;
public static final int PIVOT_COUNT = 200;
public static final int PIVOT_COUNT_FRONT = 50; //change
public static final int PIVOT_COUNT_BACK = 50; //change

Expand Down
10 changes: 5 additions & 5 deletions src/main/java/frc/team3128/PositionConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,18 +3,18 @@
public class PositionConstants {
public static enum Position {
HIGH_CONE(47, 0, true),
HIGH_CUBE(50, 20, false),
MID_CONE(22.5, 28, true),
HIGH_CUBE(48.5, 20, false),
MID_CONE(25.5, 28, true),
MID_CUBE(25, 30, false),
LOW(3, 45, true),
LOW(3, 0, true),

SHELF_CONE(53, -30, true),
SHELF_CUBE(37, 22, false),
CHUTE_CONE(10, 55, true),
CHUTE_CUBE(10, 45, false),

GROUND_CONE(10, -18, true),
GROUND_CUBE(0, 0, false),
GROUND_CONE(8, -18, true),
GROUND_CUBE(2.5, 0, false),

NEUTRAL(3, 80, false);

Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/team3128/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ public void autonomousInit() {
@Override
public void autonomousPeriodic() {
CommandScheduler.getInstance().run();
if (timer.hasElapsed(14.75) && Math.abs(Swerve.getInstance().getPitch()) > 6) {
if (timer.hasElapsed(14.75)) {
new RunCommand(()->Swerve.getInstance().xlock(), Swerve.getInstance()).schedule();
}
}
Expand Down
15 changes: 8 additions & 7 deletions src/main/java/frc/team3128/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
import frc.team3128.common.utility.Log;
import frc.team3128.subsystems.Elevator;
import frc.team3128.subsystems.Leds;
import frc.team3128.subsystems.Manipulator;
import frc.team3128.common.utility.NAR_Shuffleboard;
import frc.team3128.subsystems.Swerve;
import frc.team3128.subsystems.Vision;
Expand Down Expand Up @@ -94,25 +95,25 @@ private void configureButtonBindings() {
controller.getButton("B").onTrue(new InstantCommand(()-> swerve.resetEncoders()));
controller.getButton("Y").onTrue(runOnce(()-> swerve.throttle = 1)).onFalse(runOnce(()-> swerve.throttle = 0.8));
controller.getButton("Start").onTrue(resetSwerve());
controller.getButton("RightBumper").onTrue(pickup(Position.GROUND_CUBE, true));
controller.getButton("LeftBumper").onTrue(pickup(Position.GROUND_CONE, true));
controller.getButton("RightBumper").onTrue(pickup(Position.GROUND_CUBE, true)).onFalse(retract(Position.NEUTRAL).andThen(stopManip()));
controller.getButton("LeftBumper").onTrue(pickup(Position.GROUND_CONE, true)).onFalse(retract(Position.NEUTRAL).andThen(stopManip()));

controller.getUpPOVButton().onTrue(runOnce(()-> {
CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 0 : 180;
CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 180 : 0;
CmdSwerveDrive.enabled = true;
}));
controller.getDownPOVButton().onTrue(runOnce(()-> {
CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 180 : 0;
CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 0 : 180;
CmdSwerveDrive.enabled = true;
}));

controller.getRightPOVButton().onTrue(runOnce(()-> {
CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 270 : 90;
CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 90 : 270;
CmdSwerveDrive.enabled = true;
}));

controller.getLeftPOVButton().onTrue(runOnce(()-> {
CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 90 : 270;
CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 270 : 90;
CmdSwerveDrive.enabled = true;
}));

Expand Down Expand Up @@ -145,7 +146,7 @@ private void configureButtonBindings() {

buttonPad.getButton(13).onTrue(runOnce(()-> SINGLE_STATION = !SINGLE_STATION));

buttonPad.getButton(14).onTrue(retract(Position.NEUTRAL));
buttonPad.getButton(14).onTrue(retract(Position.NEUTRAL).beforeStarting(stopManip()));

buttonPad.getButton(16).onTrue(
HPpickup(Position.CHUTE_CONE, Position.SHELF_CONE)
Expand Down
19 changes: 15 additions & 4 deletions src/main/java/frc/team3128/autonomous/AutoPrograms.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,12 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandBase;
import static edu.wpi.first.wpilibj2.command.Commands.*;
import frc.team3128.PositionConstants.Position;
import frc.team3128.common.narwhaldashboard.NarwhalDashboard;
import frc.team3128.subsystems.Swerve;
import static frc.team3128.commands.CmdManager.*;

/**
* Class to store information about autonomous routines.
Expand Down Expand Up @@ -46,14 +50,21 @@ private void initAutoSelector() {
}

public Command getAutonomousCommand() {
//String selectedAutoName = NarwhalDashboard.getSelectedAutoName();
String selectedAutoName = "b_cable_1Cone+2Cube+Climb"; //uncomment and change this for testing without opening Narwhal Dashboard
String selectedAutoName = NarwhalDashboard.getSelectedAutoName();
// String selectedAutoName = "b_cable_1Cone+2Cube+Climb"; //uncomment and change this for testing without opening Narwhal Dashboard
SmartDashboard.putString(selectedAutoName, selectedAutoName);
if (selectedAutoName == null) {
return null;
return score(Position.HIGH_CONE, true).beforeStarting(resetAuto());
}

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

public CommandBase resetAuto() {
return sequence(
resetAll(),
retract(Position.NEUTRAL)
);
}

// /**
Expand Down
14 changes: 12 additions & 2 deletions src/main/java/frc/team3128/commands/CmdManager.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
import edu.wpi.first.wpilibj2.command.StartEndCommand;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import frc.team3128.RobotContainer;
import frc.team3128.Constants.LedConstants.Colors;
import frc.team3128.PositionConstants.Position;
import frc.team3128.common.hardware.input.NAR_XboxController;

Expand All @@ -32,9 +33,10 @@ private CmdManager() {}
private static CommandBase score(Position position, int xPos, boolean runImmediately) {
return sequence(
runOnce(()-> ENABLE = runImmediately),
// waitUntil(()-> ENABLE),
// either(none(), new CmdTrajectory(xPos + Vision.SELECTED_GRID * 3), ()-> runImmediately),
waitUntil(()-> ENABLE),

//either(none(), new CmdTrajectory(xPos), ()-> runImmediately),
//waitUntil(()-> ENABLE),
extend(position),
waitUntil(()-> !ENABLE),
outtake(),
Expand All @@ -58,6 +60,7 @@ public static CommandBase HPpickup(Position position1, Position position2) {

public static CommandBase pickup(Position position, boolean runImmediately) {
return sequence(
runOnce(()->leds.setElevatorLeds(position.cone ? Colors.CONE : Colors.CUBE)),
runOnce(()-> ENABLE = runImmediately),
waitUntil(()-> ENABLE),
extend(position),
Expand Down Expand Up @@ -154,4 +157,11 @@ public static CommandBase resetWrist() {
public static CommandBase resetSwerve() {
return runOnce(()-> swerve.zeroGyro());
}

public static CommandBase resetAll() {
return sequence(
resetWrist(),
resetElevator()
);
}
}
6 changes: 2 additions & 4 deletions src/main/java/frc/team3128/commands/CmdSwerveDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,6 @@ public class CmdSwerveDrive extends CommandBase {
private final SlewRateLimiter accelLimiter;

private final PIDController rController;
private final DoubleSupplier kS;
public static boolean enabled = false;
public static double rSetpoint;

Expand All @@ -41,10 +40,10 @@ public CmdSwerveDrive(DoubleSupplier xAxis, DoubleSupplier yAxis, DoubleSupplier
this.zAxis = zAxis;

accelLimiter = new SlewRateLimiter(maxAcceleration);
rController = new PIDController(rotationKP, 0, 0);
rController = new PIDController(turnKP, 0, 0);
rController.enableContinuousInput(0, 360);
rController.setTolerance(0.5);
kS = NAR_Shuffleboard.debug("Drivetrain", "kS", rotationKS, 5, 4);

swerve.fieldRelative = fieldRelative;
}

Expand All @@ -67,7 +66,6 @@ public void execute() {
}
if (enabled) {
rotation = Units.degreesToRadians(rController.calculate(swerve.getGyroRotation2d().getDegrees(), rSetpoint));
rotation += Math.copySign(kS.getAsDouble(), rotation);
if (rController.atSetpoint()) {
rotation = 0;
}
Expand Down
13 changes: 7 additions & 6 deletions src/main/java/frc/team3128/commands/CmdTrajectory.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@ public class CmdTrajectory extends CommandBase {

private final Swerve swerve;
private final int xPos;
private Pose2d endPoint;
private int index;
private CommandBase trajCommand;

Expand Down Expand Up @@ -59,7 +60,7 @@ private boolean isPastPoint(Translation2d start, double condition) {
private ArrayList<PathPoint> generatePoses() {
final ArrayList<PathPoint> pathPoints = new ArrayList<PathPoint>();
final Translation2d start = swerve.getPose().getTranslation();
final Rotation2d holonomicAngle = allianceFlip(END_POINTS[index].getRotation());
final Rotation2d holonomicAngle = END_POINTS[index].getRotation();
final PathPoint startPoint = new PathPoint(start, allianceFlip(HEADING), swerve.getGyroRotation2d());
final boolean topPath = start.getY() >= (POINT_2A.getY() + POINT_2B.getY()) / 2;
final boolean skipLastPoint = (topPath && index == 8) || (!topPath && index == 0);
Expand All @@ -85,10 +86,10 @@ private CommandBase generateAuto() {

@Override
public void initialize() {
if (Vision.AUTO_ENABLED) {
trajCommand = generateAuto();
trajCommand.schedule();
}
trajCommand = generateAuto();
trajCommand.schedule();
CmdSwerveDrive.enabled = false;
endPoint = allianceFlip(END_POINTS[index]);
}

@Override
Expand All @@ -106,6 +107,6 @@ public void execute() {

@Override
public boolean isFinished(){
return swerve.getPose().minus(END_POINTS[index]).getTranslation().getNorm() < 0.5 || !Vision.AUTO_ENABLED;
return swerve.getPose().minus(endPoint).getTranslation().getNorm() < 0.5;
}
}
1 change: 1 addition & 0 deletions src/main/java/frc/team3128/subsystems/Elevator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@ public Elevator() {
configMotors();
initShuffleboard(kS, kV, kG);
m_controller.setTolerance(ELV_TOLERANCE);
resetEncoder();
}

public void startPID(Position position) {
Expand Down
1 change: 0 additions & 1 deletion src/main/java/frc/team3128/subsystems/Manipulator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,6 @@ public Manipulator(){
configMotor();
initShuffleboard();
}

private void configMotor(){
m_roller = new NAR_TalonSRX(ROLLER_MOTOR_ID);
m_roller.setInverted(true);
Expand Down
Loading

0 comments on commit aa801ae

Please sign in to comment.