TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-15 15:32:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Worked on alignment state and autonomous

Browse Source
This commit is contained in:
SpencerPiha
2022-12-20 20:32:47 -06:00
parent 1f40ac7026
commit 08c5d657f0
4 changed files with 141 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

170
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightFourConeAutonomousEngine.java
View File

@@ -49,93 +49,93 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
addState(new TopArm(robot, "RightFourCone", "07-0"));
// 8 Drop cone as soon as arm is in position
addState(new CollectorState(robot, "RightFourCone", "08-0"));
// addState(new CollectorState(robot, "RightFourCone", "08-0"));
// 9 Raise arm to clear junction
addState(new TopArm(robot, "RightFourCone", "09-0"));
// 10 Back up and bring lower arm down (parallel state)
addState(new DriverStateWithOdometerLowerArmParallelState2nd(robot, "RightFourCone", "10-0"));
// 11 Bring upper arm to the correct position for the top cone on stack (check with distance sensor)
addState(new TopArm(robot, "RightFourCone", "11-0"));
// 12 Rotate towards stack
//filled in as parallel state. in parallel with previous state
// 13 Drive at stack while collecting and check to see when we grab it
addState(new CollectorDistanceState(robot, "RightFourCone", "13-0"));
// 14 Back up and raise arm
addState(new DriverStateWithOdometer(robot, "RightFourCone", "14-0"));
addState(new TopArm(robot, "RightFourCone", "14-1"));
// 15 Drive All the way back to the medium Junction and raise upper arm (parallel state)
addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0"));
// 16 Rotate and use sensor to find junction
addState(new RotationState(robot, "RightFourCone", "16-0"));
addState(new JunctionAllignmentState(robot, "RightFourCone", "16-1"));
// 17 Drive Towards Junction (This is optional, idk if this is needed atm)
addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0"));
// 18 Bring upper arm down
addState(new TopArm(robot, "RightFourCone", "18-0"));
// 19 Drop cone
addState(new CollectorState(robot, "RightFourCone", "19-0"));
// 20 Bring upper arm up
addState(new TopArm(robot, "RightFourCone", "20-0"));
// 21 Drive away from Junction (this is optional and only used if we use the drive forward from earlier)
addState(new DriverStateWithOdometer(robot, "RightFourCone", "21-0"));
// 22 Drop the Upper arm to the position of the new top cone / 4th cone and check with sensor and start driving fast to get to the stack (this is a parallel state)
addState(new TopArm(robot, "RightFourCone", "22-0"));
// 23 Drive slower at the stack and run the collector to grab a 2nd cone off of the stack
addState(new CollectorDistanceState(robot, "RightFourCone", "23-0"));
// 24 Drive Back and lift up all the way to position for the low junction
addState(new DriverStateWithOdometer(robot, "RightFourCone", "24-0"));
addState(new TopArm(robot, "RightFourCone", "24-1"));
// 25 Drive back faster after the cone is fully off of the stack
addState(new DriverStateWithOdometer(robot, "RightFourCone", "25-0"));
// 26 Turn and look for the low junction with the distance sensor and align
addState(new RotationState(robot, "RightFourCone", "26-0"));
// 27 Drive forward / backwards if you need to. (check with distance sensor)
addState(new JunctionAllignmentState(robot, "RightFourCone", "26-1"));
// 28 Bring Upper arm down on junction
addState(new TopArm(robot, "RightFourCone", "28-0"));
// 29 Let go of cone right after arm is in position
addState(new CollectorState(robot, "RightFourCone", "29-0"));
// 30 Raise arm as soon as the cone is dropped
addState(new TopArm(robot, "RightFourCone", "30-0"));
// 31 Back up / go forward (optional, only needed if we drove forwards or backwards to align to low junction
addState(new DriverStateWithOdometer(robot, "RightFourCone", "31-0"));
// 32 Rotate towards Stack of cones
addState(new RotationState(robot, "RightFourCone", "32-0"));
// 33 Decide which path after scanning image from earlier
addState(new PathDecision(robot, "RightFourCone", "33-0"));
// 34 Drive backwards, forwards, or stay put
addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-1"));
addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-2"));
addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-3"));
// 35 Rotate towards alliance terminal
addState(new RotationState(robot, "RightFourCone", "35-0"));
// addState(new TopArm(robot, "RightFourCone", "09-0"));
//
// // 10 Back up and bring lower arm down (parallel state)
// addState(new DriverStateWithOdometerLowerArmParallelState2nd(robot, "RightFourCone", "10-0"));
//
// // 11 Bring upper arm to the correct position for the top cone on stack (check with distance sensor)
// addState(new TopArm(robot, "RightFourCone", "11-0"));
//
// // 12 Rotate towards stack
// //filled in as parallel state. in parallel with previous state
//
// // 13 Drive at stack while collecting and check to see when we grab it
// addState(new CollectorDistanceState(robot, "RightFourCone", "13-0"));
//
// // 14 Back up and raise arm
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "14-0"));
// addState(new TopArm(robot, "RightFourCone", "14-1"));
//
// // 15 Drive All the way back to the medium Junction and raise upper arm (parallel state)
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0"));
//
// // 16 Rotate and use sensor to find junction
// addState(new RotationState(robot, "RightFourCone", "16-0"));
// addState(new JunctionAllignmentState(robot, "RightFourCone", "16-1"));
//
// // 17 Drive Towards Junction (This is optional, idk if this is needed atm)
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0"));
//
// // 18 Bring upper arm down
// addState(new TopArm(robot, "RightFourCone", "18-0"));
//
// // 19 Drop cone
// addState(new CollectorState(robot, "RightFourCone", "19-0"));
//
// // 20 Bring upper arm up
// addState(new TopArm(robot, "RightFourCone", "20-0"));
//
// // 21 Drive away from Junction (this is optional and only used if we use the drive forward from earlier)
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "21-0"));
//
// // 22 Drop the Upper arm to the position of the new top cone / 4th cone and check with sensor and start driving fast to get to the stack (this is a parallel state)
// addState(new TopArm(robot, "RightFourCone", "22-0"));
//
// // 23 Drive slower at the stack and run the collector to grab a 2nd cone off of the stack
// addState(new CollectorDistanceState(robot, "RightFourCone", "23-0"));
//
// // 24 Drive Back and lift up all the way to position for the low junction
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "24-0"));
// addState(new TopArm(robot, "RightFourCone", "24-1"));
//
// // 25 Drive back faster after the cone is fully off of the stack
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "25-0"));
//
// // 26 Turn and look for the low junction with the distance sensor and align
// addState(new RotationState(robot, "RightFourCone", "26-0"));
//
// // 27 Drive forward / backwards if you need to. (check with distance sensor)
// addState(new JunctionAllignmentState(robot, "RightFourCone", "26-1"));
//
// // 28 Bring Upper arm down on junction
// addState(new TopArm(robot, "RightFourCone", "28-0"));
//
// // 29 Let go of cone right after arm is in position
// addState(new CollectorState(robot, "RightFourCone", "29-0"));
//
// // 30 Raise arm as soon as the cone is dropped
// addState(new TopArm(robot, "RightFourCone", "30-0"));
//
// // 31 Back up / go forward (optional, only needed if we drove forwards or backwards to align to low junction
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "31-0"));
//
// // 32 Rotate towards Stack of cones
// addState(new RotationState(robot, "RightFourCone", "32-0"));
//
// // 33 Decide which path after scanning image from earlier
// addState(new PathDecision(robot, "RightFourCone", "33-0"));
//
// // 34 Drive backwards, forwards, or stay put
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-1"));
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-2"));
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-3"));
//
// // 35 Rotate towards alliance terminal
// addState(new RotationState(robot, "RightFourCone", "35-0"));
}

