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 for driving straight, And rotation

Browse Source
This commit is contained in:
SpencerPiha
2023-02-04 19:06:38 -06:00
parent 555788cdf9
commit 93abf54ee3
41 changed files with 391 additions and 244 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java
View File

@@ -14,7 +14,7 @@ import org.timecrafters.Autonomous.States.PathDecision;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
@Autonomous (name = "Left Side")
@Disabled

2
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java
View File

@@ -15,7 +15,7 @@ import org.timecrafters.Autonomous.States.PathDecision;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
@Autonomous (name = "left 2 cone auto")

2
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java
View File

@@ -13,7 +13,7 @@ import org.timecrafters.Autonomous.States.PathDecision;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
@Autonomous (name = "Right ")
@Disabled

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

@@ -3,18 +3,9 @@ package org.timecrafters.Autonomous.Engines;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.Autonomous.States.BottomArm;
import org.timecrafters.Autonomous.States.CollectorDistanceState;
import org.timecrafters.Autonomous.States.CollectorState;
import org.timecrafters.Autonomous.States.ConeIdentification;
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.ServoCameraRotate;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
@Autonomous(name = "Right Side")
public class RightStateAutoEngine extends CyberarmEngine {
@@ -23,22 +14,32 @@ public class RightStateAutoEngine extends CyberarmEngine {
@Override
public void setup() {
robot = new PhoenixBot1(this);
robot.imu.resetYaw();
// driving towards Low
// setupFromConfig(
// robot.configuration,
// "org.timecrafters.Autonomous.States",
// robot,
// PhoenixBot1.class,
// "Right State Auto");
// // driving forward to low junction
addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0"));
// rotate towards low
addState(new RotationState(robot, "Right State Auto", "02-1"));
// drive forwards or backwards to adjust to pole
addState(new DriverStateWithOdometer(robot, "Right State Auto", "03-0"));
// counteract distance driven
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 left towards low junction
// addState(new RotationState(robot, "Right State Auto", "02-1"));
// // driving forward/ backwards to adjust
// 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
// 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
// addState(new DriverStateWithOdometer(robot, "Right State Auto", "06-0"));

2
TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightTwoConeAutonomousEngine.java
View File

@@ -15,7 +15,7 @@ import org.timecrafters.Autonomous.States.PathDecision;
import org.timecrafters.Autonomous.States.RotationState;
import org.timecrafters.Autonomous.States.ServoCameraRotate;
import org.timecrafters.Autonomous.States.TopArm;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
@Autonomous (name = "Right 2 cone auto")

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

@@ -1,7 +1,7 @@
package org.timecrafters.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
public class BottomArm extends CyberarmState {

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

@@ -4,7 +4,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
public class CollectorDistanceState extends CyberarmState {

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

@@ -2,7 +2,7 @@ package org.timecrafters.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
public class CollectorState extends CyberarmState {

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

@@ -2,7 +2,7 @@ package org.timecrafters.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import java.util.List;

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

@@ -3,7 +3,7 @@ package org.timecrafters.Autonomous.States;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
public class DriverParkPlaceState extends CyberarmState {
private final boolean stateDisabled;

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

@@ -3,7 +3,7 @@ package org.timecrafters.Autonomous.States;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
public class DriverState extends CyberarmState {
private final boolean stateDisabled;

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

@@ -1,12 +1,10 @@
package org.timecrafters.Autonomous.States;
import android.util.Log;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
public class DriverStateWithOdometer extends CyberarmState {
private final boolean stateDisabled;
@@ -16,10 +14,20 @@ public class DriverStateWithOdometer extends CyberarmState {
private double maximumTolerance;
private float direction;
private boolean targetAchieved = false;
private double CurrentPosition;
public final double WHEEL_CIRCUMFERENCE = 7.42108499;
public final int COUNTS_PER_REVOLUTION = 8192;
public final double distanceMultiplier;
public double startOfRampUpRight;
public double startOfRampDownRight;
public double startOfRampUpLeft;
public double startOfRampDownLeft;
public double endOfRampUpRight;
public double endOfRampDownRight;
public double endOfRampUpLeft;
public double endOfRampDownLeft;
public int driveStage;
public float currentAngle;
public double currentHorizontalEncoder;
public DriverStateWithOdometer(PhoenixBot1 robot, String groupName, String actionName) {
this.robot = robot;
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
@@ -28,10 +36,10 @@ public class DriverStateWithOdometer extends CyberarmState {
this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value();
this.direction = robot.configuration.variable(groupName, actionName, "direction").value();
this.distanceMultiplier = robot.configuration.variable(groupName, actionName, "distanceMultiplier").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
}
private double drivePower, targetDrivePower;
private int traveledDistance;
@@ -44,143 +52,257 @@ public class DriverStateWithOdometer extends CyberarmState {
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.OdometerEncoderHorizontal.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
RampUpDistance = (int) ((RampUpDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
RampDownDistance = (int) ((RampDownDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * distanceMultiplier);
traveledDistance = (int) ((traveledDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
RampUpDistance = (int) ((RampUpDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
RampDownDistance = (int) ((RampDownDistance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
maximumTolerance = (int) ((maximumTolerance * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE)) * robot.DISTANCE_MULTIPLIER);
if (targetDrivePower > 0) {
startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition();
endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() + RampUpDistance;
startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance - RampDownDistance;
endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() + traveledDistance;
startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition();
endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() + RampUpDistance;
startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance - RampDownDistance;
endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() + traveledDistance;
} else {
startOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition();
endOfRampUpRight = robot.OdometerEncoderRight.getCurrentPosition() - RampUpDistance;
startOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance + RampDownDistance;
endOfRampDownRight = robot.OdometerEncoderRight.getCurrentPosition() - traveledDistance;
startOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition();
endOfRampUpLeft = robot.OdometerEncoderLeft.getCurrentPosition() - RampUpDistance;
startOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance + RampDownDistance;
endOfRampDownLeft = robot.OdometerEncoderLeft.getCurrentPosition() - traveledDistance;
}
driveStage = 0;
}
@Override
public void exec() {
if (stateDisabled) {
setHasFinished(true);
return;
}
double RightCurrentPosition = Math.abs(robot.OdometerEncoderRight.getCurrentPosition());
double LeftCurrentPosition = Math.abs(robot.OdometerEncoderLeft.getCurrentPosition());
double RightCurrentPosition = robot.OdometerEncoderRight.getCurrentPosition();
double LeftCurrentPosition = robot.OdometerEncoderLeft.getCurrentPosition();
if (RightCurrentPosition > LeftCurrentPosition) CurrentPosition = RightCurrentPosition;
if (RightCurrentPosition <= LeftCurrentPosition) CurrentPosition = LeftCurrentPosition;
// Driving Forward
if (targetDrivePower > 0 && driveStage == 0) {
if (Math.abs(CurrentPosition) <= RampUpDistance){
// ramping up
// double ratio = (Math.abs(CurrentPosition) / RampUpDistance);
if (targetDrivePower > 0) {
drivePower = (targetDrivePower - 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) + 0.25;
} else {
drivePower = (targetDrivePower + 0.25) * (Math.abs(CurrentPosition) / RampUpDistance) - 0.25;
}
}
else if (Math.abs(CurrentPosition) >= traveledDistance - RampDownDistance){
// ramping down
if (targetDrivePower > 0){
drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower - 0.25) + 0.25);
} else {
drivePower = ((((traveledDistance - Math.abs(CurrentPosition)) / RampDownDistance)) * (targetDrivePower + 0.25) - 0.25);
if ((RightCurrentPosition >= startOfRampUpRight && RightCurrentPosition <= endOfRampUpRight) ||
(LeftCurrentPosition >= startOfRampUpLeft && LeftCurrentPosition <= endOfRampUpLeft)) {
drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) *
(Math.abs(RightCurrentPosition - startOfRampUpRight) / RampUpDistance) + robot.DRIVETRAIN_MINIMUM_POWER;
}
} else {
// middle ground
drivePower = targetDrivePower;
}
// Driving Normal
else if ((RightCurrentPosition >= endOfRampUpRight && RightCurrentPosition <= startOfRampDownRight) ||
(LeftCurrentPosition >= endOfRampUpLeft && LeftCurrentPosition <= startOfRampDownLeft)) {
if (Math.abs(drivePower) > Math.abs(targetDrivePower)){
// This is limiting drive power to the targeted drive power
drivePower = targetDrivePower;
}
drivePower = targetDrivePower;
if (targetDrivePower < 0 && drivePower > 0) {
drivePower = drivePower * -1;
}
if (Math.abs(LeftCurrentPosition) < traveledDistance - maximumTolerance || Math.abs(RightCurrentPosition) < traveledDistance - maximumTolerance){
if (targetAchieved) {
drivePower = drivePower * 0.25;
if (Math.abs(drivePower) < 0.25){
if (drivePower < 0) {
drivePower = -0.25;
} else {
drivePower = 0.25;
}
}
}
robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
robot.frontRightDrive.setPower(drivePower);
}
else if (Math.abs(LeftCurrentPosition) > traveledDistance + maximumTolerance || Math.abs(RightCurrentPosition) > traveledDistance + maximumTolerance) {
targetAchieved = true;
drivePower = targetDrivePower * -0.25;
if (Math.abs(drivePower) < 0.25){
if (drivePower < 0) {
drivePower = -0.25;
} else {
drivePower = 0.25;
}
}
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(drivePower);
// Ramping down going forward
else if ((RightCurrentPosition >= startOfRampDownRight && RightCurrentPosition <= endOfRampDownRight) ||
(LeftCurrentPosition >= startOfRampDownLeft && LeftCurrentPosition <= endOfRampDownLeft)) {
drivePower = (targetDrivePower - robot.DRIVETRAIN_MINIMUM_POWER) *
(Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) + robot.DRIVETRAIN_MINIMUM_POWER;
}
else {
if (Math.abs(LeftCurrentPosition) > Math.abs(RightCurrentPosition)){
if (Math.abs(Math.abs(LeftCurrentPosition) - Math.abs(RightCurrentPosition)) < 20) {
drivePower = 0;
} else {
drivePower = 0.25;
robot.backLeftDrive.setPower(-drivePower);
robot.backRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(-drivePower);
robot.frontRightDrive.setPower(drivePower);
}
}
if (Math.abs(LeftCurrentPosition) < Math.abs(RightCurrentPosition)){
if (Math.abs(LeftCurrentPosition) == Math.abs(RightCurrentPosition)){
drivePower = 0;
} else {
drivePower = 0.25;
robot.backLeftDrive.setPower(drivePower);
robot.backRightDrive.setPower(-drivePower);
robot.frontLeftDrive.setPower(drivePower);
robot.frontRightDrive.setPower(-drivePower);
}
}
else {
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
} else if (driveStage == 0){
driveStage = 1;
robot.frontRightDrive.setPower(0);
setHasFinished(true);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
}
}
//
// Driving Backwards .................................................................................................................................Backwards
if (targetDrivePower < 0 && driveStage == 0) {
// ramping up
if ((RightCurrentPosition <= startOfRampUpRight && RightCurrentPosition >= endOfRampUpRight) ||
(LeftCurrentPosition <= startOfRampUpLeft && LeftCurrentPosition >= endOfRampUpLeft)) {
drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) *
(Math.abs(startOfRampUpRight - RightCurrentPosition) / RampUpDistance) - robot.DRIVETRAIN_MINIMUM_POWER;
}
// Driving Normal
else if ((RightCurrentPosition <= endOfRampUpRight && RightCurrentPosition >= startOfRampDownRight) ||
(LeftCurrentPosition <= endOfRampUpLeft && LeftCurrentPosition >= startOfRampDownLeft)) {
drivePower = targetDrivePower;
}
// Ramping down going backward
else if ((RightCurrentPosition <= startOfRampDownRight && RightCurrentPosition >= endOfRampDownRight) ||
(LeftCurrentPosition <= startOfRampDownLeft && LeftCurrentPosition >= endOfRampDownLeft)) {
drivePower = (targetDrivePower + robot.DRIVETRAIN_MINIMUM_POWER) *
(Math.abs( RightCurrentPosition - endOfRampDownRight) / RampDownDistance) - robot.DRIVETRAIN_MINIMUM_POWER;
} else if (driveStage == 0){
driveStage = 1;
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
}
// end of ramp down
}
// Forwards distance adjust...............................................................................................................................STAGE 1
if (driveStage == 1 && targetDrivePower > 0) {
if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) &&
RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) {
drivePower = robot.DRIVETRAIN_MINIMUM_POWER;
} else if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) &&
RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) {
drivePower = -robot.DRIVETRAIN_MINIMUM_POWER;
} else {
driveStage = 2;
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
}
}
// backwards distance adjust
if (driveStage == 1 && targetDrivePower < 0) {
if (LeftCurrentPosition > (endOfRampDownLeft + maximumTolerance) &&
RightCurrentPosition > (endOfRampDownRight + maximumTolerance)) {
drivePower = -robot.DRIVETRAIN_MINIMUM_POWER;
} else if (LeftCurrentPosition < (endOfRampDownLeft - maximumTolerance) &&
RightCurrentPosition < (endOfRampDownRight - maximumTolerance)) {
drivePower = robot.DRIVETRAIN_MINIMUM_POWER;
} else {
driveStage = 2;
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
}
}
if (driveStage == 0 || driveStage == 1) {
robot.frontRightDrive.setPower(drivePower);
robot.frontLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
robot.backRightDrive.setPower(drivePower);
robot.backLeftDrive.setPower(drivePower * robot.VEER_COMPENSATION_DBL);
}
// Heading adjustment
if (driveStage == 2 || driveStage == 4) {
currentAngle = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
if (currentAngle - direction > robot.ROTATION_TOLERANCE) {
robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER );
robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER );
robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER );
robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER );
}
else if (currentAngle - direction < -robot.ROTATION_TOLERANCE) {
robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
robot.frontLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
} else {
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
driveStage ++;
}
}
// .................................................................................................................................................Strafe Adjustment
if ( driveStage == 3 ){
currentHorizontalEncoder = robot.OdometerEncoderHorizontal.getCurrentPosition();
if (currentHorizontalEncoder > 200){
robot.frontRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
robot.frontLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER );
robot.backRightDrive.setPower(robot.STRAFE_MINIMUM_POWER );
robot.backLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
}
else if (currentHorizontalEncoder < -200){
robot.frontRightDrive.setPower(robot.STRAFE_MINIMUM_POWER );
robot.frontLeftDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
robot.backRightDrive.setPower(-robot.STRAFE_MINIMUM_POWER );
robot.backLeftDrive.setPower(robot.STRAFE_MINIMUM_POWER );
} else {
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
driveStage = 4;
}
}
if (driveStage == 5) {
// setHasFinished(true);
}
}
@Override
public void telemetry() {
public void telemetry () {
engine.telemetry.addData("Stage", driveStage);
engine.telemetry.addData("maximumTolerance", maximumTolerance);
engine.telemetry.addData("startOfRampUpRight", startOfRampUpRight);
engine.telemetry.addData("endOfRampUpRight", endOfRampUpRight);
engine.telemetry.addData("startOfRampDownRight", startOfRampDownRight);
engine.telemetry.addData("endOfRampDownRight", endOfRampDownRight);
engine.telemetry.addData("startOfRampUpLeft", startOfRampUpLeft);
engine.telemetry.addData("endOfRampUpLeft", endOfRampUpLeft);
engine.telemetry.addData("startOfRampDownLeft", startOfRampDownLeft);
engine.telemetry.addData("endOfRampDownLeft", endOfRampDownLeft);
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
@@ -189,15 +311,13 @@ 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("Odometer", robot.OdometerEncoderRight.getCurrentPosition());
engine.telemetry.addData("imu yaw", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
engine.telemetry.addData("imu pitch", robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES));
engine.telemetry.addData("imu roll", robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES));
engine.telemetry.addData("OdometerR", robot.OdometerEncoderRight.getCurrentPosition());
engine.telemetry.addData("OdometerL", robot.OdometerEncoderLeft.getCurrentPosition());
engine.telemetry.addData("OdometerH", robot.OdometerEncoderHorizontal.getCurrentPosition());
engine.telemetry.addData("imu 1 angle", robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
engine.telemetry.addData("Target Achieved", targetAchieved);
engine.telemetry.addData("drivePower", drivePower);
engine.telemetry.addData("targetDrivePower", targetDrivePower);
@@ -205,9 +325,6 @@ public class DriverStateWithOdometer extends CyberarmState {
engine.telemetry.addData("RampUpDistance", RampUpDistance);
engine.telemetry.addData("RampDownDistance", RampDownDistance);
Log.i("TELEMETRY", "imu yaw:: " + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
Log.i("TELEMETRY", "imu pitch:: " + robot.imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES));
Log.i("TELEMETRY", "imu roll:: " + robot.imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES));
}
}

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

@@ -5,7 +5,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
public class JunctionAllignmentAngleState extends CyberarmState {
private final boolean stateDisabled;

4
TeamCode/src/main/java/org/timecrafters/Autonomous/States/JunctionAllignmentDistanceState.java
View File

@@ -1,10 +1,8 @@
package org.timecrafters.Autonomous.States;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
public class JunctionAllignmentDistanceState extends CyberarmState {
private final boolean stateDisabled;

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

@@ -1,7 +1,7 @@
package org.timecrafters.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
public class PathDecision extends CyberarmState {
PhoenixBot1 robot;

113
TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java
View File

@@ -2,7 +2,7 @@ package org.timecrafters.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
public class RotationState extends CyberarmState {
private final boolean stateDisabled;
@@ -11,7 +11,7 @@ public class RotationState extends CyberarmState {
this.robot = robot;
this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value();
drivePowerVariable = drivePower;
this.ClockWiseRotation = robot.configuration.variable(groupName, actionName, "ClockWiseRotation").value();
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
@@ -19,69 +19,99 @@ public class RotationState extends CyberarmState {
private double drivePower;
private float targetRotation;
float RobotRotation;
private double RotationTarget;
private double RotationDirectionMinimum;
float CurrentRotation;
private String debugStatus = "?";
private double drivePowerVariable;
private double leftCompensator;
private double RightCompensator;
private boolean ClockWiseRotation;
private int RotationStage;
private double rotationDirection;
private long lastStepTime = 0;
@Override
public void start() {
leftCompensator = robot.OdometerEncoderLeft.getCurrentPosition();
RightCompensator = robot.OdometerEncoderRight.getCurrentPosition();
RotationStage = 0;
}
@Override
public void exec() {
if (stateDisabled){
if (stateDisabled) {
setHasFinished(true);
return;
} // end of if
} //
RobotRotation = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){
drivePowerVariable = 0.4 * drivePower;
if (Math.abs(drivePowerVariable) < 0.4) {
if (drivePowerVariable < 0){
drivePowerVariable = -0.4;
} else {
drivePowerVariable = 0.4;
}
CurrentRotation = (float) robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
if (RotationStage == 0) {
drivePowerVariable = ((Math.abs(CurrentRotation - targetRotation) / 90) * (drivePower - robot.ROTATION_MINIMUM_POWER)) + robot.ROTATION_MINIMUM_POWER;
if (ClockWiseRotation) { rotationDirection = 1;} else { rotationDirection = -1;}
robot.backLeftDrive.setPower( drivePowerVariable * rotationDirection );
robot.backRightDrive.setPower( -drivePowerVariable * rotationDirection );
robot.frontLeftDrive.setPower( drivePowerVariable * rotationDirection );
robot.frontRightDrive.setPower( -drivePowerVariable * rotationDirection );
if (Math.abs(Math.abs(CurrentRotation) - Math.abs(targetRotation)) <= robot.ROTATION_TOLERANCE &&
(RotationStage == 0) &&
(CurrentRotation - targetRotation <= robot.ROTATION_TOLERANCE)) {
RotationStage = 1;
lastStepTime = System.currentTimeMillis();
}
}
if (RotationStage == 1){
robot.backLeftDrive.setPower( 0 );
robot.backRightDrive.setPower( 0 );
robot.frontLeftDrive.setPower( 0 );
robot.frontRightDrive.setPower( 0 );
if (System.currentTimeMillis() - lastStepTime >= robot.PAUSE_ON_ROTATION ){
RotationStage = 2;
}
debugStatus = "Rotate Slow";
} // end of if
else {
drivePowerVariable = drivePower * 0.75;
debugStatus = "Rotate Fast";
}
if (RobotRotation >= targetRotation + 1){
drivePowerVariable = Math.abs(drivePowerVariable);
} else {
drivePowerVariable = -1 * Math.abs(drivePowerVariable);
if (RotationStage == 2) {
if (CurrentRotation - targetRotation > robot.ROTATION_TOLERANCE) {
// CW
robot.frontRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER );
robot.frontLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER );
robot.backRightDrive.setPower(-robot.ROTATION_MINIMUM_POWER );
robot.backLeftDrive.setPower(robot.ROTATION_MINIMUM_POWER );
}
else if (CurrentRotation - targetRotation < -robot.ROTATION_TOLERANCE) {
// CCW
robot.frontRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
robot.frontLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
robot.backRightDrive.setPower(robot.ROTATION_MINIMUM_POWER);
robot.backLeftDrive.setPower(-robot.ROTATION_MINIMUM_POWER);
} else {
robot.frontRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.backLeftDrive.setPower(0);
RotationStage ++;
// setHasFinished(true);
}
}
if (RobotRotation <= targetRotation -1 || RobotRotation >= targetRotation + 1) {
robot.backLeftDrive.setPower(drivePowerVariable);
robot.backRightDrive.setPower(-drivePowerVariable);
robot.frontLeftDrive.setPower(drivePowerVariable);
robot.frontRightDrive.setPower(-drivePowerVariable);
} else
{
robot.backLeftDrive.setPower(0);
robot.backRightDrive.setPower(0);
robot.frontLeftDrive.setPower(0);
robot.frontRightDrive.setPower(0);
PhoenixBot1.leftCompensatorGlobal = (leftCompensator + robot.OdometerEncoderLeft.getCurrentPosition()) - leftCompensator;
PhoenixBot1.RightCompensatorGlobal = (RightCompensator + robot.OdometerEncoderRight.getCurrentPosition()) - RightCompensator;
setHasFinished(true);
}
}
@Override
public void telemetry() {
@@ -89,13 +119,14 @@ public class RotationState extends CyberarmState {
engine.telemetry.addLine();
engine.telemetry.addData("Robot IMU Rotation", RobotRotation);
engine.telemetry.addData("Robot IMU Rotation", CurrentRotation);
engine.telemetry.addData("Robot Target Rotation", targetRotation);
engine.telemetry.addData("Drive Power", drivePowerVariable);
engine.telemetry.addData("front right power", robot.frontRightDrive.getPower());
engine.telemetry.addData("front left power", robot.frontLeftDrive.getPower());
engine.telemetry.addData("back left power", robot.backLeftDrive.getPower());
engine.telemetry.addData("back right power", robot.backRightDrive.getPower());
engine.telemetry.addData("RotationStage", RotationStage);
}
}

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

@@ -1,7 +1,7 @@
package org.timecrafters.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
public class ServoCameraRotate extends CyberarmState {
private final boolean stateDisabled;

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

@@ -1,7 +1,7 @@
package org.timecrafters.Autonomous.States;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
public class TopArm extends CyberarmState {

2
TeamCode/src/main/java/org/timecrafters/TeleOp/engine/CameraTestEngine.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/CameraTestEngine.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.TeleOp.engine;
package org.timecrafters.Autonomous.TeleOp.engine;
import org.cyberarm.engine.V2.CyberarmEngine;

4
TeamCode/src/main/java/org/timecrafters/TeleOp/engine/DriveDebugEngine.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/DriveDebugEngine.java
View File

@@ -1,7 +1,7 @@
package org.timecrafters.TeleOp.engine;
package org.timecrafters.Autonomous.TeleOp.engine;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
public class DriveDebugEngine extends CyberarmEngine {

2
TeamCode/src/main/java/org/timecrafters/TeleOp/engine/DynamicSetupEngine.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/DynamicSetupEngine.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.TeleOp.engine;
package org.timecrafters.Autonomous.TeleOp.engine;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

9
TeamCode/src/main/java/org/timecrafters/TeleOp/engine/PhoenixTeleOP.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/PhoenixTeleOP.java
View File

@@ -1,12 +1,11 @@
package org.timecrafters.TeleOp.engine;
package org.timecrafters.Autonomous.TeleOp.engine;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.TeleOp.states.PhoenixTeleOPState;
import org.timecrafters.TeleOp.states.TeleOPArmDriver;
import org.timecrafters.TeleOp.states.TeleOPTankDriver;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.TeleOPTankDriver;
import org.timecrafters.Autonomous.TeleOp.states.TeleOPArmDriver;
@TeleOp (name = "APhoenixTeleOP")

2
TeamCode/src/main/java/org/timecrafters/TeleOp/engine/PhoenixWingEngine.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/PhoenixWingEngine.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.TeleOp.engine;
package org.timecrafters.Autonomous.TeleOp.engine;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.hardware.CRServo;

4
TeamCode/src/main/java/org/timecrafters/TeleOp/engine/SodiEngine.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/SodiEngine.java
View File

@@ -1,10 +1,10 @@
package org.timecrafters.TeleOp.engine;
package org.timecrafters.Autonomous.TeleOp.engine;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.TeleOp.states.LaserState;
import org.timecrafters.Autonomous.TeleOp.states.LaserState;
@Disabled
@TeleOp(name = "Wheel")

6
TeamCode/src/main/java/org/timecrafters/TeleOp/engine/SpeedrunEngine.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/SpeedrunEngine.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.TeleOp.engine;
package org.timecrafters.Autonomous.TeleOp.engine;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -6,9 +6,9 @@ import com.qualcomm.robotcore.hardware.Gamepad;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.cyberarm.engine.V2.GamepadChecker;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.TeleOp.states.TeleOPSpeedrunState;
import org.timecrafters.Autonomous.TeleOp.states.TeleOPSpeedrunState;
@Disabled
@TeleOp (name = "Speedrun Engine")

7
TeamCode/src/main/java/org/timecrafters/TeleOp/engine/SterringDriveEngine.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/engine/SterringDriveEngine.java
View File

@@ -1,12 +1,11 @@
package org.timecrafters.TeleOp.engine;
package org.timecrafters.Autonomous.TeleOp.engine;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.TeleOp.states.LaserState;
import org.timecrafters.TeleOp.states.PhoenixBot1;
import org.timecrafters.TeleOp.states.SteeringDriveExperiment;
import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1;
import org.timecrafters.Autonomous.TeleOp.states.SteeringDriveExperiment;
@Disabled
@TeleOp(name = "Steering Drive Test")

2
TeamCode/src/main/java/org/timecrafters/TeleOp/states/CameraTestCommon.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/CameraTestCommon.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.TeleOp.states;
package org.timecrafters.Autonomous.TeleOp.states;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;

2
TeamCode/src/main/java/org/timecrafters/TeleOp/states/CameraTestState.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/CameraTestState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.TeleOp.states;
package org.timecrafters.Autonomous.TeleOp.states;
import org.cyberarm.engine.V2.CyberarmState;

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

@@ -1,7 +1,7 @@
package org.timecrafters.TeleOp.states;
package org.timecrafters.Autonomous.TeleOp.states;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.TeleOp.engine.DynamicSetupEngine;
import org.timecrafters.Autonomous.TeleOp.engine.DynamicSetupEngine;
public class DynamicSetupState extends CyberarmState {
private long delay;

2
TeamCode/src/main/java/org/timecrafters/TeleOp/states/LaserServoHeight.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/LaserServoHeight.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.TeleOp.states;
package org.timecrafters.Autonomous.TeleOp.states;
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.robotcore.hardware.Servo;

2
TeamCode/src/main/java/org/timecrafters/TeleOp/states/LaserState.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/LaserState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.TeleOp.states;
package org.timecrafters.Autonomous.TeleOp.states;
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;

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

@@ -1,4 +1,4 @@
package org.timecrafters.TeleOp.states;
package org.timecrafters.Autonomous.TeleOp.states;
import com.qualcomm.hardware.adafruit.AdafruitI2cColorSensor;
import com.qualcomm.hardware.bosch.BNO055IMU;
@@ -25,7 +25,7 @@ import org.timecrafters.minibots.cyberarm.chiron.states.autonomous.Arm;
public class PhoenixBot1 {
private static final float mmPerInch = 25.4f;
public static final double WHEEL_CIRCUMFERENCE = 7.42108499;
public static double WHEEL_CIRCUMFERENCE;
public static final int COUNTS_PER_REVOLUTION = 8192;
public static double leftCompensatorGlobal;
public static double RightCompensatorGlobal;
@@ -35,6 +35,8 @@ public class PhoenixBot1 {
public double STRAFE_MINIMUM_POWER;
public double DRIVE_TOLERANCE;
public double ROTATION_TOLERANCE;
public long PAUSE_ON_ROTATION;
public double DISTANCE_MULTIPLIER;
// private static final String TFOD_MODEL_ASSET = "22-23_PowerPlay_Colors.tflite";
private static final String TFOD_MODEL_ASSET = "AprilTagsV1.tflite";
@@ -99,6 +101,9 @@ public class PhoenixBot1 {
STRAFE_MINIMUM_POWER = configuration.variable("Robot", "Tuning", "STRAFE_MINIMUM_POWER").value();
DRIVE_TOLERANCE = configuration.variable("Robot", "Tuning", "DRIVE_TOLERANCE").value();
ROTATION_TOLERANCE = configuration.variable("Robot", "Tuning", "ROTATION_TOLERANCE").value();
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();
}
private void initVuforia(){
@@ -244,6 +249,8 @@ public class PhoenixBot1 {
CameraServo.setPosition(0.775);
}
public int AngleToTicks(double angle) {

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

@@ -1,9 +1,7 @@
package org.timecrafters.TeleOp.states;
package org.timecrafters.Autonomous.TeleOp.states;
import android.annotation.SuppressLint;
import android.widget.Switch;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.Servo;

2
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPv2.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixTeleOPv2.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.TeleOp.states;
package org.timecrafters.Autonomous.TeleOp.states;
import org.cyberarm.engine.V2.CyberarmState;

2
TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixWingState.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/PhoenixWingState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.TeleOp.states;
package org.timecrafters.Autonomous.TeleOp.states;
import org.cyberarm.engine.V2.CyberarmState;

2
TeamCode/src/main/java/org/timecrafters/TeleOp/states/SodiLEDState.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/SodiLEDState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.TeleOp.states;
package org.timecrafters.Autonomous.TeleOp.states;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;

2
TeamCode/src/main/java/org/timecrafters/TeleOp/states/SodiState.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/SodiState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.TeleOp.states;
package org.timecrafters.Autonomous.TeleOp.states;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;

2
TeamCode/src/main/java/org/timecrafters/TeleOp/states/SteeringDriveExperiment.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/SteeringDriveExperiment.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.TeleOp.states;
package org.timecrafters.Autonomous.TeleOp.states;
import org.cyberarm.engine.V2.CyberarmState;

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

@@ -1,13 +1,11 @@
package org.timecrafters.TeleOp.states;
package org.timecrafters.Autonomous.TeleOp.states;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.Servo;
import org.cyberarm.engine.V2.CyberarmState;
import org.cyberarm.engine.V2.GamepadChecker;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
public class TeleOPArmDriver extends CyberarmState {
private final PhoenixBot1 robot;

2
TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPSpeedrunState.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPSpeedrunState.java
View File

@@ -1,4 +1,4 @@
package org.timecrafters.TeleOp.states;
package org.timecrafters.Autonomous.TeleOp.states;
import com.qualcomm.robotcore.hardware.DcMotor;

3
TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java → TeamCode/src/main/java/org/timecrafters/Autonomous/TeleOp/states/TeleOPTankDriver.java
View File

@@ -1,6 +1,5 @@
package org.timecrafters.TeleOp.states;
package org.timecrafters.Autonomous.TeleOp.states;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.cyberarm.engine.V2.CyberarmState;