diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java new file mode 100644 index 0000000..d975600 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Drivetrain.java @@ -0,0 +1,99 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.arcrobotics.ftclib.gamepad.GamepadKeys; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.subsystems.Subsystem; + +import java.util.Objects; + +public class Drivetrain implements Subsystem { + + private GamepadEx gamepad; + + public MultipleTelemetry TELE; + + private String Mode = "Default"; + + private DcMotorEx fl; + + private DcMotorEx fr; + private DcMotorEx bl; + private DcMotorEx br; + + private double defaultSpeed = 0.7; + + private double slowSpeed = 0.3; + public Drivetrain(Robot robot, MultipleTelemetry tele, GamepadEx gamepad1){ + + this.fl = robot.frontLeft; + this.fr = robot.frontRight; + this.br = robot.backRight; + this.bl = robot.backLeft; + + this.gamepad = gamepad1; + + this.TELE = tele; + + + + } + + public void setMode (String mode){ + this.Mode = mode; + } + + public void setDefaultSpeed (double speed){ + this.defaultSpeed = speed; + } + + public void setSlowSpeed (double speed){ + this.slowSpeed = speed; + } + + public void FieldCentric(double fwd, double strafe, double turn, double turbo){ + + double y = -fwd; // Remember, Y stick value is reversed + double x = strafe * 1.1; // Counteract imperfect strafing + double rx = turn; + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, + // but only if 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; + + fl.setPower(frontLeftPower*turbo); + bl.setPower(backLeftPower*turbo); + fr.setPower(frontRightPower*turbo); + br.setPower(backRightPower*turbo); + + } + + + @Override + public void update() { + + if (Objects.equals(Mode, "Default")) { + + FieldCentric( + gamepad.getRightY(), + gamepad.getRightX(), + gamepad.getLeftX(), + (gamepad.getTrigger( + GamepadKeys.Trigger.RIGHT_TRIGGER) * (1-defaultSpeed) + - gamepad.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) * slowSpeed + + defaultSpeed + ) + ); + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Subsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Subsystem.java new file mode 100644 index 0000000..34bf534 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Subsystem.java @@ -0,0 +1,6 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +public interface Subsystem { + + public void update(); +}