diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPv2.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPv2.java index 3ff2788..ec67479 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPv2.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/PhoenixTeleOPv2.java @@ -5,6 +5,7 @@ import org.cyberarm.engine.V2.CyberarmState; public class PhoenixTeleOPv2 extends CyberarmState { private double drivePower = 1; PhoenixBot1 robot; + public PhoenixTeleOPv2(PhoenixBot1 robot) { this.robot = robot; } @@ -15,7 +16,6 @@ public class PhoenixTeleOPv2 extends CyberarmState { addParallelState(new TeleOPTankDriver(robot)); - } @Override @@ -45,9 +45,8 @@ public class PhoenixTeleOPv2 extends CyberarmState { robot.backRightDrive.setPower(BRPower); robot.frontLeftDrive.setPower(FLPower); robot.frontRightDrive.setPower(FRPower); + + } - - if (engine.gamepad1.right_stick_x ) - } } diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java index ab66ce5..4ea2779 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPArmDriver.java @@ -4,12 +4,15 @@ import com.qualcomm.robotcore.hardware.Servo; import org.cyberarm.engine.V2.CyberarmState; import org.cyberarm.engine.V2.GamepadChecker; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; public class TeleOPArmDriver extends CyberarmState { private final PhoenixBot1 robot; private long lastStepTime = 0; private int CyclingArmUpAndDown = 0; private GamepadChecker gamepad2Checker; + private int Opportunity, Endeavour; + private double drivePower; public TeleOPArmDriver(PhoenixBot1 robot) { this.robot = robot; @@ -34,6 +37,8 @@ public class TeleOPArmDriver extends CyberarmState { robot.LowRiserRight.setPosition(0.45); robot.HighRiserLeft.setPosition(0.45); robot.HighRiserRight.setPosition(0.45); + Opportunity = 0; + Endeavour = 10; gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); @@ -41,6 +46,15 @@ public class TeleOPArmDriver extends CyberarmState { @Override public void exec() { + if (robot.collectorDistance.getDistance(DistanceUnit.MM) < 275 && robot.collectorDistance.getDistance(DistanceUnit.MM) > 100) { + Endeavour = 0; + } - } -} + if (robot.collectorDistance.getDistance(DistanceUnit.MM) < 550 && robot.collectorDistance.getDistance(DistanceUnit.MM) > 275) { + Endeavour = 1; + } + + } + + + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java index ee017f1..6ad84a9 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java +++ b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/TeleOPTankDriver.java @@ -9,10 +9,12 @@ import org.cyberarm.engine.V2.GamepadChecker; public class TeleOPTankDriver extends CyberarmState { private final PhoenixBot1 robot; + private long lastStepTime = 0; private double drivePower = 0.3; private double RobotRotation; private double RotationTarget, DeltaRotation; private double MinimalPower = 0.2; + private int DeltaOdometerR, Endeavour, Spirit; private GamepadChecker gamepad1Checker; public TeleOPTankDriver(PhoenixBot1 robot) { this.robot = robot; @@ -34,6 +36,10 @@ public class TeleOPTankDriver extends CyberarmState { @Override public void exec() { + if (drivePower > 0.1 && ) { + + } + } public void CalculateDeltaRotation() { if (RotationTarget >= 0 && RobotRotation >= 0) { @@ -49,6 +55,16 @@ public class TeleOPTankDriver extends CyberarmState { DeltaRotation = Math.abs(RobotRotation + RotationTarget); } } + + public double getDeltaOdometerR() { + Spirit = robot.OdometerEncoder.getCurrentPosition(); + if (System.currentTimeMillis() - lastStepTime >= 1000) { + lastStepTime = System.currentTimeMillis(); + DeltaOdometerR = robot.OdometerEncoder.getCurrentPosition() - Spirit; + } + return DeltaOdometerR; + } + @Override public void buttonUp(Gamepad gamepad, String button) { if (engine.gamepad1 == gamepad && button.equals("start")) {