From 97bf2a6caae401e5c734917296a1cc3ec1b57e35 Mon Sep 17 00:00:00 2001 From: Yash Karandikar Date: Mon, 7 Nov 2022 21:52:45 -0600 Subject: [PATCH] Advanced trigonometry!!!! (movement API part 2) --- .../createdcode/enhancedautos/API.java | 8 ++ .../enhancedautos/MovementAPI.java | 129 ++++++++++++++++++ 2 files changed, 137 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/MovementAPI.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/API.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/API.java index 135287a..5331531 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/API.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/API.java @@ -37,4 +37,12 @@ public class API { public int getLargest(int x, int y, int z) { return Math.max(z, Math.max(x, y)); } + + public void print(String s) { + opMode.telemetry.addLine(s); + } + + public void print(String caption, String value) { + opMode.telemetry.addData(caption, value); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/MovementAPI.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/MovementAPI.java new file mode 100644 index 0000000..e2cd059 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/MovementAPI.java @@ -0,0 +1,129 @@ +package org.firstinspires.ftc.teamcode.createdcode.enhancedautos; + +import com.qualcomm.robotcore.hardware.DcMotor; + +public class MovementAPI { + private final API api; + + private final DcMotor fl; + private final DcMotor fr; + private final DcMotor bl; + private final DcMotor br; + + private boolean flipped = false; + + public DcMotor getFL() { return fl; } + public DcMotor getFR() { return fr; } + public DcMotor getBL() { return bl; } + public DcMotor getBR() { return br; } + + /** + * Initializes the API + * + * @param _fl the front-left wheel + * @param _fr the front-right wheel + * @param _bl the back-left wheel + * @param _br the back-right wheel + */ + public MovementAPI(API api, DcMotor _fl, DcMotor _fr, DcMotor _bl, DcMotor _br) { + this.api = api; + + api.print("Initializing MovementApi..."); + + fl = _fl; + fr = _fr; + bl = _bl; + br = _br; + + fl.setDirection(DcMotor.Direction.REVERSE); + bl.setDirection(DcMotor.Direction.REVERSE); + fr.setDirection(DcMotor.Direction.FORWARD); + br.setDirection(DcMotor.Direction.FORWARD); + } + + public void setFlipped(boolean flipped) { + this.flipped = flipped; + } + + /** + * Moves the robot given the speed to move forward/back and left/right + * + * @param powerY the speed to move forward/back, -1 to 1, positive being forward + * @param powerX the speed to move left/right, -1 to 1, positive being to the right + * @param turn the speed to turn at, -1 to 1, positive being clockwise + * @param speed a multiplier on the final speed + * @param verbose whether or not to log extra data to telemetry + */ + public void move(double powerY, double powerX, double turn, double speed, boolean verbose) { + if (flipped) { + powerX *= -1; + turn *= -1; + } + + double flPower = (powerY + turn + powerX) * speed; + double frPower = (powerY - turn - powerX) * speed; + double blPower = (powerY + turn - powerX) * speed; + double brPower = (powerY - turn + powerX) * speed; + + double scale = Math.max(1, (Math.abs(powerY) + Math.abs(turn) + Math.abs(powerX)) * Math.abs(speed)); // shortcut for max(abs([fl,fr,bl,br])) + flPower /= scale; + frPower /= scale; + blPower /= scale; + brPower /= scale; + + fl.setPower(flPower); + fr.setPower(frPower); + bl.setPower(blPower); + br.setPower(brPower); + + if (verbose) api.print( + "Front Left: " + flPower + System.lineSeparator() + + "Back Left: " + blPower + System.lineSeparator() + + "Front Right: " + frPower + System.lineSeparator() + + "Back Right: " + brPower + System.lineSeparator() + ); + } + + /** + * Moves the robot given the speed to move forward/back and left/right + * @param powerY the speed to move forward/back, -1 to 1, positive being forward + * @param powerX the speed to move left/right, -1 to 1, positive being to the right + * @param turn the speed to turn at, -1 to 1, positive being clockwise + * @param speed a multiplier on the final speed + * + */ + public void move(double powerY, double powerX, double turn, double speed) { + move(powerY, powerX, turn, speed, false); + } + + /** + * Moves the robot in a given direction with a given speed + * + * @param direction the direction to move in, in degrees, with positive being left of forward + * @param speed the speed to move at + * @param verbose whether or not to log extra data to telemetry + */ + public void move(double direction, double speed, boolean verbose) { + move(Math.cos(Math.toRadians(direction)), Math.sin(Math.toRadians(-direction)), 0, speed, verbose); + } + + /** + * Moves the robot in a given direction with a given speed + * + * @param direction the direction to move in, in degrees, with positive being left of forward + * @param speed the speed to move at + */ + public void move(double direction, double speed) { + move(direction, speed, false); + } + + /** + * Stops the robot + */ + public void stop() { + fl.setPower(0); + fr.setPower(0); + bl.setPower(0); + br.setPower(0); + } +}