Skip to content

Commit

Permalink
Wall and center auto
Browse files Browse the repository at this point in the history
2199robolions committed Apr 16, 2023
1 parent f8147e7 commit 44a5fed
Showing 5 changed files with 80 additions and 63 deletions.
96 changes: 46 additions & 50 deletions src/main/java/frc/robot/Auto.java
Original file line number Diff line number Diff line change
@@ -42,7 +42,7 @@ public class Auto {

// Variables
private Pose2d[] rampAutoExitCommunity = new Pose2d[1]; // Stores location that will ensure we leave community
private Pose2d[] autoSecondPieceRotate = new Pose2d[1];
private Pose2d[] autoSecondPieceRotate = new Pose2d[2];
private double balancedRoll = 0;

// Coordinates to be used in routines
@@ -133,52 +133,50 @@ 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.3, 7.101, new Rotation2d(0));
pose3 = new Pose2d(6.0, 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.3, 1.0, new Rotation2d(0));
pose3 = new Pose2d(6.0, 0.8, new Rotation2d(0));
}

status = drive.autoDriveToPoints(new Pose2d[]{pose2, pose3}, currPose);
resetArmRoutines();
break;
break;
case 6:
// Bring down wrist
status = armToGrabPosition();
Robot.acceptedArmState = ArmStates.GRAB;
Controls.armState = ArmStates.GRAB;
break;
case 7:
// Align with cone
// Align with cube
arm.startIntake();
double angleError = nTables.getGamePieceX();
status = drive.alignWithPiece(angleError, false);
break;
case 8:
// Drive to cone
status = drive.driveToCone(1.0, controls.getLimitSwitch(), currPose.getTranslation());
case 7:
// Drive to cube
arm.startIntake();
status = drive.driveToCone(1.5, controls.getLimitSwitch(), currPose.getTranslation());
break;
case 9:
// Pick up cone
case 8:
// Pick up cube
arm.closeClaw();
Robot.grabberState = GrabberStates.HOLDING_CONE;
arm.stopIntake();
Robot.grabberState = GrabberStates.HOLDING_CUBE;
status = Robot.DONE;
break;
case 10:
case 9:
// Retract arm
AngleStates armStatus2 = armToRestPosition(false);
Robot.acceptedArmState = ArmStates.REST;
if (armStatus2 == AngleStates.DONE) {
status = Robot.DONE;
}
break;
case 11:
case 10:
// Storing current location
autoSecondPieceRotate[0] = new Pose2d(currPose.getX(), currPose.getY(), new Rotation2d(Math.PI));
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 12:
case 11:
// Rotating
status = drive.autoDriveToPoints(autoSecondPieceRotate, currPose);
break;
@@ -412,24 +410,24 @@ public int centerAuto(boolean isRed, long delaySeconds) {
// Delay
status = autoDelay(delaySeconds);

if (isRed == true) {
drive.rotateWheels(0.833, -1.0256, 0);
if (isRed == false) {
drive.rotateWheels(0.833, 1.0256, 0);
}
else {
drive.rotateWheels(0.833, 1.0256, 0);
}
drive.rotateWheels(0.833, -1.0256, 0);
}
break;
case 2:
// Place object we're holding
status = armToTopCone();
Robot.acceptedArmState = ArmStates.TOP_CONE;

if (isRed == true) {
drive.rotateWheels(0.833, -1.0256, 0);
}
else {
if (isRed == false) {
drive.rotateWheels(0.833, 1.0256, 0);
}
else {
drive.rotateWheels(0.833, -1.0256, 0);
}
break;
case 3:
arm.openClaw();
@@ -438,10 +436,10 @@ public int centerAuto(boolean isRed, long delaySeconds) {
break;
case 4:
AngleStates armStatus = armToRestPosition(true);
Robot.acceptedArmState = ArmStates.REST;
if (armStatus == AngleStates.DONE || armStatus == AngleStates.CLOSE) {
status = Robot.DONE;
}
Robot.acceptedArmState = ArmStates.REST;
break;
case 5:
// Approach 1st object
@@ -452,52 +450,50 @@ 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.3, 7.151, new Rotation2d(0));
pose3 = new Pose2d(6.0, 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.3, 1.05, new Rotation2d(0));
pose3 = new Pose2d(6.0, 0.8, new Rotation2d(0));
}

status = drive.autoDriveToPoints(new Pose2d[]{pose2, pose3}, currPose);
resetArmRoutines();
break;
break;
case 6:
// Bring down wrist
status = armToGrabPosition();
Robot.acceptedArmState = ArmStates.GRAB;
Controls.armState = ArmStates.GRAB;
break;
case 7:
// Align with cone
// Align with cube
arm.startIntake();
double angleError = nTables.getGamePieceX();
status = drive.alignWithPiece(angleError, false);
break;
case 8:
// Drive to cone
status = drive.driveToCone(1.0, controls.getLimitSwitch(), currPose.getTranslation());
case 7:
// Drive to cube
arm.startIntake();
status = drive.driveToCone(1.5, controls.getLimitSwitch(), currPose.getTranslation());
break;
case 9:
// Pick up cone
case 8:
// Pick up cube
arm.closeClaw();
Robot.grabberState = GrabberStates.HOLDING_CONE;
arm.stopIntake();
Robot.grabberState = GrabberStates.HOLDING_CUBE;
status = Robot.DONE;
break;
case 10:
case 9:
// Retract arm
AngleStates armStatus2 = armToRestPosition(false);
Robot.acceptedArmState = ArmStates.REST;
if (armStatus2 == AngleStates.DONE) {
status = Robot.DONE;
}
break;
case 11:
case 10:
// Storing current location
autoSecondPieceRotate[0] = new Pose2d(currPose.getX(), currPose.getY(), new Rotation2d(Math.PI));
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 12:
case 11:
// Rotating
status = drive.autoDriveToPoints(autoSecondPieceRotate, currPose);
break;
4 changes: 1 addition & 3 deletions src/main/java/frc/robot/Controls.java
Original file line number Diff line number Diff line change
@@ -7,8 +7,6 @@
import edu.wpi.first.wpilibj.XboxController;
import frc.robot.Robot.GrabberStates;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DigitalInput;

/**
@@ -196,7 +194,7 @@ public boolean enablePrecisionDrive() {
*
******************************************************************************************/
public boolean getLeftBumper() {
return armController.getLeftBumper();
return armController.getLeftBumper() || allignWithPiece();
}

public boolean getRightBumper() {
26 changes: 24 additions & 2 deletions src/main/java/frc/robot/CustomTables.java
Original file line number Diff line number Diff line change
@@ -28,6 +28,8 @@ public class CustomTables {
private NetworkTable PieceData;
private NetworkTableEntry width;
private NetworkTableEntry centerX;
private NetworkTableEntry numCones;
private NetworkTableEntry numCubes;

// Singleton for CustomTables to ensure only one NetworkTables server is created
private static CustomTables instance = null;
@@ -60,8 +62,10 @@ private CustomTables() {

// Creates the PieceData table and its entries
PieceData = ntInst .getTable("PieceData");
width = PieceData.getEntry("Width"); // double
centerX = PieceData.getEntry("CenterX"); // double
width = PieceData.getEntry("Width"); // double
centerX = PieceData.getEntry("CenterX"); // double
numCones = PieceData.getEntry("NumCones"); // double
numCubes = PieceData.getEntry("NumCubes"); // double
}

/******************************************************************************************
@@ -166,6 +170,24 @@ public double getCamWidth() {
public double getGamePieceX() {
return centerX.getDouble(0);
}

/**
* Determines the number of cones in the stream.
*
* @return numCones
*/
public double getNumCones() {
return numCones.getDouble(0);
}

/**
* Determines the number of cubes in the stream.
*
* @return numCubes
*/
public double getNumCubes() {
return numCubes.getDouble(0);
}
}

// End of the CustomTables class
13 changes: 6 additions & 7 deletions src/main/java/frc/robot/Drive.java
Original file line number Diff line number Diff line change
@@ -76,8 +76,8 @@ public class Drive {
PIDController autoDriveRotateController;

// OpenCV rotate controller
private static final double ocvp = 0.05; //0.025
private static final double ocvi = 0.01; //0.01
private static final double ocvp = 0.035; //0.025
private static final double ocvi = 0.00; //0.01
private static final double ocvd = 0;
PIDController openCVController;

@@ -175,7 +175,6 @@ public Drive() {
openCVController = new PIDController(ocvp, ocvi, ocvd);
openCVController.setTolerance(3);
openCVController.enableContinuousInput(180, -180);
openCVController.setIntegratorRange(-0.25, 0.25);

rampBalanceController = new PIDController(rbP, rbI, rbD);
rampBalanceController.setTolerance(RAMP_BALANCE_TOLERANCE);
@@ -352,12 +351,12 @@ public int alignWithPiece(double centerX, boolean teleop) {
openCVController.setSetpoint(0);

// Cone-based drive
double radians = Math.toRadians(angleError);
double xPower = 0.80 * Math.cos(radians);
double yPower = 0.80 * Math.sin(radians);
double xPower = 0.80;
double yPower = speed;

// Drives the robot
if (teleop == true) {
teleopDrive(xPower, yPower, speed, false);
teleopDrive(xPower, yPower, 0, false);
}
else {
teleopDrive(0, 0, speed, false);
4 changes: 3 additions & 1 deletion src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
@@ -332,7 +332,9 @@ public void testPeriodic() {
*/
private void wheelControl() {
// Variables
double centerX = nTables.getGamePieceX();
double centerX = nTables.getGamePieceX();
double numCones = nTables.getNumCones();
double numCubes = nTables.getNumCubes();
boolean switchPressed = controls.getLimitSwitch();
boolean recentAprilTag = position.recentAprilTag(); // Check in pose estimation if we read April Tag within last 5 seconds

0 comments on commit 44a5fed

Please sign in to comment.