diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/MiniYAutonomousState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/MiniYAutonomousState.java new file mode 100644 index 0000000..a7e909b --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/States/MiniYAutonomousState.java @@ -0,0 +1,13 @@ +package org.timecrafters.CenterStage.Autonomous.States; + +import dev.cyberarm.engine.V2.CyberarmState; + +public class MiniYAutonomousState extends CyberarmState { + + + + @Override + public void exec() { + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniBTeleOPBot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniBTeleOPBot.java index 2ea5457..9666531 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniBTeleOPBot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniBTeleOPBot.java @@ -3,6 +3,7 @@ package org.timecrafters.CenterStage.Common; import com.arcrobotics.ftclib.hardware.motors.MotorEx; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.Servo; @@ -17,8 +18,9 @@ public class MiniBTeleOPBot extends Robot { public HardwareMap hardwareMap; public MotorEx leftDrive, rightDrive; -public Servo servLowLeft, servLowRight, servTop; +public Servo servLeft, servLow, servTop; public IMU imu; +public double wheelCircum = 28.888 /* <- Wheel circumference in cm, 11.37 inches */; public TimeCraftersConfiguration configuration; @@ -49,9 +51,18 @@ public MiniBTeleOPBot() { leftDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); rightDrive.motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + leftDrive.motor.setDirection(DcMotorSimple.Direction.FORWARD); + rightDrive.motor.setDirection(DcMotorSimple.Direction.FORWARD); + //Servos - servLowLeft = hardwareMap.servo.get("ServoLowLeft"); - servLowRight = hardwareMap.servo.get("ServoLowRight"); + servLeft = hardwareMap.servo.get("ServoLeft"); + servLow = hardwareMap.servo.get("ServoLow"); servTop = hardwareMap.servo.get("ServoTop"); + + servLeft.setDirection(Servo.Direction.FORWARD); + servLow.setDirection(Servo.Direction.FORWARD); + servTop.setDirection(Servo.Direction.FORWARD); + + } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java index cb1c400..1e6c258 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/MiniYTeleOPBot.java @@ -18,6 +18,7 @@ public class MiniYTeleOPBot extends Robot { public MotorEx flDrive, frDrive, blDrive, brDrive; public IMU imu; private String string; + public double wheelCircum = 15.9/* <- Wheel circumference in cm, 6.26 inches, meaning it would take 3.83 wheel revolutions to move 2 feet */; public TimeCraftersConfiguration configuration; @@ -37,6 +38,7 @@ public class MiniYTeleOPBot extends Robot { RevHubOrientationOnRobot.UsbFacingDirection.UP)); imu.initialize(parameters); + imu.resetYaw(); //Motors frDrive = new MotorEx(hardwareMap, "FrontRight"); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java index 88c188b..21a0498 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/ProtoBotSodi.java @@ -41,7 +41,7 @@ public class ProtoBotSodi extends Robot { this.hardwareMap = CyberarmEngine.instance.hardwareMap; CyberarmEngine engine = CyberarmEngine.instance; -// TimeCraftersConfiguration configuration = new TimeCraftersConfiguration(); + configuration = new TimeCraftersConfiguration("Blue Crab"); //Motors frDrive = new MotorEx(hardwareMap, "FrontRight"); diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBTeleOPEngine.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBTeleOPEngine.java index 804234a..a1fc648 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBTeleOPEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/Engines/MiniBTeleOPEngine.java @@ -17,6 +17,6 @@ public class MiniBTeleOPEngine extends CyberarmEngine { this.robot = new MiniBTeleOPBot(); this.robot.setup(); - addState(new BlackMiniTeleOP()); + addState(new BlackMiniTeleOP(robot)); } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/BlackMiniTeleOP.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/BlackMiniTeleOP.java index 083d51a..73248d1 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/BlackMiniTeleOP.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/BlackMiniTeleOP.java @@ -1,5 +1,6 @@ package org.timecrafters.CenterStage.TeleOp.States; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.timecrafters.CenterStage.Common.MiniBTeleOPBot; import dev.cyberarm.engine.V2.CyberarmState; @@ -9,9 +10,13 @@ public class BlackMiniTeleOP extends CyberarmState { public BlackMiniTeleOP(MiniBTeleOPBot robot) {this.robot = robot;} - private double rPower, lPower; + private double rPower, lPower, servoPower; + private boolean critTipPoint; - public BlackMiniTeleOP() { + public BlackMiniTeleOP() {} + + @Override + public void telemetry() { } @Override @@ -20,22 +25,25 @@ public class BlackMiniTeleOP extends CyberarmState { lPower = 0; robot.rightDrive.motor.setPower(rPower); robot.leftDrive.motor.setPower(lPower); - } @Override public void exec() { - if (engine.gamepad1.right_trigger > 0.1) { - rPower = engine.gamepad1.right_trigger; + if (Math.abs(engine.gamepad1.right_stick_y) > 0.1) { + rPower = engine.gamepad1.right_stick_y; + robot.rightDrive.motor.setPower(rPower); + } else { + rPower = 0; robot.rightDrive.motor.setPower(rPower); } - if (engine.gamepad1.left_trigger > 0.1) { - lPower = engine.gamepad1.left_trigger; + if (Math.abs(engine.gamepad1.left_stick_y) > 0.1) { + lPower = engine.gamepad1.left_stick_y; + robot.leftDrive.motor.setPower(lPower); + } else { + lPower = 0; robot.leftDrive.motor.setPower(lPower); } - - } } diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java index 68c697a..56346fb 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/YellowMinibotTeleOP.java @@ -14,6 +14,7 @@ import org.timecrafters.CenterStage.Common.MiniYTeleOPBot; public class YellowMinibotTeleOP extends CyberarmState { private final MiniYTeleOPBot robot; public float angleDelta, drivePower; + public double lastToldAngle /** <- The angle the bot was last told to stop at **/; YawPitchRollAngles imuInitAngle; public YellowMinibotTeleOP(MiniYTeleOPBot robot) { @@ -52,6 +53,7 @@ public class YellowMinibotTeleOP extends CyberarmState { robot.imu.resetYaw(); imuInitAngle = robot.imu.getRobotYawPitchRollAngles(); + lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); GamepadChecker gamepad1Checker = new GamepadChecker(engine, engine.gamepad1); GamepadChecker gamepad2Checker = new GamepadChecker(engine, engine.gamepad2); @@ -60,15 +62,40 @@ public class YellowMinibotTeleOP extends CyberarmState { @Override public void exec() { - if (Math.abs(engine.gamepad1.left_stick_y) < 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1 && Math.abs(engine.gamepad1.right_stick_x) < 0.1) { + if (Math.abs(engine.gamepad1.left_stick_y) < 0.1 && + Math.abs(engine.gamepad1.left_stick_x) < 0.1 && + Math.abs(engine.gamepad1.right_stick_x) < 0.1) { + drivePower = 0; - robot.flDrive.motor.setPower(0); - robot.frDrive.motor.setPower(0); - robot.blDrive.motor.setPower(0); - robot.brDrive.motor.setPower(0); + robot.flDrive.motor.setPower(drivePower); + robot.frDrive.motor.setPower(drivePower); + robot.blDrive.motor.setPower(drivePower); + robot.brDrive.motor.setPower(drivePower); + } + + if (robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > lastToldAngle + 0.5 && + Math.abs(engine.gamepad1.left_stick_y) > 0.1) { + + robot.frDrive.motor.setPower(robot.flDrive.motor.getPower() * 0.8); + robot.brDrive.motor.setPower(robot.blDrive.motor.getPower() * 0.8); + + } else + if (robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < lastToldAngle - 0.5 && + Math.abs(engine.gamepad1.left_stick_y) > 0.1) { + + robot.flDrive.motor.setPower(robot.frDrive.motor.getPower() * 0.8); + robot.blDrive.motor.setPower(robot.brDrive.motor.getPower() * 0.8); + + } else { + + robot.flDrive.motor.setPower(drivePower); + robot.frDrive.motor.setPower(drivePower); + robot.blDrive.motor.setPower(drivePower); + robot.brDrive.motor.setPower(drivePower); } if (engine.gamepad1.start && !engine.gamepad1.a) { + robot.flDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.frDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robot.blDrive.motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); @@ -76,7 +103,10 @@ public class YellowMinibotTeleOP extends CyberarmState { robot.imu.resetYaw(); } - if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1) { + if (Math.abs(engine.gamepad1.left_stick_y) > 0.1 && Math.abs(engine.gamepad1.left_stick_x) < 0.1 && + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) > lastToldAngle - 0.5 && + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES) < lastToldAngle + 0.5) { + drivePower = engine.gamepad1.left_stick_y; robot.flDrive.motor.setPower(drivePower); robot.frDrive.motor.setPower(drivePower); @@ -85,6 +115,7 @@ public class YellowMinibotTeleOP extends CyberarmState { } if (Math.abs(engine.gamepad1.left_stick_x) > 0.1) { + drivePower = engine.gamepad1.left_stick_x; robot.flDrive.motor.setPower(-drivePower); robot.frDrive.motor.setPower(drivePower); @@ -92,12 +123,14 @@ public class YellowMinibotTeleOP extends CyberarmState { robot.brDrive.motor.setPower(-drivePower); } - if (Math.abs(engine.gamepad1.right_stick_x) > 0.1){ + if (Math.abs(engine.gamepad1.right_stick_x) > 0.1) { + drivePower = engine.gamepad1.right_stick_x; robot.flDrive.motor.setPower(-drivePower); robot.frDrive.motor.setPower(drivePower); robot.blDrive.motor.setPower(-drivePower); robot.brDrive.motor.setPower(drivePower); + lastToldAngle = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); } }