mirror of
https://github.com/TimeCrafters/FTC_2022
synced 2025-12-15 15:32:35 +00:00
Added steps to new Autonomous and made states that run parallel states
This commit is contained in:
SpencerPiha
@@ -8,7 +8,14 @@ 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;
|
||||
import org.timecrafters.TeleOp.states.PhoenixBot1;
|
||||
|
||||
@Autonomous (name = "3 cone auto right")
|
||||
@@ -22,73 +29,113 @@ public class RightFourConeAutonomousEngine extends CyberarmEngine {
|
||||
|
||||
// 1 Rotate camera down at the signal
|
||||
addState(new ServoCameraRotate(robot, "RightFourCone", "01-0"));
|
||||
|
||||
// 2 Scan custom image
|
||||
addState(new ConeIdentification(robot, "RightFourCone", "02-0"));
|
||||
|
||||
// 3 Rotate Camera up, out of the way so it doesn't crash into stuff
|
||||
addState(new ServoCameraRotate(robot, "RightFourCone", "03-0"));
|
||||
// 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"));
|
||||
// 5 Turn Towards and look for junction with sensor
|
||||
|
||||
// 6 Raise lower arm while slowly driving at the junction
|
||||
// 4 Drive to the tall Pole (not all the way) while raising upper arm, this will be parallel
|
||||
addState(new DriverStateWithOdometerUpperArmParallelState(robot, "RightFourCone", "04-0"));
|
||||
|
||||
// 5 Turn Towards and look for junction with sensor
|
||||
addState(new RotationState(robot, "RightFourCone", "05-0"));
|
||||
|
||||
// 6 Raise lower arm while slowly driving at the junction (parallel state)
|
||||
addState(new JunctionAllignmentState(robot, "RightFourCone", "06-0"));
|
||||
|
||||
// 7 Drop top arm down on the junction to place cone
|
||||
addState(new TopArm(robot, "RightFourCone", "07-0"));
|
||||
|
||||
// 8 Drop cone as soon as arm is in position
|
||||
addState(new CollectorState(robot, "RightFourCone", "08-0"));
|
||||
|
||||
// 9 Raise arm to clear junction
|
||||
addState(new TopArm(robot, "RightFourCone", "09-0"));
|
||||
|
||||
// 10 Back up and bring lower arm down (parallel state)
|
||||
addState(new DriverStateWithOdometerLowerArmParallelState2nd(robot, "RightFourCone", "10-0"));
|
||||
|
||||
// 11 Bring upper arm to the correct position for the top cone on stack (check with distance sensor)
|
||||
addState(new TopArm(robot, "RightFourCone", "11-0"));
|
||||
|
||||
// 12 Rotate towards stack (this might be parallel with last step)
|
||||
// 12 Rotate towards stack
|
||||
//filled in as parallel state. in parallel with previous state
|
||||
|
||||
// 13 Drive at stack while collecting and check to see when we grab it
|
||||
addState(new CollectorDistanceState(robot, "RightFourCone", "13-0"));
|
||||
|
||||
// 14 Back up and raise arm (in parallel state)
|
||||
// 14 Back up and raise arm
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "14-0"));
|
||||
addState(new TopArm(robot, "RightFourCone", "14-1"));
|
||||
|
||||
// 15 Drive All the way back to the medium 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"));
|
||||
addState(new JunctionAllignmentState(robot, "RightFourCone", "16-1"));
|
||||
|
||||
// 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"));
|
||||
|
||||
// 19 Drop cone
|
||||
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"));
|
||||
|
||||
// 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"));
|
||||
|
||||
// 23 Drive slower at the stack and run the collector to grab a 2nd cone off of the stack
|
||||
addState(new CollectorDistanceState(robot, "RightFourCone", "23-0"));
|
||||
|
||||
// 24 Drive Back and lift up all the way to position for the low junction (parallel state)
|
||||
// 24 Drive Back and lift up all the way to position for the low junction
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "24-0"));
|
||||
addState(new TopArm(robot, "RightFourCone", "24-1"));
|
||||
|
||||
// 25 Drive back faster after the cone is fully off of the stack
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "25-0"));
|
||||
|
||||
// 26 Turn and look for the low junction with the distance sensor and align
|
||||
addState(new RotationState(robot, "RightFourCone", "26-0"));
|
||||
|
||||
// 27 Drive forward / backwards if you need to. (check with distance sensor)
|
||||
addState(new JunctionAllignmentState(robot, "RightFourCone", "26-1"));
|
||||
|
||||
// 28 Bring Upper arm down on junction
|
||||
addState(new TopArm(robot, "RightFourCone", "28-0"));
|
||||
|
||||
// 29 Let go of cone right after arm is in position
|
||||
addState(new CollectorState(robot, "RightFourCone", "29-0"));
|
||||
|
||||
// 30 Raise arm as soon as the cone is dropped
|
||||
addState(new TopArm(robot, "RightFourCone", "30-0"));
|
||||
|
||||
// 31 Back up / go forward (optional, only needed if we drove forwards or backwards to align to low junction
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "31-0"));
|
||||
|
||||
// 32 Rotate towards Stack of cones
|
||||
addState(new RotationState(robot, "RightFourCone", "32-0"));
|
||||
|
||||
// 33 Decide which path after scanning image from earlier
|
||||
addState(new PathDecision(robot, "RightFourCone", "33-0"));
|
||||
|
||||
// 34 Drive backwards, forwards, or stay put
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-1"));
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-2"));
|
||||
addState(new DriverStateWithOdometer(robot, "RightFourCone", "34-3"));
|
||||
|
||||
// 35 Rotate towards alliance terminal
|
||||
addState(new RotationState(robot, "RightFourCone", "35-0"));
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,112 @@
|
||||
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);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,112 @@
|
||||
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.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", "10-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);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,112 @@
|
||||
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 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);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
addParallelState(new TopArm(robot, "RightFourCone", "04-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);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -42,7 +42,7 @@ public class JunctionAllignmentState extends CyberarmState {
|
||||
break;
|
||||
}
|
||||
// the state is finished if the distance sensors are at the correct distance.
|
||||
if (leftDistance == TargetSensorDistance && rightDistance == TargetSensorDistance) {
|
||||
if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && (rightDistance > TargetSensorDistance -2 || rightDistance < TargetSensorDistance + 2)) {
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
robot.frontRightDrive.setPower(0);
|
||||
robot.backLeftDrive.setPower(0);
|
||||
@@ -77,29 +77,29 @@ public class JunctionAllignmentState extends CyberarmState {
|
||||
robot.backLeftDrive.setPower(drivePower);
|
||||
robot.backRightDrive.setPower(-drivePower);
|
||||
}
|
||||
// The right sensor is aligned but the robot must rotate rotate CW with only the left side powered
|
||||
else if (leftDistance < TargetSensorDistance && rightDistance == TargetSensorDistance) {
|
||||
// The right sensor is aligned but the robot must rotate CW with only the left side powered
|
||||
else 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
|
||||
else if (leftDistance > TargetSensorDistance && rightDistance == TargetSensorDistance) {
|
||||
else 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 rotate CW with only the right side powered
|
||||
else if (leftDistance == TargetSensorDistance && rightDistance < TargetSensorDistance) {
|
||||
// The left sensor is aligned but the robot must rotate CW with only the right side powered
|
||||
else 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
|
||||
else if (leftDistance == TargetSensorDistance && rightDistance > TargetSensorDistance) {
|
||||
else if ((leftDistance > TargetSensorDistance -2 || leftDistance < TargetSensorDistance + 2) && rightDistance > TargetSensorDistance) {
|
||||
robot.frontLeftDrive.setPower(0);
|
||||
robot.frontRightDrive.setPower(drivePower);
|
||||
robot.backLeftDrive.setPower(0);
|
||||
|
||||
@@ -0,0 +1,82 @@
|
||||
package org.timecrafters.Autonomous.States;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.TeleOp.states.PhoenixBot1;
|
||||
|
||||
public class TopArmRotateParallelState extends CyberarmState {
|
||||
|
||||
private final boolean stateDisabled;
|
||||
PhoenixBot1 robot;
|
||||
double UpperRiserRightPos, UpperRiserLeftPos, AddedDistance;
|
||||
long time;
|
||||
long lastStepTime = 0;
|
||||
boolean up;
|
||||
boolean directPosition;
|
||||
|
||||
public TopArmRotateParallelState(PhoenixBot1 robot, String groupName, String actionName) {
|
||||
this.robot = robot;
|
||||
this.UpperRiserLeftPos = robot.configuration.variable(groupName, actionName, "UpperRiserLeftPos").value();
|
||||
this.UpperRiserRightPos = robot.configuration.variable(groupName, actionName, "UpperRiserRightPos").value();
|
||||
this.time = robot.configuration.variable(groupName, actionName, "time").value();
|
||||
this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value();
|
||||
this.directPosition = robot.configuration.variable(groupName, actionName, "directPosition").value();
|
||||
|
||||
this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled;
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void start() {
|
||||
up = robot.HighRiserLeft.getPosition() < UpperRiserLeftPos;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
addParallelState(new RotationState(robot, "RightFourCone", "12-0"));
|
||||
|
||||
if (stateDisabled){
|
||||
setHasFinished(true);
|
||||
return;
|
||||
}
|
||||
|
||||
if (directPosition) {
|
||||
robot.HighRiserLeft.setPosition(UpperRiserLeftPos);
|
||||
robot.HighRiserRight.setPosition(UpperRiserRightPos);
|
||||
|
||||
if (runTime() >= time){
|
||||
setHasFinished(true);
|
||||
}
|
||||
} else {
|
||||
if (System.currentTimeMillis() - lastStepTime >= time) {
|
||||
lastStepTime = System.currentTimeMillis();
|
||||
|
||||
if (robot.HighRiserLeft.getPosition() < UpperRiserLeftPos && up) {
|
||||
|
||||
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + AddedDistance);
|
||||
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + AddedDistance);
|
||||
|
||||
} else if (robot.HighRiserLeft.getPosition() > UpperRiserLeftPos && !up) {
|
||||
|
||||
robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - AddedDistance);
|
||||
robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - AddedDistance);
|
||||
|
||||
} else {
|
||||
setHasFinished(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
engine.telemetry.addData("UpperRiserRightPos",UpperRiserRightPos);
|
||||
engine.telemetry.addData("UpperRiserLeftPos",UpperRiserLeftPos);
|
||||
engine.telemetry.addData("AddedDistance",AddedDistance);
|
||||
engine.telemetry.addData("time",time);
|
||||
engine.telemetry.addData("servo position left", robot.HighRiserLeft.getPosition());
|
||||
engine.telemetry.addData("servo position Right", robot.HighRiserRight.getPosition());
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -19,6 +19,10 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig
|
||||
|
||||
public class PhoenixBot1 {
|
||||
|
||||
private static final float mmPerInch = 25.4f;
|
||||
public static final double WHEEL_CIRCUMFERENCE = 7.42108499;
|
||||
public static final int COUNTS_PER_REVOLUTION = 8192;
|
||||
|
||||
// private static final String TFOD_MODEL_ASSET = "22-23_PowerPlay_Colors.tflite";
|
||||
private static final String TFOD_MODEL_ASSET = "AprilTagsV1.tflite";
|
||||
|
||||
@@ -52,6 +56,14 @@ public class PhoenixBot1 {
|
||||
|
||||
public AdafruitI2cColorSensor AdafruitEncoder;
|
||||
|
||||
public double ticksToInches(int ticks) {
|
||||
return ticks * (WHEEL_CIRCUMFERENCE / COUNTS_PER_REVOLUTION);
|
||||
}
|
||||
|
||||
public double inchesToTicks(double inches) {
|
||||
return inches * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE);
|
||||
}
|
||||
|
||||
|
||||
public PhoenixBot1(CyberarmEngine engine) {
|
||||
this.engine = engine;
|
||||
@@ -62,6 +74,7 @@ public class PhoenixBot1 {
|
||||
}
|
||||
|
||||
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");
|
||||
|
||||
Reference in New Issue
Block a user