mirror of
https://github.com/TimeCrafters/FTC_2022
synced 2025-12-16 19:22:34 +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.CollectorDistanceState;
|
||||||
import org.timecrafters.Autonomous.States.CollectorState;
|
import org.timecrafters.Autonomous.States.CollectorState;
|
||||||
import org.timecrafters.Autonomous.States.ConeIdentification;
|
import org.timecrafters.Autonomous.States.ConeIdentification;
|
||||||
import org.timecrafters.Autonomous.States.DriverState;
|
|
||||||
import org.timecrafters.Autonomous.States.DriverStateWithOdometer;
|
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.JunctionAllignmentState;
|
||||||
import org.timecrafters.Autonomous.States.PathDecision;
|
|
||||||
import org.timecrafters.Autonomous.States.RotationState;
|
import org.timecrafters.Autonomous.States.RotationState;
|
||||||
import org.timecrafters.Autonomous.States.ServoCameraRotate;
|
import org.timecrafters.Autonomous.States.ServoCameraRotate;
|
||||||
import org.timecrafters.Autonomous.States.TopArm;
|
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
|
// 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"));
|
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)
|
// 6 Raise lower arm while slowly driving at the junction (parallel state)
|
||||||
addState(new BottomArm(robot, "RightFourCone", "05-0"));
|
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
|
// 6-1 drive forward or backwards if needed this is customizable so we can adapt
|
||||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "06-1"));
|
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
|
//pause
|
||||||
addState(new BottomArm(robot, "RightFourCone", "06-2"));
|
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)
|
// 15 Drive All the way back to the tall Junction and raise upper arm (parallel state)
|
||||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0"));
|
addState(new DriverStateWithOdometer(robot, "RightFourCone", "15-0"));
|
||||||
//
|
//
|
||||||
// // 16 Rotate and use sensor to find junction
|
// 16 Rotate and use sensor to find junction
|
||||||
// addState(new RotationState(robot, "RightFourCone", "16-0"));
|
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"));
|
|
||||||
//
|
//
|
||||||
|
// 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
|
// // 18 Bring upper arm down
|
||||||
// addState(new TopArm(robot, "RightFourCone", "18-0"));
|
addState(new TopArm(robot, "RightFourCone", "18-0"));
|
||||||
//
|
//
|
||||||
// // 19 Drop cone
|
// // 19 Drop cone
|
||||||
// addState(new CollectorState(robot, "RightFourCone", "19-0"));
|
addState(new CollectorState(robot, "RightFourCone", "19-0"));
|
||||||
//
|
//
|
||||||
// // 20 Bring upper arm up
|
// // 20 Bring upper arm up
|
||||||
// addState(new TopArm(robot, "RightFourCone", "20-0"));
|
// 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)
|
// // 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)
|
// // 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"));
|
// addState(new TopArm(robot, "RightFourCone", "22-0"));
|
||||||
@@ -148,7 +146,7 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
|
|||||||
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "31-0"));
|
// addState(new DriverStateWithOdometer(robot, "RightFourCone", "31-0"));
|
||||||
//
|
//
|
||||||
// // 32 Rotate towards Stack of cones
|
// // 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
|
// // 33 Decide which path after scanning image from earlier
|
||||||
// addState(new PathDecision(robot, "RightFourCone", "33-0"));
|
// addState(new PathDecision(robot, "RightFourCone", "33-0"));
|
||||||
|
|||||||
@@ -115,7 +115,7 @@ public class CollectorDistanceState extends CyberarmState {
|
|||||||
|
|
||||||
distanceDelta = LastDistanceRead - currentDistance;
|
distanceDelta = LastDistanceRead - currentDistance;
|
||||||
|
|
||||||
if (distanceDelta >= -50.0 || currentDistance > 500) {
|
if (distanceDelta >= -15.0 || currentDistance > 500) {
|
||||||
// I am moving forward
|
// I am moving forward
|
||||||
// and im close too my target.
|
// and im close too my target.
|
||||||
LastDistanceRead = currentDistance;
|
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;
|
package org.timecrafters.Autonomous.States;
|
||||||
|
|
||||||
|
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||||
|
|
||||||
import org.cyberarm.engine.V2.CyberarmState;
|
import org.cyberarm.engine.V2.CyberarmState;
|
||||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||||
import org.timecrafters.TeleOp.states.PhoenixBot1;
|
import org.timecrafters.TeleOp.states.PhoenixBot1;
|
||||||
@@ -7,15 +9,34 @@ import org.timecrafters.TeleOp.states.PhoenixBot1;
|
|||||||
public class JunctionAllignmentState extends CyberarmState {
|
public class JunctionAllignmentState extends CyberarmState {
|
||||||
private final boolean stateDisabled;
|
private final boolean stateDisabled;
|
||||||
PhoenixBot1 robot;
|
PhoenixBot1 robot;
|
||||||
private double TargetSensorDistance;
|
|
||||||
private final String targetedJunction;
|
|
||||||
private final double drivePower;
|
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) {
|
public JunctionAllignmentState(PhoenixBot1 robot, String groupName, String actionName) {
|
||||||
this.robot = robot;
|
this.robot = robot;
|
||||||
this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value();
|
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;
|
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -23,133 +44,178 @@ public class JunctionAllignmentState extends CyberarmState {
|
|||||||
public void telemetry() {
|
public void telemetry() {
|
||||||
engine.telemetry.addData("right sensor distance", robot.rightPoleDistance.getDistance(DistanceUnit.MM));
|
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("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
|
@Override
|
||||||
public void exec() {
|
public void exec() {
|
||||||
|
|
||||||
|
currentAngle = robot.imu.getAngularOrientation().firstAngle;
|
||||||
|
|
||||||
|
|
||||||
if (stateDisabled){
|
if (stateDisabled){
|
||||||
setHasFinished(true);
|
setHasFinished(true);
|
||||||
} else {
|
} else {
|
||||||
|
|
||||||
double leftDistance = robot.leftPoleDistance.getDistance(DistanceUnit.MM);
|
double leftDistance = robot.leftPoleDistance.getDistance(DistanceUnit.MM);
|
||||||
double rightDistance = robot.rightPoleDistance.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!!!
|
// The minimum Value that can be seen when in distance of the pole is 90.0 the maximum is 200.0
|
||||||
switch (targetedJunction) {
|
|
||||||
case "low":
|
|
||||||
TargetSensorDistance = 90.0;
|
|
||||||
break;
|
|
||||||
case "mid":
|
|
||||||
TargetSensorDistance = 135.0;
|
|
||||||
break;
|
|
||||||
case "high":
|
|
||||||
TargetSensorDistance = 200.0;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
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 (!rightInRange && leftInRange) {
|
||||||
if (leftDistance > TargetSensorDistance && rightDistance > TargetSensorDistance){
|
|
||||||
robot.frontLeftDrive.setPower(drivePower);
|
// rotate 1 degree CCW
|
||||||
robot.frontRightDrive.setPower(drivePower);
|
|
||||||
robot.backLeftDrive.setPower(drivePower);
|
targetAngle = currentAngle - rotationAmount;
|
||||||
robot.backRightDrive.setPower(drivePower);
|
|
||||||
|
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);
|
if (!rightInRange && !leftInRange && driveLoops < 2){
|
||||||
robot.backLeftDrive.setPower(-drivePower);
|
|
||||||
robot.backRightDrive.setPower(-drivePower);
|
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) {
|
} else {
|
||||||
robot.frontLeftDrive.setPower(-drivePower);
|
|
||||||
robot.frontRightDrive.setPower(drivePower);
|
robot.backLeftDrive.setPower(0);
|
||||||
robot.backLeftDrive.setPower(-drivePower);
|
robot.backRightDrive.setPower(0);
|
||||||
robot.backRightDrive.setPower(drivePower);
|
robot.frontLeftDrive.setPower(0);
|
||||||
}
|
robot.frontRightDrive.setPower(0);
|
||||||
// the robot is going to rotate CW until a distance is met
|
|
||||||
if (leftDistance < TargetSensorDistance && rightDistance > TargetSensorDistance) {
|
robot.OdometerEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||||
robot.frontLeftDrive.setPower(drivePower);
|
robot.OdometerEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||||
robot.frontRightDrive.setPower(-drivePower);
|
|
||||||
robot.backLeftDrive.setPower(drivePower);
|
if (driveLoops == 2) {
|
||||||
robot.backRightDrive.setPower(-drivePower);
|
loopsCurrent = loopsTotal;
|
||||||
}
|
}
|
||||||
// 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);
|
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|||||||
@@ -81,6 +81,10 @@ public class RotationState extends CyberarmState {
|
|||||||
engine.telemetry.addData("Robot IMU Rotation", RobotRotation);
|
engine.telemetry.addData("Robot IMU Rotation", RobotRotation);
|
||||||
engine.telemetry.addData("Robot Target Rotation", targetRotation);
|
engine.telemetry.addData("Robot Target Rotation", targetRotation);
|
||||||
engine.telemetry.addData("Drive Power", drivePowerVariable);
|
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();
|
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 () {
|
private void setupRobot () {
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance");
|
collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance");
|
||||||
downSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "downDistance");
|
downSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "downDistance");
|
||||||
leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance");
|
leftPoleDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "Left Pole Distance");
|
||||||
@@ -170,34 +198,10 @@ public class PhoenixBot1 {
|
|||||||
HighRiserLeft.setPosition(0.45);
|
HighRiserLeft.setPosition(0.45);
|
||||||
HighRiserRight.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 RobotRotation;
|
||||||
private double RotationTarget, DeltaRotation;
|
private double RotationTarget, DeltaRotation;
|
||||||
private double MinimalPower = 0.25, topServoOffset = -0.05;
|
private double MinimalPower = 0.25, topServoOffset = -0.05;
|
||||||
private double servoCollectLow = 0.45; //Low servos, A button
|
private double servoCollectLow = 0.40; //Low servos, A button
|
||||||
private double servoCollectHigh = 0.55; //High servos, A button
|
private double servoCollectHigh = 0.40; //High servos, A button
|
||||||
private double servoLowLow = 0.5; //Low servos, X button
|
private double servoLowLow = 0.5; //Low servos, X button
|
||||||
private double servoLowHigh = 0.75; //High servos, X button
|
private double servoLowHigh = 0.75; //High servos, X button
|
||||||
private double servoMedLow = 0.5; //Low servos, B button
|
private double servoMedLow = 0.5; //Low servos, B button
|
||||||
|
|||||||
Reference in New Issue
Block a user