diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java index 4b5b584..dd0ff23 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java @@ -24,7 +24,7 @@ public class RightSideAutonomousEngine extends CyberarmEngine { public void setup() { robot = new PhoenixBot1(this); addState(new ServoCameraRotate(robot, "RightSideAutonomous", "00-0")); - addState(new ConeIdentification(robot, "RightSideAutonomous", "00-1")); +// addState(new ConeIdentification(robot, "RightSideAutonomous", "00-1")); addState(new ServoCameraRotate(robot, "RightSideAutonomous", "00-2")); //drive to high pole addState(new DriverState(robot, "RightSideAutonomous", "01-0")); diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java index f905481..04a9f67 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java @@ -23,6 +23,7 @@ public class CollectorDistanceState extends CyberarmState { private boolean inRange = false; private float collectTime; private double inRangeTime; + private boolean stateDisabled; public CollectorDistanceState(PhoenixBot1 robot, String groupName, String actionName) { @@ -32,6 +33,8 @@ public class CollectorDistanceState extends CyberarmState { this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value(); this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value(); this.collectTime = robot.configuration.variable(groupName, actionName, "collectTime").value(); + this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; + } @@ -92,6 +95,10 @@ public class CollectorDistanceState extends CyberarmState { @Override public void exec() { + if (stateDisabled){ + setHasFinished(true); + return; + } if (System.currentTimeMillis() - lastMeasuredTime > 150) { // checking to see if time is greater than 150 milliseconds diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/Mecanum_fancy_drive_TeleOp.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/Mecanum_fancy_drive_TeleOp.java index 13fe54f..533cdf8 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/Mecanum_fancy_drive_TeleOp.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/Mecanum_fancy_drive_TeleOp.java @@ -1,11 +1,12 @@ package org.timecrafters.minibots.cyberarm.engines; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.minibots.cyberarm.states.MecanumRobot; import org.timecrafters.minibots.cyberarm.states.Mecanum_Fancy_Drive_State; - +@Disabled @TeleOp(name = "Fancy Drive TeleOp") public class Mecanum_fancy_drive_TeleOp extends CyberarmEngine { diff --git a/TeamCode/src/main/java/org/timecrafters/testing/engine/PhoenixTeleOP.java b/TeamCode/src/main/java/org/timecrafters/testing/engine/PhoenixTeleOP.java index 5c32be9..4f5d2fe 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/engine/PhoenixTeleOP.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/engine/PhoenixTeleOP.java @@ -7,7 +7,7 @@ import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.testing.states.PhoenixBot1; import org.timecrafters.testing.states.PhoenixTeleOPState; -@TeleOp (name = "PhoenixTeleOP") +@TeleOp (name = "APhoenixTeleOP") public class PhoenixTeleOP extends CyberarmEngine { diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java index 4ad241f..afa9f46 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixBot1.java @@ -60,7 +60,7 @@ public class PhoenixBot1 { } private void setupRobot () { -// collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance"); + collectorDistance = engine.hardwareMap.get(Rev2mDistanceSensor.class, "collectorDistance"); BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixTeleOPState.java index f249fc1..5bf5aef 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PhoenixTeleOPState.java @@ -80,7 +80,7 @@ public class PhoenixTeleOPState extends CyberarmState { robot.frontRightDrive.setPower(drivePower); } - if (engine.gamepad1.left_trigger > 0) { + else if (engine.gamepad1.left_trigger > 0) { drivePower = engine.gamepad1.left_trigger; robot.backLeftDrive.setPower(-drivePower); robot.backRightDrive.setPower(-drivePower); @@ -88,14 +88,14 @@ public class PhoenixTeleOPState extends CyberarmState { robot.frontRightDrive.setPower(-drivePower); } - if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) { - drivePower = engine.gamepad1.left_stick_y; + if (Math.abs(engine.gamepad1.left_stick_y) > 0.05) { + drivePower = engine.gamepad1.left_stick_y * 0.3; robot.backRightDrive.setPower(drivePower); robot.frontRightDrive.setPower(drivePower); } - if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) { - drivePower = engine.gamepad1.right_stick_y; + if (Math.abs(engine.gamepad1.right_stick_y) > 0.05) { + drivePower = engine.gamepad1.right_stick_y * 0.3; robot.backLeftDrive.setPower(drivePower); robot.frontLeftDrive.setPower(drivePower); } @@ -117,7 +117,7 @@ public class PhoenixTeleOPState extends CyberarmState { robot.frontRightDrive.setPower(-drivePower); } - if (engine.gamepad1.a) { + if (engine.gamepad1.y) { RobotRotation = robot.imu.getAngularOrientation().firstAngle; RotationTarget = 180; CalculateDeltaRotation(); @@ -128,7 +128,7 @@ public class PhoenixTeleOPState extends CyberarmState { robot.frontLeftDrive.setPower(drivePower); robot.frontRightDrive.setPower(-drivePower); } - else if (RobotRotation > 0) { + else if (RobotRotation >= 0) { drivePower = (-1 * DeltaRotation/180) - MinimalPower; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); @@ -144,7 +144,7 @@ public class PhoenixTeleOPState extends CyberarmState { } } - if (engine.gamepad1.y) { + if (engine.gamepad1.a) { RobotRotation = robot.imu.getAngularOrientation().firstAngle; RotationTarget = 0; CalculateDeltaRotation(); @@ -199,23 +199,23 @@ public class PhoenixTeleOPState extends CyberarmState { if (engine.gamepad1.x) { RobotRotation = robot.imu.getAngularOrientation().firstAngle; - RotationTarget = -90; + RotationTarget = -45; CalculateDeltaRotation(); - if (RobotRotation < 90 && RobotRotation < -89) {//CCW + if (RobotRotation < -46 || RobotRotation > 135) {//CCW drivePower = (-1 * DeltaRotation/180) - MinimalPower; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); robot.frontLeftDrive.setPower(drivePower); robot.frontRightDrive.setPower(-drivePower); } - if (RobotRotation > 90 || RobotRotation < -91) {//CW + if (RobotRotation > -44 && RobotRotation < 135) {//CW drivePower = (1 * DeltaRotation/180) + MinimalPower; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); robot.frontLeftDrive.setPower(drivePower); robot.frontRightDrive.setPower(-drivePower); } - if (RobotRotation > -91 && RobotRotation < -89) { + if (RobotRotation > -46 && RobotRotation < -44) { drivePower = 0; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); @@ -253,23 +253,23 @@ public class PhoenixTeleOPState extends CyberarmState { if (engine.gamepad1.b) { RobotRotation = robot.imu.getAngularOrientation().firstAngle; - RotationTarget = 90; + RotationTarget = 45; CalculateDeltaRotation(); - if (RobotRotation > -90 && RobotRotation < 89) {//CCW + if (RobotRotation > -135 && RobotRotation < 44) {//CCW drivePower = (-1 * DeltaRotation/180) - MinimalPower; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); robot.frontLeftDrive.setPower(drivePower); robot.frontRightDrive.setPower(-drivePower); } - if (RobotRotation < -90 || RobotRotation > 91) {//CW + if (RobotRotation < -135 || RobotRotation > 44) {//CW drivePower = (1 * DeltaRotation/180) + MinimalPower; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); robot.frontLeftDrive.setPower(drivePower); robot.frontRightDrive.setPower(-drivePower); } - if (RobotRotation < 91 && RobotRotation > 89) { + if (RobotRotation < 46 && RobotRotation > 44) { drivePower = 0; robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(-drivePower); @@ -310,7 +310,7 @@ public class PhoenixTeleOPState extends CyberarmState { } if (engine.gamepad2.y) { - if (robot.HighRiserLeft.getPosition() < 0.9) { + if (robot.HighRiserLeft.getPosition() < 0.85) { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); @@ -391,62 +391,6 @@ public class PhoenixTeleOPState extends CyberarmState { - if (engine.gamepad2.start) { - - if (System.currentTimeMillis() - lastStepTime >= 150) { - lastStepTime = System.currentTimeMillis(); - - switch (CyclingArmUpAndDown) { - - // upper arm up - case 0: - if (robot.HighRiserLeft.getPosition() < 1) { - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); - robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() + 0.05); - } else { - CyclingArmUpAndDown = CyclingArmUpAndDown + 1; - } - break; - - // lower arm up - case 1: - if (robot.LowRiserLeft.getPosition() < 1) { - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + 0.05); - } else { - CyclingArmUpAndDown = CyclingArmUpAndDown + 1; - } - break; - - // lower arm down - case 2: - if (robot.LowRiserLeft.getPosition() >= 0.44) { - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - 0.05); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - 0.05); - } else { - CyclingArmUpAndDown = CyclingArmUpAndDown + 1; - } - break; - - // upper arm down - case 3: - if (robot.HighRiserLeft.getPosition() >= 0.45) { - robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() - 0.05); - robot.HighRiserRight.setPosition(robot.HighRiserRight.getPosition() - 0.05); - } else { - CyclingArmUpAndDown = 0; - } - break; - - default: - break; - - } // end of switch - }// end of time if statement - }// end of start button press - - - gamepad1Checker.update(); gamepad2Checker.update(); } diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/SteeringDriveExperiment.java b/TeamCode/src/main/java/org/timecrafters/testing/states/SteeringDriveExperiment.java new file mode 100644 index 0000000..7928b04 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/SteeringDriveExperiment.java @@ -0,0 +1,66 @@ +package org.timecrafters.testing.states; + +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.minibots.cyberarm.states.MecanumRobot; + + +public class SteeringDriveExperiment extends CyberarmState { + + private final PhoenixBot1 robot; + public boolean A; + public boolean X; + public boolean Y; + private double drivePower = 1; + private boolean bprev; + + + public SteeringDriveExperiment(PhoenixBot1 robot) { + this.robot = robot; + } + @Override + public void exec() { + + //Gamepad Read + + A = engine.gamepad1.a; + X = engine.gamepad1.x; + Y = engine.gamepad1.y; + + //Drivetrain Variables + double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative + double rx = engine.gamepad1.right_stick_x; + + //toggle for drive speed + boolean b = engine.gamepad1.b; + if (b && !bprev) { + if (drivePower == 1) { + drivePower = 0.5; + } else { + drivePower = 1; + } + } + bprev = b; + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, but only when + // at least one is out of the range [-1, 1] + + double denominator = Math.max(Math.abs(y) + Math.abs(rx), 1); + double frontLeftPower = (y + rx) / denominator; + double backLeftPower = (y + rx) / denominator; + double frontRightPower = (y - rx) / denominator; + double backRightPower = (y - rx) / denominator; + + robot.frontLeftDrive.setPower(frontLeftPower * drivePower); + robot.backLeftDrive.setPower(backLeftPower * drivePower); + robot.frontRightDrive.setPower(frontRightPower * drivePower); + robot.backRightDrive.setPower(backRightPower * drivePower); + + +//------------------------------------------------------------------------------------------------------------------------------------ + } +} + +