2
TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java
View File

@@ -85,7 +85,7 @@ public class DriverStateWithOdometer extends CyberarmState {
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
// setHasFinished(true);
setHasFinished(true);
}

63
TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentState.java
View File

@@ -10,6 +10,7 @@ public class JunctionAllignmentState extends CyberarmState {
private double TargetSensorDistance;
private final String targetedJunction;
private final double drivePower;
private int whatToDo;
public JunctionAllignmentState(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot;
@@ -18,6 +19,14 @@ public class JunctionAllignmentState extends CyberarmState {
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
@Override
public void telemetry() {
engine.telemetry.addData("right sensor distance", robot.rightPoleDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("left sensor distance", robot.leftPoleDistance.getDistance(DistanceUnit.MM));
}
@Override
public void exec() {
@@ -41,65 +50,101 @@ public class JunctionAllignmentState extends CyberarmState {
TargetSensorDistance = 200.0;
break;
}
// the state is finished if the distance sensors are at the correct distance.
if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && (rightDistance > TargetSensorDistance -2 || rightDistance < TargetSensorDistance + 2)) {
if ((leftDistance > TargetSensorDistance - 2.0 || leftDistance < TargetSensorDistance + 2.0) && (rightDistance > TargetSensorDistance -2.0 || rightDistance < TargetSensorDistance + 2.0)) {
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
setHasFinished(true);
}
// whatToDo = (int)((leftDistance > TargetSensorDistance) << 1 ) + (int)(rightDistance > TargetSensorDistance);
switch (whatToDo){
case 0: // drive back
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(-drivePower);
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(-drivePower);
break;
case 1: // rotate CW
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
break;
case 2: // rotate CCW
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(drivePower);
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(drivePower);
break;
case 3: // Drive Forward
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
break;
}
// the robot is lined up but needs to drive forward till the robot is at the specified distance
else if (leftDistance > TargetSensorDistance && rightDistance > TargetSensorDistance){
if (leftDistance > TargetSensorDistance && rightDistance > TargetSensorDistance){
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
}
// the robot is lined up but needs to drive backward till the robot is at the specified distance
else if (leftDistance < TargetSensorDistance && rightDistance < TargetSensorDistance){
if (leftDistance < TargetSensorDistance && rightDistance < TargetSensorDistance){
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(-drivePower);
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(-drivePower);
}
// the robot is going to rotate CCW until a distance is met
else if (leftDistance > TargetSensorDistance && rightDistance < TargetSensorDistance) {
if (leftDistance > TargetSensorDistance && rightDistance < TargetSensorDistance) {
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(drivePower);
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(drivePower);
}
// the robot is going to rotate CW until a distance is met
else if (leftDistance < TargetSensorDistance && rightDistance > TargetSensorDistance) {
if (leftDistance < TargetSensorDistance && rightDistance > TargetSensorDistance) {
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
}
// The right sensor is aligned but the robot must rotate CW with only the left side powered
else if (leftDistance < TargetSensorDistance && (rightDistance > TargetSensorDistance -2 || rightDistance < TargetSensorDistance + 2)) {
if (leftDistance < TargetSensorDistance && (rightDistance > TargetSensorDistance -2 || rightDistance < TargetSensorDistance + 2)) {
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(0);
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(0);
}
// The right sensor is aligned but the robot must rotate rotate CCW with only the left side powered
else if (leftDistance > TargetSensorDistance && (rightDistance > TargetSensorDistance -2 || rightDistance < TargetSensorDistance + 2)) {
if (leftDistance > TargetSensorDistance && (rightDistance > TargetSensorDistance -2 || rightDistance < TargetSensorDistance + 2)) {
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(0);
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(0);
}
// The left sensor is aligned but the robot must rotate CW with only the right side powered
else if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && rightDistance < TargetSensorDistance) {
if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && rightDistance < TargetSensorDistance) {
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(-drivePower);
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(-drivePower);
}
// The left sensor is aligned but the robot must rotate CCW with only the right side powered
else if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && rightDistance > TargetSensorDistance) {
if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && rightDistance > TargetSensorDistance) {
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(drivePower);
robot.backLeftDrive.setPower(0);

2
TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArm.java
View File

@@ -53,7 +53,7 @@ public class TopArm extends CyberarmState {
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + AddedDistance);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + AddedDistance);
} else if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos && !up) {
} if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos && !up) {
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - AddedDistance);
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - AddedDistance);