Skip to content

Commit

Permalink
Start of playoffs
Browse files Browse the repository at this point in the history
  • Loading branch information
@rar1741programmer
rar1741programmer committed Mar 12, 2023
1 parent a5369ce commit bcf8c0f
Show file tree
Hide file tree
Showing 3 changed files with 94 additions and 17 deletions.
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@ public static class Shoulder {
public static final double k_minAngle = Units.degreesToRadians(-45.0);
public static final double k_maxAngle = Units.degreesToRadians(225.0);

public static final double k_offset = 0.660370;
public static final double k_offset = 0.749014;
}

public static class Elbow {
Expand All @@ -85,7 +85,7 @@ public static class Elbow {
public static final double k_minAngle = Units.degreesToRadians(-360.0);
public static final double k_maxAngle = Units.degreesToRadians(360.0);

public static final double k_offset = 0.381174;
public static final double k_offset = 0.386390;
}

public static class Wrist {
Expand All @@ -96,12 +96,12 @@ public static class Wrist {
public static final double k_minAngle = Units.degreesToRadians(-360.0);
public static final double k_maxAngle = Units.degreesToRadians(360.0);

public static final double k_offset = 0.646119;
public static final double k_offset = 0.606600;
}

public static enum Preset {
HOME(new ArmPose(0.0, Constants.Arm.k_homeHeight, null)),
SCORE_MID_CUBE(new ArmPose(-41.9, 35.0, null)),
SCORE_MID_CUBE(new ArmPose(-47.6, 39.66, null)),
SCORE_HIGH_CUBE(new ArmPose(-58.4, 53.9, null)),
SCORE_MID_CONE(new ArmPose(-42.0, 55.2, null)),
SCORE_HIGH_CONE(new ArmPose(-52.5, 63.5, null)),
Expand Down
102 changes: 89 additions & 13 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,10 @@ public class Robot extends TimedRobot {
private final Timer m_stoppedTimer = new Timer();
private boolean m_running = true;

// private int m_autoChoice = 2; //0 = yes cube yes drive, 1 = no cube yes drive, 2 = yes cube and no drive, 3 = do nothing

private final boolean m_scoring = true;

private final Field2d m_field = new Field2d();

@Override
Expand Down Expand Up @@ -98,6 +102,7 @@ public void robotPeriodic() {
m_allSubsystems.forEach(subsystem -> subsystem.writeToLog());
}

//Disabled because not working yet
// @Override
// public void autonomousInit() {
// // TODO: Use PathPlannerTrajectory.transformTrajectoryForAlliance make the state
Expand Down Expand Up @@ -179,27 +184,98 @@ public void autonomousInit() {
m_swerve.brakeOff();
m_swerve.resetOdometry(new Pose2d(0, 0, new Rotation2d(0)));

m_arm.generateTrajectoryToPose(Constants.Arm.Preset.SCORE_HIGH_CUBE.getPose());

m_runningTimer.reset();
m_runningTimer.start();
if (m_scoring) {
m_arm.generateTrajectoryToPose(Constants.Arm.Preset.SCORE_HIGH_CUBE.getPose());

m_runningTimer.reset();
m_runningTimer.start();
} else {
m_arm.generateTrajectoryToPose(Constants.Arm.Preset.HOME.getPose());
}
}


//Disabled because not tested yet
// @Override
// public void autonomousInit() {
// m_swerve.brakeOff();
// m_swerve.resetOdometry(new Pose2d(0, 0, new Rotation2d(0)));

// if (m_autoChoice == 0) {
// m_arm.generateTrajectoryToPose(Constants.Arm.Preset.SCORE_HIGH_CUBE.getPose());

// m_runningTimer.reset();
// m_runningTimer.start();
// } else if (m_autoChoice == 1) {
// m_arm.generateTrajectoryToPose(Constants.Arm.Preset.HOME.getPose());
// } else if (m_autoChoice == 2) {
// m_arm.generateTrajectoryToPose(Constants.Arm.Preset.SCORE_HIGH_CUBE.getPose());

// m_runningTimer.reset();
// m_runningTimer.start();
// } else if (m_autoChoice == 3) {
// m_arm.generateTrajectoryToPose(Constants.Arm.Preset.HOME.getPose());
// }
// }

@Override
public void autonomousPeriodic() {
m_arm.runTrajectory();

if (m_runningTimer.get() > 7){
m_arm.setGripper(false);
}

if(m_swerve.getPose().getX() < 5 && m_runningTimer.get() > 8) {
m_swerve.drive(1, 0, 0, true);
m_arm.generateTrajectoryToPose(Constants.Arm.Preset.HOME.getPose());
if (m_scoring) {
if (m_runningTimer.get() > 7){
m_arm.setGripper(false);
}

if(m_swerve.getPose().getX() < 5 && m_runningTimer.get() > 8) {
m_swerve.drive(1, 0, 0, true);
m_arm.generateTrajectoryToPose(Constants.Arm.Preset.HOME.getPose());
} else {
m_swerve.drive(0, 0, 0, false);
}
} else {
m_swerve.drive(0, 0, 0, false);
m_arm.generateTrajectoryToPose(Constants.Arm.Preset.HOME.getPose());
if(m_swerve.getPose().getX() < 5) {
m_swerve.drive(1, 0, 0, true);
} else {
m_swerve.drive(0, 0, 0, false);
}
}
}

//Disabled because not tested yet
// @Override
// public void autonomousPeriodic() {
// m_arm.runTrajectory();
// if (m_autoChoice == 0) {
// if (m_runningTimer.get() > 7){
// m_arm.setGripper(false);
// }

// if(m_swerve.getPose().getX() < 5 && m_runningTimer.get() > 8) {
// m_swerve.drive(1, 0, 0, true);
// m_arm.generateTrajectoryToPose(Constants.Arm.Preset.HOME.getPose());
// } else {
// m_swerve.drive(0, 0, 0, false);
// }
// } else if (m_autoChoice == 1) {
// m_arm.generateTrajectoryToPose(Constants.Arm.Preset.HOME.getPose());
// if(m_swerve.getPose().getX() < 5) {
// m_swerve.drive(1, 0, 0, true);
// } else {
// m_swerve.drive(0, 0, 0, false);
// }
// } else if (m_autoChoice == 2) {
// if (m_runningTimer.get() > 7){
// m_arm.setGripper(false);

// m_arm.generateTrajectoryToPose(Constants.Arm.Preset.HOME.getPose());
// }
// m_swerve.drive(0, 0, 0, false);
// } else if (m_autoChoice == 3) {
// m_arm.generateTrajectoryToPose(Constants.Arm.Preset.HOME.getPose());
// m_swerve.drive(0, 0, 0, false);
// }
// }

@Override
public void teleopInit() {
Expand Down
1 change: 1 addition & 0 deletions src/main/java/frc/robot/subsystems/arm/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -414,6 +414,7 @@ public void outputTelemetry() {

// Wrist
SmartDashboard.putNumber(m_smartDashboardKey + "Wrist/Position", getWristPositionDegrees());
SmartDashboard.putNumber(m_smartDashboardKey + "Wrist/AbsPosition", m_wristEncoder.getAbsolutePosition());
SmartDashboard.putNumber(m_smartDashboardKey + "Wrist/PositionError", m_wristPID.getPositionError());
SmartDashboard.putBoolean(m_smartDashboardKey + "Wrist/AtTarget", m_wristPID.atSetpoint());
SmartDashboard.putNumber(m_smartDashboardKey + "Wrist/Velocity", m_wristMotor.get());
Expand Down

0 comments on commit bcf8c0f

Please sign in to comment.