Skip to content

Commit

Permalink
All autos done
Browse files Browse the repository at this point in the history
  • Loading branch information
@2199robolions
2199robolions committed Apr 16, 2023
1 parent 44a5fed commit 2b54815
Show file tree
Hide file tree
Showing 5 changed files with 194 additions and 48 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ public enum AngleStates {
public static double[] REST_ANGLES = {0.825, 2.8, -2.9};
public static double[] MID_CONE_ANGLES = {1.05, 2.0, -0.32};
public static double[] MID_CUBE_ANGLES = {0.825, 1.986, -0.08};
public static double[] TOP_CONE_ANGLES = {2.15, 0.1, 0.6};
public static double[] TOP_CONE_ANGLES = {2.15, 0.1, 0.75};
public static double[] TOP_CUBE_ANGLES = {1.6, 1.02, 0.45};
public static double[] SHELF_ANGLES = {1.2, 0.67, 1.42};
public static double[] CHUTE_CONE_ANGLES = {0.825, 1.7, -2.8}; // Reaching backwards
Expand Down
224 changes: 188 additions & 36 deletions src/main/java/frc/robot/Auto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Arm.AngleStates;
import frc.robot.Controls.ArmStates;
import frc.robot.Controls.Objects;
Expand Down Expand Up @@ -133,50 +134,55 @@ public int wallAuto(boolean isRed, long delaySeconds) {
if (isRed == true) {
pose1 = new Pose2d(2.6, 7.401, new Rotation2d(Math.PI));
pose2 = new Pose2d(2.6, 7.401, new Rotation2d(0));
pose3 = new Pose2d(6.0, 7.401, new Rotation2d(0));
pose3 = new Pose2d(5.5, 7.401, new Rotation2d(0));
}
else {
pose1 = new Pose2d(2.6, 0.8, new Rotation2d(Math.PI));
pose2 = new Pose2d(2.6, 0.8, new Rotation2d(0));
pose3 = new Pose2d(6.0, 0.8, new Rotation2d(0));
pose3 = new Pose2d(5.5, 0.8, new Rotation2d(0));
}

status = drive.autoDriveToPoints(new Pose2d[]{pose2, pose3}, currPose);
resetArmRoutines();
break;
break;
case 6:
// Align with cube
status = armToGrabPosition();
break;
case 7:
// Align with object
arm.startIntake();
armToGrabPosition();
double angleError = nTables.getGamePieceX();
status = drive.alignWithPiece(angleError, false);
break;
case 7:
// Drive to cube
case 8:
// Drive to object
arm.startIntake();
armToGrabPosition();
status = drive.driveToCone(1.5, controls.getLimitSwitch(), currPose.getTranslation());
break;
case 8:
// Pick up cube
case 9:
// Pick up object
arm.closeClaw();
arm.stopIntake();
Robot.grabberState = GrabberStates.HOLDING_CUBE;
status = Robot.DONE;
Robot.grabberState = GrabberStates.HOLDING_CONE;
status = autoDelayMS(500);
break;
case 9:
case 10:
// Retract arm
AngleStates armStatus2 = armToRestPosition(false);
Robot.acceptedArmState = ArmStates.REST;
if (armStatus2 == AngleStates.DONE) {
status = Robot.DONE;
}
break;
case 10:
case 11:
// Storing current location
autoSecondPieceRotate[0] = new Pose2d(currPose.getX() - 0.5, currPose.getY(), new Rotation2d(Math.PI));
autoSecondPieceRotate[1] = new Pose2d(currPose.getX() - 3, currPose.getY(), new Rotation2d(Math.PI));
status = Robot.DONE;
break;
case 11:
case 12:
// Rotating
status = drive.autoDriveToPoints(autoSecondPieceRotate, currPose);
break;
Expand Down Expand Up @@ -208,7 +214,7 @@ public int wallAuto(boolean isRed, long delaySeconds) {
* @param delaySeconds
* @return status
*/
public int rampAuto(boolean isRed, Objects objectPlaced, long delaySeconds) {
public int rampAuto(boolean isRed, long delaySeconds) {
int status = Robot.CONT;

if (firstTime == true) {
Expand All @@ -227,15 +233,8 @@ public int rampAuto(boolean isRed, Objects objectPlaced, long delaySeconds) {
break;
case 2:
// Place object we're holding
if (objectPlaced == Objects.CONE) {
status = armToTopCone();
Robot.acceptedArmState = ArmStates.TOP_CONE;
}
else {
status = armToTopCube();
Robot.acceptedArmState = ArmStates.TOP_CUBE;
}

status = armToTopCone();
Robot.acceptedArmState = ArmStates.TOP_CONE;
drive.rotateWheels(-1, 0, 0);
break;
case 3:
Expand Down Expand Up @@ -281,6 +280,133 @@ public int rampAuto(boolean isRed, Objects objectPlaced, long delaySeconds) {

return Robot.CONT;
}

/**
* Shoots cube, picks up object, then balances on ramp
* @param isRed
* @param objectPlaced
* @param delaySeconds
* @return status
*/
public int rampAutoCube(boolean isRed, long delaySeconds) {
int status = Robot.CONT;
Pose2d currPose = position.getOdometryPose();

if (firstTime == true) {
firstTime = false;
step = 1;

arm.closeClaw();
Robot.grabberState = GrabberStates.HOLDING_CUBE;
}

switch(step) {
case 1:
// Delay
status = autoDelay(delaySeconds);
drive.rotateWheels(-1, 0, 0);
break;
case 2:
arm.openClaw();
arm.startLaunch();

Robot.grabberState = GrabberStates.EMPTY;
balancedRoll = drive.getRoll();

status = autoDelayMS(300);
break;
case 3:
status = drive.chargeRamp(true);
arm.stopIntake();
break;
case 4:
status = drive.leaveRamp(true);
break;
case 5:
// Storing a pose 1.55 meter beyond ramp
if (isRed) {
rampAutoExitCommunity[0] = new Pose2d(currPose.getX() + 1, currPose.getY() + 0.75, new Rotation2d(0));
}
else {
rampAutoExitCommunity[0] = new Pose2d(currPose.getX() + 1, currPose.getY() - 0.75, new Rotation2d(0));
}
status = Robot.DONE;
break;
case 6:
// Exiting community
armToGrabPosition();
status = drive.autoDriveToPoints(rampAutoExitCommunity, currPose);
break;
case 7:
// Align with cone
arm.startIntake();
armToGrabPosition();
double angleError = nTables.getGamePieceX();
status = drive.alignWithPiece(angleError, false);

// Stops alignment after 1 second
int status2 = autoDelay(1);
if (status2 == Robot.DONE) {
status = Robot.DONE;
}
break;
case 8:
delayFirstTime = true;

// Drive to cone
arm.startIntake();
armToGrabPosition();
status = drive.driveToCone(1.0, controls.getLimitSwitch(), currPose.getTranslation());
break;
case 9:
// Pick up cone
arm.closeClaw();
arm.stopIntake();
Robot.grabberState = GrabberStates.HOLDING_CONE;
balancedRoll = drive.getRoll();
status = autoDelayMS(250);
break;
case 10:
AngleStates armStatus2 = armToRestPosition(false);
Robot.acceptedArmState = ArmStates.REST;
if (armStatus2 == AngleStates.DONE) {
arm.stopArm();
}

status = drive.chargeRamp(false);
break;
case 11:
armStatus2 = armToRestPosition(false);
if (armStatus2 == AngleStates.DONE) {
arm.stopArm();
}

// Balance on ramp
status = drive.balanceRamp(balancedRoll);
break;
case 12:
// Lock wheels
status = autoDelay(1);
drive.crossWheels();
break;
default:
// Finished routine
step = 1;
firstTime = true;

// Stops applicable motors
drive.stopWheels();
arm.stopArm();
return Robot.DONE;
}

//If we are done with a step, we go on to the next one and continue the routine
if (status == Robot.DONE) {
step++;
}

return Robot.CONT;
}

/**
* Places cube or cone, leaves community, and balances on ramp
Expand Down Expand Up @@ -450,50 +576,55 @@ public int centerAuto(boolean isRed, long delaySeconds) {
if (isRed == false) {
pose1 = new Pose2d(2.6, 7.401, new Rotation2d(Math.PI));
pose2 = new Pose2d(2.6, 7.401, new Rotation2d(0));
pose3 = new Pose2d(6.0, 7.401, new Rotation2d(0));
pose3 = new Pose2d(5.5, 7.401, new Rotation2d(0));
}
else {
pose1 = new Pose2d(2.6, 0.8, new Rotation2d(Math.PI));
pose2 = new Pose2d(2.6, 0.8, new Rotation2d(0));
pose3 = new Pose2d(6.0, 0.8, new Rotation2d(0));
pose3 = new Pose2d(5.5, 0.8, new Rotation2d(0));
}

status = drive.autoDriveToPoints(new Pose2d[]{pose2, pose3}, currPose);
resetArmRoutines();
break;
break;
case 6:
// Align with cube
status = armToGrabPosition();
break;
case 7:
// Align with object
arm.startIntake();
armToGrabPosition();
double angleError = nTables.getGamePieceX();
status = drive.alignWithPiece(angleError, false);
break;
case 7:
// Drive to cube
case 8:
// Drive to object
arm.startIntake();
armToGrabPosition();
status = drive.driveToCone(1.5, controls.getLimitSwitch(), currPose.getTranslation());
break;
case 8:
// Pick up cube
case 9:
// Pick up object
arm.closeClaw();
arm.stopIntake();
Robot.grabberState = GrabberStates.HOLDING_CUBE;
status = Robot.DONE;
Robot.grabberState = GrabberStates.HOLDING_CONE;
status = autoDelayMS(500);
break;
case 9:
case 10:
// Retract arm
AngleStates armStatus2 = armToRestPosition(false);
Robot.acceptedArmState = ArmStates.REST;
if (armStatus2 == AngleStates.DONE) {
status = Robot.DONE;
}
break;
case 10:
case 11:
// Storing current location
autoSecondPieceRotate[0] = new Pose2d(currPose.getX() - 0.5, currPose.getY(), new Rotation2d(Math.PI));
autoSecondPieceRotate[1] = new Pose2d(currPose.getX() - 3, currPose.getY(), new Rotation2d(Math.PI));
status = Robot.DONE;
break;
case 11:
case 12:
// Rotating
status = drive.autoDriveToPoints(autoSecondPieceRotate, currPose);
break;
Expand Down Expand Up @@ -931,6 +1062,27 @@ public int autoDelay(long seconds) {
return Robot.CONT;
}

/**
* Delays the program for a set number of milliseconds.
*
* @param seconds
* @return status
*/
public int autoDelayMS(long ms) {
long currentMS = System.currentTimeMillis();

if (delayFirstTime) {
delayEnd = currentMS + ms;
delayFirstTime = false;
}

if (currentMS > delayEnd) {
delayFirstTime = true;
return Robot.DONE;
}
return Robot.CONT;
}

public void resetArmRoutines() {
armFirstTime = true;
armStep = 1;
Expand Down
1 change: 0 additions & 1 deletion src/main/java/frc/robot/Controls.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,6 @@ public class Controls {

// Limit button on claw
private DigitalInput limitButton;
private boolean lastButtonPressed = false;

// Rate limiters
private SlewRateLimiter xLimiter;
Expand Down
2 changes: 0 additions & 2 deletions src/main/java/frc/robot/Drive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -485,13 +485,11 @@ public int chargeRamp(boolean frontEndFirst) {
default:
rampStep = 1;
rampFirstTime = true;
System.out.println("Returning done");
return Robot.DONE;
}

if (status == Robot.DONE) {
rampStep++;
System.out.println("Going to step " + rampStep);
}

return Robot.CONT;
Expand Down
Loading

0 comments on commit 2b54815

Please sign in to comment.