diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java index ddf4f54..8dd4318 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java @@ -9,6 +9,6 @@ public class RedWarehouseEngine extends CyberarmEngine { public void setup() { Robot robot = new Robot(this); - addState(new DriveState(robot, 2000, 2000,.25,.25)); + addState(new DriveState(robot, 2000, 2000,1,1)); } } diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/FaceState.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/FaceState.java new file mode 100644 index 0000000..ce8e29b --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/FaceState.java @@ -0,0 +1,42 @@ +package org.timecrafters.FreightFrenzy.Competition.Autonomous.States; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.FreightFrenzy.Competition.Common.Robot; + +public class FaceState extends CyberarmState { + private double faceDirection, tolerance, power; + private Robot robot; + public FaceState(Robot robot, double faceDirection, double tolerance, double power) { + this.robot = robot; + this.faceDirection = faceDirection; + this.tolerance = tolerance; + this.power = power; + } + + @Override + public void exec() { + robot.updateRobotOrientation(); + + if (robot.heading() - tolerance > faceDirection) { + robot.driveGoalLeft.setPower(power); + robot.driveWarehouseLeft.setPower(power); + robot.driveGoalRight.setPower(0); + robot.driveWarehouseRight.setPower(0); + } + + else if (robot.heading() + tolerance < faceDirection){ + robot.driveGoalLeft.setPower(0); + robot.driveWarehouseLeft.setPower(0); + robot.driveGoalRight.setPower(power); + robot.driveWarehouseRight.setPower(power); + } + + else { + robot.driveGoalLeft.setPower(0); + robot.driveWarehouseLeft.setPower(0); + robot.driveGoalRight.setPower(0); + robot.driveWarehouseRight.setPower(0); + setHasFinished(true); + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java new file mode 100644 index 0000000..23a1fae --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java @@ -0,0 +1,26 @@ +package org.timecrafters.FreightFrenzy.Competition.Autonomous.States; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.FreightFrenzy.Competition.Common.Robot; + +public class TurretArmExtension extends CyberarmState { + + private Robot robot; + private DcMotor motor; + private int position; + private double power; + + public TurretArmExtension(Robot robot, DcMotor motor, int position, double power) { + this.robot = robot; + this.motor = motor; + this.position = position; + this.power = power; + } + + @Override + public void exec() { + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretOrbit.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretOrbit.java new file mode 100644 index 0000000..94e5328 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretOrbit.java @@ -0,0 +1,31 @@ +package org.timecrafters.FreightFrenzy.Competition.Autonomous.States; + +import com.qualcomm.robotcore.hardware.CRServo; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.FreightFrenzy.Competition.Common.Robot; + +public class TurretOrbit extends CyberarmState { + private Robot robot; + private CRServo servo; + private long time; + private double power; + + public TurretOrbit(Robot robot, CRServo servo, long time, double power) { + this.robot = robot; + this.servo = servo; + this.time = time; + this. power = power; + } + + @Override + public void exec() { + if (runTime() < time) { + servo.setPower(power); + } + else { + servo.setPower(0); + setHasFinished(true); + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java index 1a0f919..bf05954 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java @@ -65,6 +65,7 @@ public class Robot { .translation(VUFORIA_CAMERA_FORWARD_DISPLACEMENT, VUFORIA_CAMERA_LEFT_DISPLACEMENT, VUFORIA_CAMERA_VERTICAL_DISPLACEMENT) .multiplied(Orientation.getRotationMatrix(AxesReference.EXTRINSIC, AxesOrder.XZY, AngleUnit.DEGREES, 90, 90, 0)); private RobotLocation vuforiaLastLocation; + private Orientation orientation; // Sensors public BNO055IMU imu; @@ -102,15 +103,19 @@ public class Robot { } public double heading() { - return imu.getAngularOrientation().firstAngle; + return orientation.firstAngle; } public double roll() { - return imu.getAngularOrientation().secondAngle; + return orientation.secondAngle; } public double pitch() { - return imu.getAngularOrientation().thirdAngle; + return orientation.thirdAngle; + } + + public void updateRobotOrientation(){ + orientation = imu.getAngularOrientation(); } public void activateTensorflow() { @@ -215,6 +220,8 @@ public class Robot { orangeArmRiser.setDirection(DcMotorSimple.Direction.FORWARD); orangeArmRiser.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); collectorOrange = engine.hardwareMap.crservo.get("collectorOrange"); + + orangeDispenser.setPosition(0.5); } private void initDepositor(){ diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java index 2cee19c..1c1475b 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/TeleOp/States/TeleOpState.java @@ -12,7 +12,7 @@ public class TeleOpState extends CyberarmState { double maxDriveSpeed, maxCollectorArmSpeed, maxDepositorArmSpeed; int maxArmTravelDistance, minArmTravelDistance; boolean armLimitToggle = false, orangeCollectorToggle = false, whiteCollectorToggle = false; - boolean allianceRed = true; + boolean allianceRedDriver1 = true, allianceRedDriver2 = true; public TeleOpState(Robot robot) { this.robot = robot; @@ -48,13 +48,13 @@ public class TeleOpState extends CyberarmState { // dispenser powered if (engine.gamepad1.b){ - robot.orangeDispenser.setPosition(.5); - } - - // if not pressed dispenser off - else { robot.orangeDispenser.setPosition(0); } + // if not pressed dispenser off + else { + robot.orangeDispenser.setPosition(0.5); + } + // if one of triggers pressed arm extends or unextends, there is also a limit on how far arm can extend if (robot.orangeArmBobbin.getCurrentPosition() <= maxArmTravelDistance && engine.gamepad1.left_trigger > 0) { @@ -84,7 +84,7 @@ public class TeleOpState extends CyberarmState { else { robot.turretServoOrange.setPower(0); }*/ - turretOrbitControl(engine.gamepad1, robot.turretServoOrange, allianceRed); + turretOrbitControl(engine.gamepad1, robot.turretServoOrange, allianceRedDriver1); // if dpad verticles pressed arm rises or lowers... if (engine.gamepad1.dpad_up || engine.gamepad1.dpad_down) { @@ -154,7 +154,7 @@ public class TeleOpState extends CyberarmState { // else { // robot.turretServoWhite.setPower(0); // } - turretOrbitControl(engine.gamepad2, robot.turretServoWhite, allianceRed); + turretOrbitControl(engine.gamepad2, robot.turretServoWhite, allianceRedDriver2); // if dpad verticles pressed arm rises or lowers if (engine.gamepad2.dpad_down || engine.gamepad2.dpad_up) { @@ -228,6 +228,12 @@ public class TeleOpState extends CyberarmState { robot.collectorOrange.setPower(-1); } break; + + case "guide": + allianceRedDriver1 = !allianceRedDriver1; + break; + + } } @@ -252,6 +258,9 @@ public class TeleOpState extends CyberarmState { robot.collectorWhite.setPower(-1); } break; + case "guide": + allianceRedDriver2 = !allianceRedDriver2; + break; } } }