mirror of
https://github.com/TimeCrafters/FTC_2022
synced 2025-12-16 12:22:34 +00:00
autonomous work
This commit is contained in:
SpencerPiha
@@ -7,13 +7,8 @@ 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.DriverState;
|
||||
import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
|
||||
import org.timecrafters.Autonomous.States.DriverStateWithOdometerLowerArmParallelState;
|
||||
import org.timecrafters.Autonomous.States.DriverStateWithOdometerLowerArmParallelState2nd;
|
||||
import org.timecrafters.Autonomous.States.DriverStateWithOdometerUpperArmParallelState;
|
||||
import org.timecrafters.Autonomous.States.JunctionAllignmentState;
|
||||
import org.timecrafters.Autonomous.States.PathDecision;
|
||||
import org.timecrafters.Autonomous.States.RotationState;
|
||||
import org.timecrafters.Autonomous.States.ServoCameraRotate;
|
||||
import org.timecrafters.Autonomous.States.TopArm;
|
||||
@@ -50,6 +45,9 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
|
||||
// 6-1 drive forward or backwards if needed this is customizable so we can adapt
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "06-1"));
|
||||
|
||||
// 6-3 align to junction with rotation or skip if it looks like it won't be able to
|
||||
addState(new JunctionAllignmentState(robot, "RightFourCone", "06-3"));
|
||||
|
||||
//pause
|
||||
addState(new BottomArm(robot, "RightFourCone", "06-2"));
|
||||
|
||||
|
||||
@@ -115,7 +115,7 @@ public class CollectorDistanceState extends CyberarmState {
|
||||
|
||||
distanceDelta = LastDistanceRead - currentDistance;
|
||||
|
||||
if (distanceDelta >= -50.0 || currentDistance > 500) {
|
||||
if (distanceDelta >= -15.0 || currentDistance > 500) {
|
||||
// I am moving forward
|
||||
// and im close too my target.
|
||||
LastDistanceRead = currentDistance;
|
||||
|
||||
@@ -1,112 +0,0 @@
|
||||
package org.timecrafters.Autonomous.States;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.TeleOp.states.PhoenixBot1;
|
||||
|
||||
public class DriverStateWithOdometerLowerArmParallelState extends CyberarmState {
|
||||
private final boolean stateDisabled;
|
||||
PhoenixBot1 robot;
|
||||
private int RampUpDistance;
|
||||
private int RampDownDistance;
|
||||
private int maximumTolerance;
|
||||
public DriverStateWithOdometerLowerArmParallelState(PhoenixBot1 robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
|
||||
this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
|
||||
this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
|
||||
this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
|
||||
this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value();
|
||||
|
||||
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
|
||||
}
|
||||
private double drivePower, targetDrivePower;
|
||||
private int traveledDistance;
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
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.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);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
addParallelState(new BottomArm(robot, "RightFourCone", "06-1"));
|
||||
if (stateDisabled) {
|
||||
setHasFinished(true);
|
||||
return;
|
||||
}
|
||||
|
||||
double CurrentPosition = robot.OdometerEncoder.getCurrentPosition();
|
||||
double delta = traveledDistance - Math.abs(CurrentPosition);
|
||||
|
||||
if (Math.abs(CurrentPosition) <= RampUpDistance){
|
||||
// ramping up
|
||||
drivePower = (Math.abs((float)CurrentPosition) / RampUpDistance) + 0.25;
|
||||
}
|
||||
else if (Math.abs(CurrentPosition) >= delta){
|
||||
// ramping down
|
||||
drivePower = ((delta / RampDownDistance) + 0.25);
|
||||
} else {
|
||||
// middle ground
|
||||
drivePower = 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(CurrentPosition) < traveledDistance - maximumTolerance){
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
|
||||
}
|
||||
else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) {
|
||||
robot.backLeftDrive.setPower(targetDrivePower * -0.25);
|
||||
robot.backRightDrive.setPower(targetDrivePower * -0.25);
|
||||
robot.frontLeftDrive.setPower(targetDrivePower * -0.25);
|
||||
robot.frontRightDrive.setPower(targetDrivePower * -0.25);
|
||||
} else {
|
||||
robot.backLeftDrive.setPower(0);
|
||||
robot.backRightDrive.setPower(0);
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
robot.frontRightDrive.setPower(0);
|
||||
setHasFinished(true);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("Odometer", robot.OdometerEncoder.getCurrentPosition());
|
||||
|
||||
engine.telemetry.addData("drivePower", drivePower);
|
||||
engine.telemetry.addData("targetDrivePower", targetDrivePower);
|
||||
|
||||
engine.telemetry.addData("traveledDistance", traveledDistance);
|
||||
engine.telemetry.addData("RampUpDistance", RampUpDistance);
|
||||
engine.telemetry.addData("RampDownDistance", RampDownDistance);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,116 +0,0 @@
|
||||
package org.timecrafters.Autonomous.States;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.TeleOp.states.PhoenixBot1;
|
||||
|
||||
public class DriverStateWithOdometerLowerArmParallelState2nd extends CyberarmState {
|
||||
private final boolean stateDisabled;
|
||||
PhoenixBot1 robot;
|
||||
private int RampUpDistance;
|
||||
private int RampDownDistance;
|
||||
private int maximumTolerance;
|
||||
public DriverStateWithOdometerLowerArmParallelState2nd(PhoenixBot1 robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
|
||||
this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
|
||||
this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
|
||||
this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
|
||||
this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value();
|
||||
|
||||
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
|
||||
}
|
||||
private double drivePower, targetDrivePower;
|
||||
private int traveledDistance;
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
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.OdometerEncoder.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.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
if (stateDisabled) {
|
||||
setHasFinished(true);
|
||||
return;
|
||||
}
|
||||
|
||||
double CurrentPosition = robot.OdometerEncoder.getCurrentPosition();
|
||||
double delta = traveledDistance - Math.abs(CurrentPosition);
|
||||
|
||||
if (Math.abs(CurrentPosition) <= RampUpDistance){
|
||||
// ramping up
|
||||
drivePower = (Math.abs((float)CurrentPosition) / RampUpDistance) + 0.25;
|
||||
}
|
||||
else if (Math.abs(CurrentPosition) >= delta){
|
||||
// ramping down
|
||||
drivePower = ((delta / RampDownDistance) + 0.25);
|
||||
} else {
|
||||
// middle ground
|
||||
drivePower = 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(CurrentPosition) < traveledDistance - maximumTolerance){
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
|
||||
}
|
||||
else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) {
|
||||
robot.backLeftDrive.setPower(targetDrivePower * -0.25);
|
||||
robot.backRightDrive.setPower(targetDrivePower * -0.25);
|
||||
robot.frontLeftDrive.setPower(targetDrivePower * -0.25);
|
||||
robot.frontRightDrive.setPower(targetDrivePower * -0.25);
|
||||
} else {
|
||||
robot.backLeftDrive.setPower(0);
|
||||
robot.backRightDrive.setPower(0);
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
robot.frontRightDrive.setPower(0);
|
||||
setHasFinished(true);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("Odometer", robot.OdometerEncoder.getCurrentPosition());
|
||||
|
||||
engine.telemetry.addData("drivePower", drivePower);
|
||||
engine.telemetry.addData("targetDrivePower", targetDrivePower);
|
||||
|
||||
engine.telemetry.addData("traveledDistance", traveledDistance);
|
||||
engine.telemetry.addData("RampUpDistance", RampUpDistance);
|
||||
engine.telemetry.addData("RampDownDistance", RampDownDistance);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,144 +0,0 @@
|
||||
package org.timecrafters.Autonomous.States;
|
||||
|
||||
import android.util.Log;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.TeleOp.states.PhoenixBot1;
|
||||
|
||||
public class DriverStateWithOdometerUpperArmParallelState extends CyberarmState {
|
||||
private final boolean stateDisabled;
|
||||
PhoenixBot1 robot;
|
||||
private int RampUpDistance;
|
||||
private int RampDownDistance;
|
||||
private int maximumTolerance;
|
||||
public DriverStateWithOdometerUpperArmParallelState(PhoenixBot1 robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value();
|
||||
this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value();
|
||||
this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value();
|
||||
this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value();
|
||||
this.maximumTolerance = robot.configuration.variable(groupName, actionName, "maximumTolerance").value();
|
||||
|
||||
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
|
||||
}
|
||||
private double drivePower, targetDrivePower;
|
||||
private int traveledDistance;
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
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.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);
|
||||
|
||||
addParallelState(new TopArm(robot, "RightFourCone", "04-1"));
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
if (stateDisabled) {
|
||||
setHasFinished(true);
|
||||
return;
|
||||
}
|
||||
|
||||
double CurrentPosition = robot.OdometerEncoder.getCurrentPosition();
|
||||
double delta = traveledDistance - Math.abs(CurrentPosition);
|
||||
|
||||
if (Math.abs(CurrentPosition) <= RampUpDistance){
|
||||
// ramping up
|
||||
drivePower = (Math.abs((float)CurrentPosition) / RampUpDistance) + 0.25;
|
||||
}
|
||||
else if (Math.abs(CurrentPosition) >= delta){
|
||||
// ramping down
|
||||
drivePower = ((delta / RampDownDistance) + 0.25);
|
||||
} else {
|
||||
// middle ground
|
||||
drivePower = 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(CurrentPosition) < traveledDistance - maximumTolerance){
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
|
||||
}
|
||||
else if (Math.abs(CurrentPosition) > traveledDistance + maximumTolerance) {
|
||||
robot.backLeftDrive.setPower(targetDrivePower * -0.25);
|
||||
robot.backRightDrive.setPower(targetDrivePower * -0.25);
|
||||
robot.frontLeftDrive.setPower(targetDrivePower * -0.25);
|
||||
robot.frontRightDrive.setPower(targetDrivePower * -0.25);
|
||||
} else {
|
||||
robot.backLeftDrive.setPower(0);
|
||||
robot.backRightDrive.setPower(0);
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
robot.frontRightDrive.setPower(0);
|
||||
setHasFinished(true);
|
||||
}
|
||||
if (!getHasFinished()){
|
||||
float angle = robot.imu.getAngularOrientation().firstAngle;
|
||||
|
||||
if (angle < -0.25){
|
||||
robot.backLeftDrive.setPower(drivePower * 0.25);
|
||||
robot.backRightDrive.setPower(drivePower * 1.25);
|
||||
robot.frontLeftDrive.setPower(drivePower * 0.25);
|
||||
robot.frontRightDrive.setPower(drivePower * 1.25);
|
||||
}
|
||||
if (angle > 0.25){
|
||||
robot.backLeftDrive.setPower(drivePower * 1.25);
|
||||
robot.backRightDrive.setPower(drivePower * 0.25);
|
||||
robot.frontLeftDrive.setPower(drivePower * 1.25);
|
||||
robot.frontRightDrive.setPower(drivePower * 0.25);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition());
|
||||
engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getPower());
|
||||
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.OdometerEncoder.getCurrentPosition());
|
||||
engine.telemetry.addData("imu 1 angle", robot.imu.getAngularOrientation().firstAngle);
|
||||
engine.telemetry.addData("imu 2 angle", robot.imu.getAngularOrientation().secondAngle);
|
||||
engine.telemetry.addData("imu 3 angle", robot.imu.getAngularOrientation().thirdAngle);
|
||||
|
||||
|
||||
engine.telemetry.addData("drivePower", drivePower);
|
||||
engine.telemetry.addData("targetDrivePower", targetDrivePower);
|
||||
|
||||
engine.telemetry.addData("traveledDistance", traveledDistance);
|
||||
engine.telemetry.addData("RampUpDistance", RampUpDistance);
|
||||
engine.telemetry.addData("RampDownDistance", RampDownDistance);
|
||||
|
||||
Log.i("TELEMETRY", "imu 1 angle:: " + robot.imu.getAngularOrientation().firstAngle);
|
||||
Log.i("TELEMETRY", "imu 2 angle:: " + robot.imu.getAngularOrientation().secondAngle);
|
||||
Log.i("TELEMETRY", "imu 3 angle:: " + robot.imu.getAngularOrientation().thirdAngle);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -7,15 +7,23 @@ import org.timecrafters.TeleOp.states.PhoenixBot1;
|
||||
public class JunctionAllignmentState extends CyberarmState {
|
||||
private final boolean stateDisabled;
|
||||
PhoenixBot1 robot;
|
||||
private double TargetSensorDistance;
|
||||
private final String targetedJunction;
|
||||
private final double drivePower;
|
||||
private int whatToDo;
|
||||
private int loopsTotal;
|
||||
private float rotationAmount;
|
||||
private float currentAngle;
|
||||
private float targetAngle;
|
||||
private float slop;
|
||||
private double checkTime = 250;
|
||||
|
||||
|
||||
|
||||
public JunctionAllignmentState(PhoenixBot1 robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
|
||||
this.targetedJunction = robot.configuration.variable(groupName, actionName, "targetedJunction").value();
|
||||
this.loopsTotal = robot.configuration.variable(groupName, actionName, "loopsTotal").value();
|
||||
this.rotationAmount = robot.configuration.variable(groupName, actionName, "rotationAmount").value();
|
||||
this.slop = robot.configuration.variable(groupName, actionName, "slop").value();
|
||||
|
||||
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
|
||||
}
|
||||
|
||||
@@ -23,133 +31,132 @@ public class JunctionAllignmentState extends CyberarmState {
|
||||
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));
|
||||
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("target angle", targetAngle);
|
||||
engine.telemetry.addData("current angle", currentAngle);
|
||||
engine.telemetry.addData("drive power", drivePower);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
currentAngle = robot.imu.getAngularOrientation().firstAngle;
|
||||
|
||||
|
||||
if (stateDisabled){
|
||||
setHasFinished(true);
|
||||
} else {
|
||||
|
||||
double leftDistance = robot.leftPoleDistance.getDistance(DistanceUnit.MM);
|
||||
double rightDistance = robot.rightPoleDistance.getDistance(DistanceUnit.MM);
|
||||
boolean rightInRange = rightDistance > 170 && rightDistance < 200;
|
||||
boolean leftInRange = leftDistance > 170 && leftDistance < 200;
|
||||
|
||||
|
||||
// TODO: 12/11/2022 Make sure these are the correct values for the distance from low, mid, and high junctions!!!
|
||||
switch (targetedJunction) {
|
||||
case "low":
|
||||
TargetSensorDistance = 90.0;
|
||||
break;
|
||||
case "mid":
|
||||
TargetSensorDistance = 135.0;
|
||||
break;
|
||||
case "high":
|
||||
TargetSensorDistance = 200.0;
|
||||
break;
|
||||
}
|
||||
// The minimum Value that can be seen when in distance of the pole is 90.0 the maximum is 200.0
|
||||
|
||||
if (loopsTotal >= 5){
|
||||
setHasFinished(true);
|
||||
} else if (runTime() - checkTime >= 250 ) {
|
||||
|
||||
checkTime = runTime();
|
||||
loopsTotal = loopsTotal + 1;
|
||||
|
||||
|
||||
// the state is finished if the distance sensors are at the correct distance.
|
||||
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);
|
||||
} else {
|
||||
|
||||
if (rightInRange && leftInRange){
|
||||
setHasFinished(true);
|
||||
}
|
||||
|
||||
// whatToDo = (int)((leftDistance > TargetSensorDistance) << 1 ) + (int)(rightDistance > TargetSensorDistance);
|
||||
if (rightInRange && !leftInRange) {
|
||||
|
||||
switch (whatToDo){
|
||||
case 0: // drive back
|
||||
robot.frontLeftDrive.setPower(-drivePower);
|
||||
robot.frontRightDrive.setPower(-drivePower);
|
||||
robot.backLeftDrive.setPower(-drivePower);
|
||||
robot.backRightDrive.setPower(-drivePower);
|
||||
break;
|
||||
// rotate 1 degree CW
|
||||
|
||||
case 1: // rotate CW
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
robot.frontRightDrive.setPower(-drivePower);
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(-drivePower);
|
||||
break;
|
||||
targetAngle = currentAngle + rotationAmount;
|
||||
|
||||
case 2: // rotate CCW
|
||||
robot.frontLeftDrive.setPower(-drivePower);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
robot.backLeftDrive.setPower(-drivePower);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
break;
|
||||
if (targetAngle + slop < currentAngle && targetAngle - slop > currentAngle) {
|
||||
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && rightDistance < TargetSensorDistance) {
|
||||
robot.backLeftDrive.setPower(0);
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
|
||||
} else {
|
||||
|
||||
if (targetAngle - slop < currentAngle) {
|
||||
|
||||
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
|
||||
if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && rightDistance > TargetSensorDistance) {
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
|
||||
} else if (targetAngle + slop > currentAngle) {
|
||||
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
robot.backLeftDrive.setPower(0);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
robot.backLeftDrive.setPower(-drivePower);
|
||||
robot.frontLeftDrive.setPower(-drivePower);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
if (!rightInRange && leftInRange) {
|
||||
|
||||
// rotate 1 degree CCW
|
||||
|
||||
targetAngle = currentAngle - rotationAmount;
|
||||
|
||||
if (targetAngle + slop < currentAngle && targetAngle - slop > currentAngle) {
|
||||
|
||||
robot.frontRightDrive.setPower(0);
|
||||
robot.backRightDrive.setPower(0);
|
||||
robot.backLeftDrive.setPower(0);
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
|
||||
} else {
|
||||
|
||||
if (targetAngle - slop < currentAngle) {
|
||||
|
||||
robot.frontRightDrive.setPower(-drivePower);
|
||||
robot.backRightDrive.setPower(-drivePower);
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
|
||||
} else if (targetAngle + slop > currentAngle) {
|
||||
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
robot.backLeftDrive.setPower(-drivePower);
|
||||
robot.frontLeftDrive.setPower(-drivePower);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (!rightInRange && !leftInRange){
|
||||
|
||||
setHasFinished(true);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -81,6 +81,10 @@ public class RotationState extends CyberarmState {
|
||||
engine.telemetry.addData("Robot IMU Rotation", RobotRotation);
|
||||
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());
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user