From 79d1b205a0530decaebfbc853fdb0e6d883ea94b Mon Sep 17 00:00:00 2001 From: 535tobor <535tobor@gmail.com> Date: Fri, 26 Jan 2024 02:07:41 -0500 Subject: [PATCH] 1.1 --- .../teamcode/gamecode/autonomous/RedFarSide.java | 2 +- .../teamcode/gamecode/autonomous/TeamProps.java | 15 +++++++-------- .../EncoderTickDefinitions.java | 15 +++++++++++---- 3 files changed, 19 insertions(+), 13 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedFarSide.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedFarSide.java index 6ceef47..aa01cf4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedFarSide.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/RedFarSide.java @@ -43,7 +43,7 @@ public void runInitLoop() { } - @Override //290 + @Override public void runStart() { forwardAuto(3, 3,500); // strafeLeftAuto(8,3,500); // diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/TeamProps.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/TeamProps.java index 60432ae..47dff5e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/TeamProps.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/gamecode/autonomous/TeamProps.java @@ -50,12 +50,14 @@ public void runInit() { //arm = hardwareMap.dcMotor.get("arm"); mapMotors(hardwareMap, Wheels.FRONT_LEFT.abbreviation(),Wheels.FRONT_RIGHT.abbreviation(),Wheels.BACK_LEFT.abbreviation(),Wheels.BACK_RIGHT.abbreviation()); forwardMotors(true,false,true,false); + sensorRange = hardwareMap.get(DistanceSensor.class, "eye"); // ^ set motor directions initAprilTag(hardwareMap, "Webcam 1", telemetry); - sensorRange = hardwareMap.get(DistanceSensor.class, "color"); + Encoders.use(); Encoders.clear(); + } @Override @@ -63,13 +65,10 @@ public void runInitLoop(){} @Override public void runStart() { - backwardAuto(5,1,500); // away from wall. - sleep(2000); - turnRightAuto(800, 1, 500); // straighten out. - sleep(2000); - forwardAuto(5,1,500); - sleep(2000); - strafeRightAuto(5,1,500); + backwardAuto(13,3,200); // away from wall. + turnRightAuto(580+45+22,3,500); +// styles said 290 should = 45 degrees so 45 + 45 is 90 so 580 because 290 + 290 = 580 + /*sleep(2500); backwardAuto(5,1,500); sleep(2500); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/motors/drive/definingDriveMovements/EncoderTickDefinitions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/motors/drive/definingDriveMovements/EncoderTickDefinitions.java index cde4d3a..f1a2dfd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/motors/drive/definingDriveMovements/EncoderTickDefinitions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/operations/outputs/motors/drive/definingDriveMovements/EncoderTickDefinitions.java @@ -17,12 +17,18 @@ public static void forwardAuto(double inches, long seconds, int encoderSpeed) { Encoders.target(-finalTicks,-finalTicks,-finalTicks,-finalTicks); // sets target position Encoders.go(); // goes to target position drive(-encoderSpeed,-encoderSpeed,-encoderSpeed,-encoderSpeed); // sets the velocity drive - driveStop(); sleep(seconds*1000); + driveStop(); } public static void backwardAuto(double inches, long seconds, int encoderSpeed) { // forward good. - forwardAuto(-inches, seconds, -encoderSpeed); + int finalTicks = (int) (inches * ticksPerInch); + Encoders.clear(); // resets wheel encoders + Encoders.target(finalTicks,finalTicks,finalTicks,finalTicks); // sets target position + Encoders.go(); // goes to target position + drive(encoderSpeed,encoderSpeed,encoderSpeed,encoderSpeed); // sets the velocity drive + sleep(seconds*1000); + driveStop(); } public static void strafeLeftAuto(double inches, long seconds, int encoderSpeed) { // strafe good. // this is left @@ -31,8 +37,9 @@ public static void strafeLeftAuto(double inches, long seconds, int encoderSpeed) Encoders.target(finalTicks,-finalTicks,-finalTicks,finalTicks); // sets target position Encoders.go(); // goes to target position // fl, fr, bl, br drive(encoderSpeed,-encoderSpeed,-encoderSpeed,encoderSpeed); // sets the velocity drive - driveStop(); sleep(seconds*1000); + driveStop(); + } public static void strafeRightAuto(double inches, long seconds, int encoderSpeed) { // strafe good. // this is left @@ -45,8 +52,8 @@ public static void turnRightAuto(float degrees, long seconds, int encoderSpeed) Encoders.target(-finalTicks,finalTicks,-finalTicks,finalTicks); // sets target position Encoders.go(); // goes to target position // fl, fr, bl, br drive(-encoderSpeed,encoderSpeed,-encoderSpeed,encoderSpeed); // sets the velocity drive - driveStop(); sleep(seconds*1000); + driveStop(); } public static void turnLeftAuto(float degrees, long seconds, int encoderSpeed) { // turn