eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Goog

This commit is contained in:
sihan 2024-03-09 09:41:48 -06:00
parent 86370534f5
commit af634882bd
3 changed files with 33 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java
View File

@ -68,10 +68,10 @@ public class DriveConstants {
* small and gradually increase them later after everything is working. All distance units are
* inches.
*/
public static double MAX_VEL = 50;
public static double MAX_ACCEL = 50;
public static double MAX_ANG_VEL = Math.toRadians(60);
public static double MAX_ANG_ACCEL = Math.toRadians(60);
public static double MAX_VEL = 80;
public static double MAX_ACCEL = 80;
public static double MAX_ANG_VEL = Math.toRadians(360);
public static double MAX_ANG_ACCEL = Math.toRadians(660);
/*
* Adjust the orientations here to match your robot. See the FTC SDK documentation for details.

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java
View File

@ -105,10 +105,13 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr
DriveConstants.LOGO_FACING_DIR, DriveConstants.USB_FACING_DIR));
imu.initialize(parameters);
leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
this.leftFront.setDirection(DcMotor.Direction.REVERSE);
this.rightFront.setDirection(DcMotor.Direction.FORWARD);
this.leftRear.setDirection(DcMotor.Direction.REVERSE);
@ -329,8 +332,8 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr
this.setWeightedDrivePower(
new Pose2d(
gamepad1.left_stick_y * -1 * speedScale,
gamepad1.left_stick_x * -1 * speedScale,
gamepad1.left_stick_y * speedScale,
gamepad1.left_stick_x * speedScale,
-gamepad1.right_stick_x * turnScale
));
}

27
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TurnTest.java
View File

@ -1,8 +1,10 @@
package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
import com.arcrobotics.ftclib.gamepad.GamepadEx;
import com.arcrobotics.ftclib.gamepad.GamepadKeys;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
@ -11,10 +13,10 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
* This is a simple routine to test turning capabilities.
*/
@Config
@Disabled
@Autonomous(group = "drive")
public class TurnTest extends LinearOpMode {
public static double ANGLE = 90; // deg
public static double DISTANCE = 24;
@Override
public void runOpMode() throws InterruptedException {
@ -24,6 +26,25 @@ public class TurnTest extends LinearOpMode {
if (isStopRequested()) return;
drive.turn(Math.toRadians(ANGLE));
GamepadEx controller = new GamepadEx(this.gamepad1);
boolean zig = false;
while (opModeIsActive() && !isStopRequested()) {
controller.readButtons();
if (controller.wasJustPressed(GamepadKeys.Button.A)) {
drive.turn(Math.toRadians(ANGLE));
}
if (controller.wasJustPressed(GamepadKeys.Button.B)) {
TrajectoryBuilder builder = drive.trajectoryBuilder(drive.getPoseEstimate());
if (zig) {
zig = false;
builder.forward(DISTANCE);
} else {
zig = true;
builder.forward(-DISTANCE);
}
drive.followTrajectory(builder.build());
}
}
}
}