Skip to content

Commit

Permalink
Merge branch 'integration' into dev
Browse files Browse the repository at this point in the history
  • Loading branch information
@IsaacThoman
IsaacThoman committed Feb 20, 2024
2 parents a0429e6 + 0799bae commit 008251c
Show file tree
Hide file tree
Showing 9 changed files with 251 additions and 80 deletions.
49 changes: 29 additions & 20 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,24 +35,26 @@ public static class EFCConstants{
public static final double shooterkA = 0.055923;


public static final int intakeMasterID = 20;
public static final int intakeFollowerID = 21;
public static final int intakeMasterID = 11;
public static final int intakeFollowerID = 12;

public static final int shooterMasterID = 22;
public static final int shooterFollowerID = 23;

public static final double intakeSpeed = 0.5;
public static final double outtakeSpeed = -0.5;
public static final double outtakeShooterVelocity = -1;
public static final double intakeSpeed = 0.7;
public static final double outtakeSpeed = -0.4;
public static final double outtakeShooterVelocity = 0;

public static final double ampShooterVelocity = 0;
public static final double ampShooterVelocity = 1000;

public static final double feedToShooterSpeed = 0.75;
public static final double feedToShooterSpeed = 1;

}

public static class ArmConstants{

public static final double armGearRatio = (40.0/14) * 80.0;

public static final int armMasterID = 30;
public static final int armFollowerID = 31;
public static final double duckingRad = Units.degreesToRadians(20.0);
Expand All @@ -63,6 +65,13 @@ public static class ArmConstants{
public static final double intakeRad = Units.degreesToRadians(6);
}

public static class ClimbConstants{
public static final int climbMasterID = 50;
public static final int climbFollowerID = 51;

public static final double rotationsInTheClimbRange = 73.2;
}



public static class AutoConstants {
Expand All @@ -82,7 +91,7 @@ public static class OperatorConstants {
public static final double turnMaxSpeed = 11; //11


public static final double maxDrivePower = 1;
public static final double maxDrivePower = 0.2;

public static final double controllerDeadband = 0.06;

Expand Down Expand Up @@ -122,21 +131,21 @@ public static final class DriveConstants{


/*Motor IDs and offsets */
public static final int frontLeftDriveID = 1;
public static final int frontLeftTurningID = 4;
public static final double frontLeftOffsetRad = 0.945;
public static final int frontLeftDriveID = 8;
public static final int frontLeftTurningID = 2;
public static final double frontLeftOffsetRad = 3.80 - Math.PI/2;

public static final int frontRightDriveID = 6;
public static final int frontRightTurningID = 5;
public static final double frontRightOffsetRad = -5.1929;
public static final int frontRightDriveID = 3;
public static final int frontRightTurningID = 7;
public static final double frontRightOffsetRad = 2.33 + Math.PI/2;

public static final int rearRightDriveID = 3;
public static final int rearRightTurningID = 7;
public static final double rearRightOffsetRad = 2.361;
public static final int rearRightDriveID = 1;
public static final int rearRightTurningID = 4;
public static final double rearRightOffsetRad = 2.27 + Math.PI/2;

public static final int rearLeftDriveID = 8;
public static final int rearLeftTurningID = 2;
public static final double rearLeftOffsetRad = -2.296;
public static final int rearLeftDriveID = 6;
public static final int rearLeftTurningID = 5;
public static final double rearLeftOffsetRad = 2.19 + Math.PI/2;



Expand Down
16 changes: 13 additions & 3 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ public class RobotContainer {
CommandXboxController driver = new CommandXboxController(0);
CommandXboxController operator = new CommandXboxController(1);

StateControllerSub stateController = new StateControllerSub(()->driver.getLeftTriggerAxis()>0.5 || driver.getRightTriggerAxis()>0.5); //this MUST be created before any pathplanner commands
StateControllerSub stateController = new StateControllerSub(()->driver.getLeftTriggerAxis()>0.5); //this MUST be created before any pathplanner commands


VisionSubsystem visionSub = new VisionSubsystem();
Expand Down Expand Up @@ -70,6 +70,11 @@ private void configureBindings() {
operator.x().onTrue(new InstantCommand(stateController::ejectPressed));
operator.y().onTrue(new InstantCommand(stateController::readyToShootPressed));

driver.a().onTrue(new InstantCommand(stateController::intakePressed));
driver.b().onTrue(new InstantCommand(stateController::holdPressed));
driver.x().onTrue(new InstantCommand(stateController::ejectPressed));
driver.y().onTrue(new InstantCommand(stateController::readyToShootPressed));

// operator.leftBumper().onTrue(new InstantCommand(stateController::stowPressed));
// operator.rightBumper().onTrue(new InstantCommand(stateController::raiseClimbPressed));
// operator.rightTrigger(0.5).onTrue(new InstantCommand(stateController::climbPressed));
Expand All @@ -80,10 +85,13 @@ private void configureBindings() {
operator.povLeft().onTrue(new InstantCommand(stateController::trapPressed));

operator.leftTrigger(0.5).onTrue(new InstantCommand(stateController::shootPressed));
driver.leftBumper().onTrue(new InstantCommand(stateController::shootPressed));


driver.rightTrigger(0.5).onTrue(new InstantCommand(stateController::scheduleAlignmentCommand,swerveSubsystem));
driver.rightTrigger(0.5).onFalse(swerveSubsystem.getDefaultCommand());


driver.leftTrigger(0.5).onTrue(new InstantCommand(stateController::scheduleAlignmentCommand,swerveSubsystem));
driver.leftTrigger(0.5).onFalse(swerveSubsystem.getDefaultCommand());

//TODO driver.x().onTrue(new InstantCommand(stateController::shootPressed));

Expand All @@ -101,6 +109,8 @@ private void configureBindings() {
driver.a().whileTrue(efSub.sysIdDynamic(SysIdRoutine.Direction.kForward));
driver.b().whileTrue(efSub.sysIdDynamic(SysIdRoutine.Direction.kReverse));



// operator.a().onTrue(new InstantCommand(()->stateController.setArmState(StateControllerSub.ArmState.INTAKE)));

}
Expand Down
68 changes: 47 additions & 21 deletions src/main/java/frc/robot/StateControllerSub.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.*;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
Expand All @@ -28,17 +29,17 @@ public enum ClimbState{DOWN,READY,CLIMBED}
public enum Objective{SPEAKER,AMP,SOURCE,TRAP}
public enum SelectedTrap{AMP,SOURCE,REAR}

public enum DuckMode {DUCKING,NOT_LOWERED}
public enum DuckMode {UNDUCK, DOWN}

private ArmState armState = ArmState.HOLD;
private EFState efState = EFState.HOLD;
private ClimbState climbState = ClimbState.READY;
Objective objective = Objective.SPEAKER;
SelectedTrap selectedTrap = SelectedTrap.AMP;

DuckMode duckMode = DuckMode.NOT_LOWERED;
DuckMode duckMode = DuckMode.DOWN;

boolean alignWhenClose = true;
boolean alignWhenClose = false;


private Pose2d robotPose = new Pose2d();
Expand All @@ -55,7 +56,7 @@ public ClimbState getClimbState(){
}

public Optional<Rotation2d> getRotationTargetOverride(){
if(objective == Objective.SOURCE && distanceToObjective(Objective.SOURCE) > 2.5)
if(objective == Objective.SOURCE && distanceToObjective(Objective.SOURCE) > 3)
return Optional.of(AllianceFlipUtil.apply(Rotation2d.fromDegrees(0)));

if(objective == Objective.AMP && distanceToObjective(Objective.AMP) > 3)
Expand All @@ -67,7 +68,7 @@ public Optional<Rotation2d> getRotationTargetOverride(){


public void scheduleAlignmentCommand(){
duckMode = DuckMode.DUCKING;
duckMode = DuckMode.DOWN;
armState = ArmState.HOLD;


Expand Down Expand Up @@ -118,7 +119,7 @@ private double distanceToMySpeaker(){
public double getHoldAngle(){ // this is redundant now :)
// if(duckMode == DuckMode.DUCKING)
// return Constants.ArmConstants.duckingRad;
return Constants.ArmConstants.holdingRadSafe;
return Constants.ArmConstants.duckingRad;
}

public double getSpeakerAngle(){ //TODO: arm angle in radians
Expand Down Expand Up @@ -163,7 +164,7 @@ public void setArmState(ArmState state){

public Objective getObjective(){return objective;}

Supplier<Boolean> forceDuck;
Supplier<Boolean> forceUnDuck;


public StateControllerSub(Supplier<Boolean> forceDuck){
Expand All @@ -179,7 +180,7 @@ public StateControllerSub(Supplier<Boolean> forceDuck){
SmartDashboard.putNumber("tuningFlywheelVel",10);


this.forceDuck = forceDuck;
this.forceUnDuck = forceDuck;
}

double climbAngle = 0.0;
Expand All @@ -202,8 +203,12 @@ public void periodic(){
Pose3d armPose = new Pose3d(0.24,0,0.21, new Rotation3d(0,armAngleRad-(Math.PI/2),0));
Pose3d climbPose = new Pose3d(-0.07,0,0.14, new Rotation3d(0,climbAngle,0));

SmartDashboard.putNumberArray("armPose2D", new double[]{armPose.getX(),armPose.getY(),armPose.getZ(),armPose.getRotation().getQuaternion().getW(),armPose.getRotation().getQuaternion().getX(),armPose.getRotation().getQuaternion().getY(),armPose.getRotation().getQuaternion().getZ()});
SmartDashboard.putNumberArray("climbPose2D", new double[]{climbPose.getX(),climbPose.getY(),climbPose.getZ(),climbPose.getRotation().getQuaternion().getW(),climbPose.getRotation().getQuaternion().getX(),climbPose.getRotation().getQuaternion().getY(),climbPose.getRotation().getQuaternion().getZ()});
//SmartDashboard.putNumberArray("armPose2D", new double[]{armPose.getX(),armPose.getY(),armPose.getZ(),armPose.getRotation().getQuaternion().getW(),armPose.getRotation().getQuaternion().getX(),armPose.getRotation().getQuaternion().getY(),armPose.getRotation().getQuaternion().getZ()});
//SmartDashboard.putNumberArray("climbPose2D", new double[]{climbPose.getX(),climbPose.getY(),climbPose.getZ(),climbPose.getRotation().getQuaternion().getW(),climbPose.getRotation().getQuaternion().getX(),climbPose.getRotation().getQuaternion().getY(),climbPose.getRotation().getQuaternion().getZ()});

//publish arm and climb 3d poses to network tables
table.getEntry("armPose").setDoubleArray(new double[]{armPose.getX(),armPose.getY(),armPose.getZ(),armPose.getRotation().getQuaternion().getW(),armPose.getRotation().getQuaternion().getX(),armPose.getRotation().getQuaternion().getY(),armPose.getRotation().getQuaternion().getZ()});
table.getEntry("climbPose").setDoubleArray(new double[]{climbPose.getX(),climbPose.getY(),climbPose.getZ(),climbPose.getRotation().getQuaternion().getW(),climbPose.getRotation().getQuaternion().getX(),climbPose.getRotation().getQuaternion().getY(),climbPose.getRotation().getQuaternion().getZ()});


SmartDashboard.putNumber("distanceToMySpeaker",distanceToMySpeaker());
Expand All @@ -220,19 +225,27 @@ public void periodic(){

SmartDashboard.putNumber("distToObjective",distanceToObjective(objective));

if(duckMode == DuckMode.DUCKING){
climbState = ClimbState.DOWN;
}else{
if(duckMode == DuckMode.UNDUCK){
if(climbState == ClimbState.DOWN)
climbState = ClimbState.READY;
}else{
climbState = ClimbState.DOWN;
}

SmartDashboard.putBoolean("alignToObjective",alignWhenClose);

if(forceDuck.get())
duckMode = DuckMode.DUCKING;
if(forceUnDuck.get())
duckMode = DuckMode.UNDUCK;
else
duckMode = DuckMode.NOT_LOWERED;
duckMode = DuckMode.DOWN;


if(makeEFHoldTimer.hasElapsed(0.3)){
efState = EFState.HOLD;
makeEFHoldTimer.stop();
makeEFHoldTimer.reset();

}

}

Expand All @@ -251,9 +264,11 @@ public void toggleAlignWhenClose() {
}

public void publishTableEntries(){
table.getEntry("odom_x").setDouble(robotPose.getX());
table.getEntry("odom_y").setDouble(robotPose.getY());
table.getEntry("odom_rad").setDouble(robotPose.getRotation().getRadians());
// table.getEntry("odom_x").setDouble(robotPose.getX());
// table.getEntry("odom_y").setDouble(robotPose.getY());
// table.getEntry("odom_rad").setDouble(robotPose.getRotation().getRadians());

table.getEntry("odometry").setDoubleArray(new double[]{robotPose.getX(),robotPose.getY(),robotPose.getRotation().getRadians()});

table.getEntry("armState").setString(armState.toString());
table.getEntry("efState").setString(efState.toString());
Expand All @@ -270,9 +285,20 @@ public void intakePressed(){
efState = EFState.INTAKE;
// climbState = ClimbState.STOWED; //TODO: should this be here?
}

Timer makeEFHoldTimer = new Timer();

public void holdPressed(){
armState = ArmState.HOLD;
efState = EFState.HOLD;
armState=ArmState.HOLD;

if(efState == EFState.INTAKE){
efState= EFState.EJECT;
makeEFHoldTimer.reset();
makeEFHoldTimer.start();
}else{
efState = EFState.HOLD;
}

}
public void ejectPressed(){
//armState = ArmState.HOLD;
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/commands/drive/Alignments.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ public class Alignments {


// Load the path we want to pathfind to and follow
static PathPlannerPath path = PathPlannerPath.fromPathFile("speaker-align");
// static PathPlannerPath path = PathPlannerPath.fromPathFile("speaker-align");

// Create the constraints to use while pathfinding. The constraints defined in the path will only be used for the path.
static PathConstraints constraints = new PathConstraints(4.5, 3.0, Units.degreesToRadians(540), Units.degreesToRadians(720));
Expand All @@ -30,7 +30,7 @@ public class Alignments {
);

public static Command sourceAlign(){
Pose2d pose = new Pose2d(14,1.1, Rotation2d.fromDegrees(0));
Pose2d pose = new Pose2d(14,1.1, Rotation2d.fromDegrees(180));
pose = AllianceFlipUtil.apply(pose);
return AutoBuilder.pathfindToPose(pose,constraints,0);

Expand Down
Loading

0 comments on commit 008251c

Please sign in to comment.