diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java index 7dce93b..2ba2ade 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/Common/PrototypeRobot.java @@ -2,9 +2,13 @@ package org.timecrafters.CenterStage.Common; import com.arcrobotics.ftclib.drivebase.HDrive; import com.arcrobotics.ftclib.hardware.RevIMU; +import com.arcrobotics.ftclib.hardware.ServoEx; import com.arcrobotics.ftclib.hardware.motors.MotorEx; import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; import org.timecrafters.Library.Robot; @@ -13,9 +17,9 @@ import dev.cyberarm.engine.V2.CyberarmEngine; public class PrototypeRobot extends Robot { private HardwareMap hardwareMap; - private MotorEx frontLeft, frontRight, backLeft, backRight; + public MotorEx frontLeft, frontRight, backLeft, backRight; private RevIMU imu; - + public Servo depositorFlip, depositor; private HDrive xDrive; private String string; private CyberarmEngine engine; @@ -36,6 +40,16 @@ public class PrototypeRobot extends Robot { backRight = new MotorEx(hardwareMap, "backRight"); backLeft = new MotorEx(hardwareMap, "backLeft"); + frontRight.motor.setDirection(DcMotorSimple.Direction.FORWARD); + backRight.motor.setDirection(DcMotorSimple.Direction.FORWARD); + backLeft.motor.setDirection(DcMotorSimple.Direction.FORWARD); + frontLeft.motor.setDirection(DcMotorSimple.Direction.FORWARD); + + + frontRight.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backRight.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backLeft.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + frontLeft.motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); //IMU imu = new RevIMU(hardwareMap, "imu"); BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); @@ -43,14 +57,18 @@ public class PrototypeRobot extends Robot { parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; imu.init(parameters); + //SERVO + depositorFlip = hardwareMap.servo.get("depositorFlip"); + depositor = hardwareMap.servo.get("depositor"); + // input motors exactly as shown below xDrive = new HDrive(frontLeft, frontRight, - backLeft, backRight); + backLeft, backRight); } public void driveTrainTeleOp() { - xDrive.driveFieldCentric(engine.gamepad1.left_stick_x, engine.gamepad1.left_stick_y, engine.gamepad1.right_stick_x, heading()); + xDrive.driveRobotCentric(-engine.gamepad1.left_stick_x, engine.gamepad1.left_stick_y, -engine.gamepad1.right_stick_x); } public double heading() { diff --git a/TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java b/TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java index 14c3446..12f631b 100644 --- a/TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java +++ b/TeamCode/src/main/java/org/timecrafters/CenterStage/States/PrototypeRobotDrivetrainState.java @@ -6,11 +6,26 @@ import dev.cyberarm.engine.V2.CyberarmState; public class PrototypeRobotDrivetrainState extends CyberarmState { private PrototypeRobot robot; + public PrototypeRobotDrivetrainState(PrototypeRobot robot) { this.robot = robot; } + @Override public void exec() { robot.driveTrainTeleOp(); + + if (engine.gamepad1.y){ + robot.depositorFlip.setPosition(0.75); + } else if (engine.gamepad1.a){ + robot.depositorFlip.setPosition(0.05); + } + + // depositor + if (engine.gamepad1.b){ + robot.depositor.setPosition(0.8); + } else if (engine.gamepad1.x){ + robot.depositor.setPosition(0.2); + } } }