TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-13 06:22:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Autonomous work

Browse Source
This commit is contained in:
SpencerPiha
2023-02-07 20:33:52 -06:00
parent f2bd08a69a
commit 32556f9c1e
4 changed files with 140 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files

110
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java
View File

@@ -3,7 +3,9 @@ package org.timecrafters.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.DriverParkPlaceState;
import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
import org.timecrafters.Autonomous.States.PathDecision;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.TopArmv2;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
@@ -12,6 +14,13 @@ import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
public class RightStateAutoEngine extends CyberarmEngine {
PhoenixBot1 robot;
@Override
public void loop() {
super.loop();
telemetry.addData("BlackBoard Input", blackboardGetString("parkPlace"));
}
@Override
public void setup() {
robot = new PhoenixBot1(this);
@@ -24,26 +33,95 @@ public class RightStateAutoEngine extends CyberarmEngine {
PhoenixBot1.class,
"Right State Auto");
// addState(new TopArmv2(robot, "Right State Auto", "06-0"));
// // driving forward to low junction
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0"));
// // rotate left towards low junction
//// addState(new TopArmv2(robot, "Right State Auto", "06-0"));
//
//// // forward to low junction ..........................................................................(I have PreLoaded Cone)
//// addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0"));
//
// // rotate left towards low junction (angle = 45, direction = CCW) (I have PreLoaded Cone)
// addState(new RotationState(robot, "Right State Auto", "02-1"));
// // driving forward/ backwards to adjust
//
// // driving forward / backwards to adjust (I have PreLoaded Cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "03-0"));
// // drive forwards or backwards to adjust to pole
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "03-0"));
// // counteract distance driven
//
// // counteract distance driven .........................................................................(I'm going for 2nd cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "04-0"));
// // rotate towards opposing alliance
// addState(new RotationState(robot, "Right State Auto", "04-1"));
// // drive to stack column
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "05-0"));
// // rotate at stack
// addState(new RotationState(robot, "Right State Auto", "05-1"));
// // drive at stack
//
// // rotate towards opposing alliance (angle = 0, direction = CW) (I'm going for 2nd cone)
// addState(new RotationState(robot, "Right State Auto", "05-0"));
//
// // drive to tall junction (to adjust to cone stack) (I'm going for 2nd cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "06-0"));
//
// // rotate at stack (angle = -90, direction = CW) (I'm going for 2nd cone)
// addState(new RotationState(robot, "Right State Auto", "07-0"));
//
// // drive at stack (I'm going for 2nd cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "08-0"));
//
//
// // drive away from stack slightly....................................................................... (I have a 2nd cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "09-0"));
//
// //drive away to low (I have a 2nd cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "10-0"));
//
// // rotate at low junction (angle = -135, direction = CW) (I have a 2nd cone)
// addState(new RotationState(robot, "Right State Auto", "11-0"));
//
// // driving forward / backwards to adjust (I have a 2nd cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "12-0"));
//
// // counteract distance driven .......................................................................(I'm going for 3rd cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "13-0"));
//
// // rotate at stack (angle = -90, direction = CCW) (I'm going for 3rd cone)
// addState(new RotationState(robot, "Right State Auto", "14-0"));
//
// // drive at stack (I'm going for 3rd cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "15-0"));
//
// // drive away from stack slightly................................................................... (I have a 3rd and final cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "16-0"));
//
// //drive away to mid (I have a 3rd and final cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "17-0"));
//
// // rotate at mid junction (angle = -135, direction = CW) (I have a 3rd and final cone)
// addState(new RotationState(robot, "Right State Auto", "18-0"));
//
// // driving forward / backwards to adjust (I have a 3rd and final cone)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "19-0"));
//
// // counteract distance driven.............................................................................. (I'm parking)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "20-0"));
//
// // rotate at opposing alliance (angle = 0, direction = CCW) (I'm parking)
// addState(new RotationState(robot, "Right State Auto", "21-0"));
//
// // Drive back one tile (I'm parking)
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "22-0"));
//
// // rotate towards stack side (-90 CW) (I'm parking)
// addState(new RotationState(robot, "Right State Auto", "23-0"));
//
// // Choose Parking Spot (I'm parking)
// addState(new PathDecision(robot, "RightSideAutonomous", "24-0"));
//
// // case 1 drive forward (I'm parking)
// addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "24-1"));
//
// // case 2 drive forward (I'm parking)
// addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "24-2"));
//
// // case 3 drive forward (I'm parking)
// addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "24-3"));
//
// // rotate towards opposing alliance (angle = 0, direction = CCW) (I'm parking)
// addState(new RotationState(robot, "RightSideAutonomous", "25-0"));

