diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/SlowModeTestEngine.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/SlowModeTestEngine.java new file mode 100644 index 0000000..b5fd4ce --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/engines/SlowModeTestEngine.java @@ -0,0 +1,22 @@ +package org.timecrafters.minibots.cyberarm.engines; + +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.SlowModeTestPingPong; + +@TeleOp (name = "Slow Mode Test") + +public class SlowModeTestEngine extends CyberarmEngine { + MecanumRobot robot; + + @Override + public void setup() { + + robot = new MecanumRobot(this); + + addState(new SlowModeTestPingPong(robot)); + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DemoPingPongState.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DemoPingPongState.java index 3c24fa3..dc42ac8 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DemoPingPongState.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DemoPingPongState.java @@ -85,5 +85,10 @@ public class DemoPingPongState extends CyberarmState { } } + + @Override + public void exac() { + + } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DistanceSensorTest.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DistanceSensorTest.java index 1fe0646..5a6a500 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DistanceSensorTest.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/DistanceSensorTest.java @@ -136,4 +136,9 @@ public class DistanceSensorTest extends CyberarmState { engine.telemetry.addData("Amount", ballAmount); } + + @Override + public void exac() { + + } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Fancy_Drive_State.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Fancy_Drive_State.java index 88ed1ce..e57f762 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Fancy_Drive_State.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Fancy_Drive_State.java @@ -13,19 +13,17 @@ public class Mecanum_Fancy_Drive_State extends CyberarmState { public boolean A; public boolean X; public boolean Y; + private double drivePower = 1; + private boolean bprev; public Mecanum_Fancy_Drive_State(MecanumRobot robot) { this.robot = robot; } - @Override public void init() { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN); - } - @Override public void exec() { @@ -35,16 +33,21 @@ public class Mecanum_Fancy_Drive_State extends CyberarmState { 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 x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing double rx = engine.gamepad1.right_stick_x; - - //Shooter Variables - - //Gamepad variables + //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 @@ -61,10 +64,10 @@ public class Mecanum_Fancy_Drive_State extends CyberarmState { // directions on drive motors or put a negative in behind the joystick power to reverse it. // I put negatives in to reverse it because it was the easiest at the moment. - robot.frontLeftDrive.setPower(-frontLeftPower); - robot.backLeftDrive.setPower(-backLeftPower); - robot.frontRightDrive.setPower(-frontRightPower); - robot.backRightDrive.setPower(-backRightPower); + robot.frontLeftDrive.setPower(-frontLeftPower * drivePower); + robot.backLeftDrive.setPower(-backLeftPower * drivePower); + robot.frontRightDrive.setPower(-frontRightPower * drivePower); + robot.backRightDrive.setPower(-backRightPower * drivePower); //----------------------------------------------------------------- // Shooter program @@ -72,14 +75,10 @@ public class Mecanum_Fancy_Drive_State extends CyberarmState { if (A) { robot.shooterWheel.setPower(1); robot.collectorRight.setPower(0); - } - - if (Y) { + } if (Y) { robot.shooterWheel.setPower(0); robot.collectorRight.setPower(1); - } - - if (X) { + } if (X) { robot.shooterWheel.setPower(0); robot.collectorRight.setPower(0); } @@ -117,11 +116,11 @@ public class Mecanum_Fancy_Drive_State extends CyberarmState { } else if (robot.shooterWheel.getPower() == 1) { robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.RED); } else if (ballAmount.equals("3 balls")) { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN); + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.RED); } else if (ballAmount.equals("2 balls")) { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE_GREEN); + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.ORANGE); } else if (ballAmount.equals("1 balls")) { - robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.CP1_2_END_TO_END_BLEND); + robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.YELLOW); } else { robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE); } @@ -133,9 +132,17 @@ public class Mecanum_Fancy_Drive_State extends CyberarmState { @Override public void telemetry() { - engine.telemetry.addData("Distance", robot.ballPositionSensor.getDistance(DistanceUnit.CM)); engine.telemetry.addData("Amount", ballAmount); + engine.telemetry.addData("Speed", drivePower); + engine.telemetry.addData("FrontLeftEncoder", robot.frontLeftDrive.getCurrentPosition()); + engine.telemetry.addData("FrontRightEncoder", robot.frontRightDrive.getCurrentPosition()); + engine.telemetry.addData("BackLeftEncoder", robot.backLeftDrive.getCurrentPosition()); + engine.telemetry.addData("BackRightEncoder", robot.backRightDrive.getCurrentPosition()); + + } + @Override + public void exac() { } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Robot_State.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Robot_State.java index cd840e6..8fa63af 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Robot_State.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/Mecanum_Robot_State.java @@ -111,4 +111,9 @@ public class Mecanum_Robot_State extends CyberarmState { } } + + @Override + public void exac() { + + } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/SlowModeTestPingPong.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/SlowModeTestPingPong.java new file mode 100644 index 0000000..662d1a3 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/SlowModeTestPingPong.java @@ -0,0 +1,71 @@ +package org.timecrafters.minibots.cyberarm.states; + +import org.cyberarm.engine.V2.CyberarmState; + +public class SlowModeTestPingPong extends CyberarmState { + + MecanumRobot robot; + private double drivePower = 1; + private boolean bprev; + + + + public SlowModeTestPingPong (MecanumRobot robot){ + this.robot = robot; + } + + @Override + public void exec() { + + //toggle for drive speed + boolean b = engine.gamepad1.b; + if (b && !bprev) { + if (drivePower == 1) { + drivePower = 0.5; + } else { + drivePower = 1; + } + } + bprev = b; + + + + //Drivetrain Variables + double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative + double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing + double rx = engine.gamepad1.right_stick_x; + + //Gamepad variables + + // 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(x) + Math.abs(rx), 1); + double frontLeftPower = (y + x + rx) / denominator; + double backLeftPower = (y - x + rx) / denominator; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; + + // As I programed this and ran it, I realized everything was backwards + // in direction so to fix that i either went in the robot object state and reversed + // directions on drive motors or put a negative in behind the joystick power to reverse it. + // I put negatives in to reverse it because it was the easiest at the moment. + + robot.frontLeftDrive.setPower(-frontLeftPower * drivePower); + robot.backLeftDrive.setPower(-backLeftPower * drivePower); + robot.frontRightDrive.setPower(-frontRightPower * drivePower); + robot.backRightDrive.setPower(-backRightPower * drivePower); + + //----------------------------------------------------------------- + + + + + } + + @Override + public void exac() { + + } +}