mirror of
https://github.com/TimeCrafters/FTC_2022
synced 2025-12-15 16:42:35 +00:00
Merge remote-tracking branch 'origin/master'
This commit is contained in:
Sodi
@@ -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;
|
||||
@@ -39,8 +34,8 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
|
||||
|
||||
// 4 Drive to the tall Pole (not all the way) while raising upper arm, this will be parallel
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "04-0"));
|
||||
addParallelStateToLastState(new TopArm(robot, "RightFourCone", "04-1"));
|
||||
|
||||
// addParallelStateToLastState(new TopArm(robot, "RightFourCone", "04-1"));
|
||||
addState(new TopArm(robot, "RightFourCone", "04-1"));
|
||||
// 6 Raise lower arm while slowly driving at the junction (parallel state)
|
||||
addState(new BottomArm(robot, "RightFourCone", "05-0"));
|
||||
|
||||
@@ -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"));
|
||||
|
||||
@@ -98,23 +96,23 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
|
||||
// 15 Drive All the way back to the tall Junction and raise upper arm (parallel state)
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0"));
|
||||
//
|
||||
// // 16 Rotate and use sensor to find junction
|
||||
// addState(new RotationState(robot, "RightFourCone", "16-0"));
|
||||
//
|
||||
// // 17 Drive Towards Junction (This is optional, idk if this is needed atm)
|
||||
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0"));
|
||||
// 16 Rotate and use sensor to find junction
|
||||
addState(new RotationState(robot, "RightFourCone", "16-0"));
|
||||
//
|
||||
// 17 Drive Towards Junction (This is optional, idk if this is needed atm)
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "17-0"));
|
||||
|
||||
// // 18 Bring upper arm down
|
||||
// addState(new TopArm(robot, "RightFourCone", "18-0"));
|
||||
addState(new TopArm(robot, "RightFourCone", "18-0"));
|
||||
//
|
||||
// // 19 Drop cone
|
||||
// addState(new CollectorState(robot, "RightFourCone", "19-0"));
|
||||
addState(new CollectorState(robot, "RightFourCone", "19-0"));
|
||||
//
|
||||
// // 20 Bring upper arm up
|
||||
// addState(new TopArm(robot, "RightFourCone", "20-0"));
|
||||
//
|
||||
// // 21 Drive away from Junction (this is optional and only used if we use the drive forward from earlier)
|
||||
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "21-0"));
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "21-0"));
|
||||
//
|
||||
// // 22 Drop the Upper arm to the position of the new top cone / 4th cone and check with sensor and start driving fast to get to the stack (this is a parallel state)
|
||||
// addState(new TopArm(robot, "RightFourCone", "22-0"));
|
||||
@@ -148,7 +146,7 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
|
||||
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "31-0"));
|
||||
//
|
||||
// // 32 Rotate towards Stack of cones
|
||||
// addState(new RotationState(robot, "RightFourCone", "32-0"));
|
||||
addState(new RotationState(robot, "RightFourCone", "32-0"));
|
||||
//
|
||||
// // 33 Decide which path after scanning image from earlier
|
||||
// addState(new PathDecision(robot, "RightFourCone", "33-0"));
|
||||
|
||||
@@ -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);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,5 +1,7 @@
|
||||
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;
|
||||
@@ -7,15 +9,34 @@ 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;
|
||||
private int traveledDistance;
|
||||
private int loopsCurrent = 0;
|
||||
private double minDistance;
|
||||
private double maxDistance;
|
||||
private String whereAmI = "init";
|
||||
private int driveLoops;
|
||||
private boolean finishedEnabled;
|
||||
|
||||
|
||||
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.traveledDistance = robot.configuration.variable(groupName, actionName, "traveled distance").value();
|
||||
this.minDistance = robot.configuration.variable(groupName, actionName, "minDistance").value();
|
||||
this.maxDistance = robot.configuration.variable(groupName, actionName, "maxDistance").value();
|
||||
this.finishedEnabled = robot.configuration.variable(groupName, actionName, "finishedEnabled").value();
|
||||
|
||||
|
||||
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
|
||||
}
|
||||
|
||||
@@ -23,133 +44,178 @@ 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);
|
||||
engine.telemetry.addData("traveled distance", traveledDistance);
|
||||
engine.telemetry.addData("Loops", loopsCurrent);
|
||||
engine.telemetry.addData("where am i??", whereAmI);
|
||||
engine.telemetry.addData("time", runTime() - checkTime);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@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);
|
||||
|
||||
checkTime = System.currentTimeMillis() - 250;
|
||||
|
||||
driveLoops = 0;
|
||||
|
||||
}
|
||||
|
||||
@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 > minDistance && rightDistance < maxDistance;
|
||||
boolean leftInRange = leftDistance > minDistance && leftDistance < maxDistance;
|
||||
|
||||
|
||||
// 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 (loopsCurrent >= loopsTotal){
|
||||
setHasFinished(finishedEnabled);
|
||||
} else if (System.currentTimeMillis() - checkTime >= 250 ) {
|
||||
|
||||
whereAmI = "post 250 ms";
|
||||
|
||||
checkTime = runTime();
|
||||
|
||||
loopsCurrent = loopsCurrent + 1;
|
||||
|
||||
if (rightInRange && leftInRange){
|
||||
setHasFinished(finishedEnabled);
|
||||
}
|
||||
|
||||
if (rightInRange && !leftInRange) {
|
||||
|
||||
// rotate 1 degree CW
|
||||
|
||||
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);
|
||||
|
||||
setHasFinished(finishedEnabled);
|
||||
|
||||
} 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);
|
||||
|
||||
}
|
||||
|
||||
// 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);
|
||||
setHasFinished(true);
|
||||
}
|
||||
|
||||
// whatToDo = (int)((leftDistance > TargetSensorDistance) << 1 ) + (int)(rightDistance > TargetSensorDistance);
|
||||
|
||||
switch (whatToDo){
|
||||
case 0: // drive back
|
||||
robot.frontLeftDrive.setPower(-drivePower);
|
||||
robot.frontRightDrive.setPower(-drivePower);
|
||||
robot.backLeftDrive.setPower(-drivePower);
|
||||
robot.backRightDrive.setPower(-drivePower);
|
||||
break;
|
||||
|
||||
case 1: // rotate CW
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
robot.frontRightDrive.setPower(-drivePower);
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(-drivePower);
|
||||
break;
|
||||
|
||||
case 2: // rotate CCW
|
||||
robot.frontLeftDrive.setPower(-drivePower);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
robot.backLeftDrive.setPower(-drivePower);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
break;
|
||||
|
||||
case 3: // Drive Forward
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
break;
|
||||
|
||||
|
||||
}
|
||||
// the robot is lined up but needs to drive forward till the robot is at the specified distance
|
||||
if (leftDistance > TargetSensorDistance && rightDistance > TargetSensorDistance){
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.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);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
// 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);
|
||||
|
||||
}
|
||||
|
||||
if (!rightInRange && !leftInRange && driveLoops < 2){
|
||||
|
||||
if (Math.abs(robot.OdometerEncoder.getCurrentPosition()) < traveledDistance) {
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
robot.frontLeftDrive.setPower(drivePower);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
|
||||
driveLoops += 1;
|
||||
}
|
||||
// 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.frontLeftDrive.setPower(0);
|
||||
robot.frontRightDrive.setPower(-drivePower);
|
||||
robot.backLeftDrive.setPower(0);
|
||||
robot.backRightDrive.setPower(-drivePower);
|
||||
}
|
||||
// The left sensor is aligned but the robot must rotate CCW with only the right side powered
|
||||
if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && rightDistance > TargetSensorDistance) {
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
robot.backLeftDrive.setPower(0);
|
||||
robot.backRightDrive.setPower(drivePower);
|
||||
|
||||
} else {
|
||||
|
||||
robot.backLeftDrive.setPower(0);
|
||||
robot.backRightDrive.setPower(0);
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
robot.frontRightDrive.setPower(0);
|
||||
|
||||
robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
|
||||
if (driveLoops == 2) {
|
||||
loopsCurrent = loopsTotal;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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());
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -73,8 +73,36 @@ public class PhoenixBot1 {
|
||||
setupRobot();
|
||||
}
|
||||
|
||||
private void initVuforia(){
|
||||
/*
|
||||
* Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
|
||||
*/
|
||||
int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
|
||||
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
|
||||
|
||||
parameters.vuforiaLicenseKey = VUFORIA_KEY;
|
||||
parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
|
||||
|
||||
|
||||
// Instantiate the Vuforia engine
|
||||
vuforia = ClassFactory.getInstance().createVuforia(parameters);
|
||||
}
|
||||
|
||||
private void initTfod() {
|
||||
int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
|
||||
"tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
|
||||
TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
|
||||
tfodParameters.minResultConfidence = 0.75f;
|
||||
tfodParameters.isModelTensorFlow2 = true;
|
||||
tfodParameters.inputSize = 300;
|
||||
tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
|
||||
tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABELS);
|
||||
}
|
||||
|
||||
private void setupRobot () {
|
||||
|
||||
|
||||
|
||||
collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance");
|
||||
downSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "downDistance");
|
||||
leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance");
|
||||
@@ -170,34 +198,10 @@ public class PhoenixBot1 {
|
||||
HighRiserLeft.setPosition(0.45);
|
||||
HighRiserRight.setPosition(0.45);
|
||||
|
||||
CameraServo.setPosition(0.8);
|
||||
CameraServo.setPosition(0.775);
|
||||
|
||||
}
|
||||
|
||||
private void initVuforia(){
|
||||
/*
|
||||
* Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
|
||||
*/
|
||||
int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
|
||||
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
|
||||
|
||||
parameters.vuforiaLicenseKey = VUFORIA_KEY;
|
||||
parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
|
||||
|
||||
|
||||
// Instantiate the Vuforia engine
|
||||
vuforia = ClassFactory.getInstance().createVuforia(parameters);
|
||||
}
|
||||
|
||||
private void initTfod() {
|
||||
int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
|
||||
"tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
|
||||
TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
|
||||
tfodParameters.minResultConfidence = 0.75f;
|
||||
tfodParameters.isModelTensorFlow2 = true;
|
||||
tfodParameters.inputSize = 300;
|
||||
tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
|
||||
tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABELS);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -20,8 +20,8 @@ public class PhoenixTeleOPState extends CyberarmState {
|
||||
private double RobotRotation;
|
||||
private double RotationTarget, DeltaRotation;
|
||||
private double MinimalPower = 0.25, topServoOffset = -0.05;
|
||||
private double servoCollectLow = 0.45; //Low servos, A button
|
||||
private double servoCollectHigh = 0.55; //High servos, A button
|
||||
private double servoCollectLow = 0.40; //Low servos, A button
|
||||
private double servoCollectHigh = 0.40; //High servos, A button
|
||||
private double servoLowLow = 0.5; //Low servos, X button
|
||||
private double servoLowHigh = 0.75; //High servos, X button
|
||||
private double servoMedLow = 0.5; //Low servos, B button
|
||||
|
||||
Reference in New Issue
Block a user