92
TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java
View File

@@ -12,7 +12,6 @@ public class CollectorDistanceState extends CyberarmState {
private int traveledDistance;
private int RampUpDistance;
private int RampDownDistance;
private double drivePower;
private double targetDrivePower;
private double lastMeasuredTime;
private double currentDistance;
@@ -24,6 +23,7 @@ public class CollectorDistanceState extends CyberarmState {
private float collectTime;
private double inRangeTime;
private boolean stateDisabled;
private double distanceLimit;
public CollectorDistanceState(PhoenixBot1 robot, String groupName, String actionName) {
@@ -33,6 +33,7 @@ public class CollectorDistanceState extends CyberarmState {
this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
this.collectTime = robot.configuration.variable(groupName, actionName, "collectTime").value();
this.distanceLimit = robot.configuration.variable(groupName, actionName, "distanceLimit").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
@@ -44,21 +45,15 @@ public class CollectorDistanceState extends CyberarmState {
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackLeftDrive", robot.OdometerEncoderRight.getCurrentPosition());
engine.telemetry.addLine();
engine.telemetry.addData("traveledDistance", traveledDistance);
engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance);
engine.telemetry.addLine();
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);
engine.telemetry.addLine();
engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("Current Distance", currentDistance);
engine.telemetry.addData("Current Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM));
engine.telemetry.addData("last Distance", LastDistanceRead);
engine.telemetry.addLine();
@@ -75,17 +70,11 @@ public class CollectorDistanceState extends CyberarmState {
robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.OdometerEncoderRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.OdometerEncoderRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.OdometerEncoderLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
robot.collectorLeft.setPower(1);
@@ -102,7 +91,7 @@ public class CollectorDistanceState extends CyberarmState {
@Override
public void exec() {
if (stateDisabled){
if (stateDisabled) {
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
@@ -141,60 +130,41 @@ public class CollectorDistanceState extends CyberarmState {
}
}
if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 70) {
if (robot.collectorDistance.getDistance(DistanceUnit.MM) > distanceLimit) {
double delta = traveledDistance - Math.abs(robot.OdometerEncoderRight.getCurrentPosition());
if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) <= RampUpDistance) {
// ramping up
drivePower = (Math.abs((float) robot.OdometerEncoderRight.getCurrentPosition()) / RampUpDistance) + 0.15;
} else if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) >= delta) {
// ramping down
drivePower = ((delta / RampDownDistance) + 0.15);
} else {
// middle ground
drivePower = targetDrivePower;
}
robot.backLeftDrive.setPower(targetDrivePower);
robot.backRightDrive.setPower(targetDrivePower);
robot.frontLeftDrive.setPower(targetDrivePower);
robot.frontRightDrive.setPower(targetDrivePower);
if (Math.abs(drivePower) > Math.abs(targetDrivePower)) {
// This is limiting drive power to the targeted drive power
drivePower = targetDrivePower;
}
if (targetDrivePower < 0 && drivePower != targetDrivePower) {
drivePower = drivePower * -1;
}
if (Math.abs(robot.OdometerEncoderRight.getCurrentPosition()) < traveledDistance) {
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
}
} else {
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
if (!inRange){
inRange = true;
inRangeTime = runTime();
} else {
if (runTime() - inRangeTime >= collectTime){
robot.collectorRight.setPower(0);
robot.collectorLeft.setPower(0);
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
setHasFinished(true);
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
if (!inRange){
inRange = true;
inRangeTime = runTime();
} else {
if (runTime() - inRangeTime >= collectTime){
robot.collectorRight.setPower(0);
robot.collectorLeft.setPower(0);
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
setHasFinished(true);
}
}
}
}
}
}
}

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

@@ -15,7 +15,7 @@ public class DriverStateWithOdometer extends CyberarmState {
private float direction;
private boolean targetAchieved = false;
public final double WHEEL_CIRCUMFERENCE = 7.42108499;
public final int COUNTS_PER_REVOLUTION = 8192;
public final double COUNTS_PER_REVOLUTION = 8192;
public double startOfRampUpRight;
public double startOfRampDownRight;
public double startOfRampUpLeft;
@@ -40,8 +40,7 @@ public class DriverStateWithOdometer extends CyberarmState {
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
private double drivePower, targetDrivePower;
private int traveledDistance;
private double drivePower, targetDrivePower, traveledDistance;
@Override
public void start() {
@@ -62,24 +61,24 @@ public class DriverStateWithOdometer extends CyberarmState {
if (targetDrivePower > 0) {
startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition();
startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - 100;
endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + RampUpDistance;
startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance - RampDownDistance;
endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance;
startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition();
startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - 100;
endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + RampUpDistance;
startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance - RampDownDistance;
endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance;
} else {
startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition();
startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + 100;
endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - RampUpDistance;
startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance + RampDownDistance;
endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance;
startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition();
startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + 100;
endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - RampUpDistance;
startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance + RampDownDistance;
endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance;
@@ -255,7 +254,7 @@ public class DriverStateWithOdometer extends CyberarmState {
}
}
// .................................................................................................................................................Strafe Adjustment
// ...........................................................................................................................................Strafe Adjustment
if ( driveStage == 3 ){
currentHorizontalEncoder = robot.OdometerEncoderHorizontal.getCurrentPosition();
@@ -294,7 +293,10 @@ public class DriverStateWithOdometer extends CyberarmState {
@Override
public void telemetry () {
engine.telemetry.addData("Stage", driveStage);
engine.telemetry.addData("maximumTolerance", maximumTolerance);
engine.telemetry.addData("OdometerR", robot.OdometerEncoderRight.getCurrentPosition());
engine.telemetry.addData("OdometerL", robot.OdometerEncoderLeft.getCurrentPosition());
engine.telemetry.addData("OdometerH", robot.OdometerEncoderHorizontal.getCurrentPosition());
engine.telemetry.addData("startOfRampUpRight", startOfRampUpRight);
engine.telemetry.addData("endOfRampUpRight", endOfRampUpRight);
engine.telemetry.addData("startOfRampDownRight", startOfRampDownRight);
@@ -311,9 +313,7 @@ public class DriverStateWithOdometer extends CyberarmState {
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getPower());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getPower());
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getPower());
engine.telemetry.addData("OdometerR", robot.OdometerEncoderRight.getCurrentPosition());
engine.telemetry.addData("OdometerL", robot.OdometerEncoderLeft.getCurrentPosition());
engine.telemetry.addData("OdometerH", robot.OdometerEncoderHorizontal.getCurrentPosition());
engine.telemetry.addData("maximumTolerance", maximumTolerance);
engine.telemetry.addData("imu 1 angle", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
engine.telemetry.addData("Target Achieved", targetAchieved);

4
TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixBot1.java
View File

@@ -37,6 +37,7 @@ public class PhoenixBot1 {
public double ROTATION_TOLERANCE;
public long PAUSE_ON_ROTATION;
public double DISTANCE_MULTIPLIER;
public double CAMERA_INITiAL_POS;
// private static final String TFOD_MODEL_ASSET = "22-23_PowerPlay_Colors.tflite";
private static final String TFOD_MODEL_ASSET = "AprilTagsV1.tflite";
@@ -104,6 +105,7 @@ public class PhoenixBot1 {
PAUSE_ON_ROTATION = configuration.variable("Robot", "Tuning", "PAUSE_ON_ROTATION").value();
WHEEL_CIRCUMFERENCE = configuration.variable("Robot", "Tuning", "WHEEL_CIRCUMFERENCE").value();
DISTANCE_MULTIPLIER = configuration.variable("Robot", "Tuning", "DISTANCE_MULTIPLIER").value();
CAMERA_INITiAL_POS = configuration.variable("Robot", "Tuning", "CAMERA_INITiAL_POS").value();
}
private void initVuforia(){
@@ -247,7 +249,7 @@ public class PhoenixBot1 {
// HighRiserLeft.setPosition(0.40);
// HighRiserRight.setPosition(0.40);
CameraServo.setPosition(0.775);
CameraServo.setPosition(CAMERA_INITiAL_POS);