KeshavAnandCode/FTCDecodeMain
Archived
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'main' into danielv5

Browse Source
This commit is contained in:
DanTheMan-byte
2026-06-01 14:44:08 -05:00
parent 76f58308fb 6e31da5f1c
commit dd2890ea4a
4 changed files with 148 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
View File

@@ -1,7 +1,6 @@
package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
@@ -13,7 +12,6 @@ import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.Pose;
@@ -22,9 +20,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.constants.Color;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.constants.StateEnums;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.utils.Drivetrain;
import org.firstinspires.ftc.teamcode.utils.Flywheel;

49
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java Normal file
View File

@@ -0,0 +1,49 @@
package org.firstinspires.ftc.teamcode.teleop;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.utilsv2.Drivetrain;
import org.firstinspires.ftc.teamcode.utils.Robot;
@TeleOp
@Config
public class TeleopV4 extends LinearOpMode {
Robot robot;
Drivetrain drivetrain;
MultipleTelemetry TELE;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(
FtcDashboard.getInstance().getTelemetry(), telemetry
);
drivetrain = new Drivetrain(robot, TELE);
drivetrain.setTelemetry(true);
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()){
//Drivetrain
drivetrain.drive(
-gamepad1.right_stick_y,
gamepad1.right_stick_x,
gamepad1.left_stick_x
);
TELE.update();
}
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java
View File

@@ -22,7 +22,7 @@ public class Drivetrain {
private final ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200);
public Drivetrain (Robot rob, Follower follower){
public Drivetrain(Robot rob, Follower follower){
this.robot = rob;
this.follower = follower;

98
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java Normal file
View File

@@ -0,0 +1,98 @@
package org.firstinspires.ftc.teamcode.utilsv2;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
public class Drivetrain {
Robot robot;
MultipleTelemetry telemetry;
private static final double DEADZONE = 0.15;
private static final double AXIS_SNAP_THRESHOLD = 0.12;
private static final double STRAFE_MULTIPLIER = 1.2;
public static double FORWARD_ROTATION_CORRECTION = 0.03;
public static double STRAFE_ROTATION_CORRECTION = -0.03;
private boolean tele = false;
public Drivetrain(Robot rob, MultipleTelemetry TELE) {
this.robot = rob;
robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
robot.frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
this.telemetry = TELE;
}
public void setTelemetry(boolean input) {
tele = input;
}
public void drive(double y, double x, double rx) {
boolean snappedForward = false;
boolean snappedStrafe = false;
if (Math.abs(y) < DEADZONE) y = 0;
if (Math.abs(x) < DEADZONE) x = 0;
if (Math.abs(rx) < DEADZONE) rx = 0;
if (Math.abs(x) < AXIS_SNAP_THRESHOLD) {
x = 0;
snappedForward = true;
}
if (Math.abs(y) < AXIS_SNAP_THRESHOLD) {
y = 0;
snappedStrafe = true;
}
x *= STRAFE_MULTIPLIER;
double correctionRX = 0;
if (rx == 0) {
correctionRX += (y * FORWARD_ROTATION_CORRECTION);
correctionRX += (x * STRAFE_ROTATION_CORRECTION);
rx += correctionRX;
}
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;
robot.frontLeft.setPower(frontLeftPower);
robot.backLeft.setPower(backLeftPower);
robot.frontRight.setPower(frontRightPower);
robot.backRight.setPower(backRightPower);
if (tele) {
telemetry.addData("Forward Snap", snappedForward);
telemetry.addData("Strafe Snap", snappedStrafe);
telemetry.addData("Correction RX", correctionRX);
telemetry.addData("FL", frontLeftPower);
telemetry.addData("BL", backLeftPower);
telemetry.addData("FR", frontRightPower);
telemetry.addData("BR", backRightPower);
}
}
}