forked from FIRST-Tech-Challenge/FtcRobotController
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request FIRST-Tech-Challenge#4 from narehhovsepyan/work/nh
Add field centric teleop
- Loading branch information
Showing
3 changed files
with
166 additions
and
44 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
79 changes: 79 additions & 0 deletions
79
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FieldCentricMecanumTeleOp.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,79 @@ | ||
| package org.firstinspires.ftc.teamcode; | ||
|
|
||
| import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; | ||
| import com.qualcomm.robotcore.hardware.IMU; | ||
| import com.qualcomm.robotcore.eventloop.opmode.TeleOp; | ||
| import com.qualcomm.robotcore.hardware.DcMotor; | ||
| import com.qualcomm.robotcore.hardware.DcMotorSimple; | ||
| import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; | ||
| import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; | ||
|
|
||
| @TeleOp(name="FieldCentricMecanum", group="Linear OpMode") | ||
| public class FieldCentricMecanumTeleOp extends LinearOpMode { | ||
| @Override | ||
| public void runOpMode() throws InterruptedException { | ||
| // Declare our motors | ||
| // Make sure your ID's match your configuration | ||
| DcMotor frontLeftMotor = hardwareMap.dcMotor.get("left_front_drive"); | ||
| DcMotor backLeftMotor = hardwareMap.dcMotor.get("left_back_drive"); | ||
| DcMotor frontRightMotor = hardwareMap.dcMotor.get("right_front_drive"); | ||
| DcMotor backRightMotor = hardwareMap.dcMotor.get("right_back_drive"); | ||
|
|
||
| // Reverse the right side motors. This may be wrong for your setup. | ||
| // If your robot moves backwards when commanded to go forwards, | ||
| // reverse the left side instead. | ||
| // See the note about this earlier on this page. | ||
| frontLeftMotor.setDirection(DcMotor.Direction.REVERSE); | ||
| backLeftMotor.setDirection(DcMotor.Direction.REVERSE); | ||
| frontRightMotor.setDirection(DcMotor.Direction.FORWARD); | ||
| backRightMotor.setDirection(DcMotor.Direction.FORWARD); | ||
|
|
||
| // Retrieve the IMU from the hardware map | ||
| IMU imu = hardwareMap.get(IMU.class, "imu"); | ||
| // Adjust the orientation parameters to match your robot | ||
| IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( | ||
| RevHubOrientationOnRobot.LogoFacingDirection.UP, | ||
| RevHubOrientationOnRobot.UsbFacingDirection.FORWARD)); | ||
| // Without this, the REV Hub's orientation is assumed to be logo up / USB forward | ||
| imu.initialize(parameters); | ||
|
|
||
| waitForStart(); | ||
|
|
||
| if (isStopRequested()) return; | ||
|
|
||
| while (opModeIsActive()) { | ||
| double y = -gamepad1.left_stick_y; // Remember, Y stick value is reversed | ||
| double x = gamepad1.left_stick_x; | ||
| double rx = gamepad1.right_stick_x; | ||
|
|
||
| // This button choice was made so that it is hard to hit on accident, | ||
| // it can be freely changed based on preference. | ||
| // The equivalent button is start on Xbox-style controllers. | ||
| if (gamepad1.options) { | ||
| imu.resetYaw(); | ||
| } | ||
|
|
||
| double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); | ||
|
|
||
| // Rotate the movement direction counter to the bot's rotation | ||
| double rotX = x * Math.cos(-botHeading) - y * Math.sin(-botHeading); | ||
| double rotY = x * Math.sin(-botHeading) + y * Math.cos(-botHeading); | ||
|
|
||
| rotX = rotX * 1.1; // Counteract imperfect strafing | ||
|
|
||
| // 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(rotY) + Math.abs(rotX) + Math.abs(rx), 1); | ||
| double frontLeftPower = (rotY + rotX + rx) / denominator; | ||
| double backLeftPower = (rotY - rotX + rx) / denominator; | ||
| double frontRightPower = (rotY - rotX - rx) / denominator; | ||
| double backRightPower = (rotY + rotX - rx) / denominator; | ||
|
|
||
| frontLeftMotor.setPower(frontLeftPower); | ||
| backLeftMotor.setPower(backLeftPower); | ||
| frontRightMotor.setPower(frontRightPower); | ||
| backRightMotor.setPower(backRightPower); | ||
| } | ||
| } | ||
| } |