Scribd
Upload
3 views2 pages

Message

This document contains a Java class for a robot's teleoperated control using a field-centric drive system. It initializes the robot's motors and IMU, allowing for precise movement and orientation adjustments based on the robot's heading. The loop method processes input from gamepads to control the robot's movement and includes commented-out sections for additional functionalities like arm and claw control.

Uploaded by

Surayanarayana Raju Rskk
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
3 views2 pages

Message

This document contains a Java class for a robot's teleoperated control using a field-centric drive system. It initializes the robot's motors and IMU, allowing for precise movement and orientation adjustments based on the robot's heading. The loop method processes input from gamepads to control the robot's movement and includes commented-out sections for additional functionalities like arm and claw control.

Uploaded by

Surayanarayana Raju Rskk
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 2

package org.firstinspires.ftc.

teamcode;

import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.Servo;

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;

@TeleOp (name="New drivetrain")

public class Field_centric_Gobilda extends OpMode{


public DcMotor leftBack = null;
public DcMotor leftFront = null;
public DcMotor rightBack = null;
public DcMotor rightFront = null;
//public Servo ArmServo = null;
//public Servo ClawServo = null;
public IMU imu = null;
@Override
public void init() {
//MECANUM DRIVE MOTORS MAPPING
leftBack = hardwareMap.get(DcMotor.class, "leftBack");
leftFront = hardwareMap.get(DcMotor.class, "leftFront");
rightBack = hardwareMap.get(DcMotor.class, "rightBack");
rightFront = hardwareMap.get(DcMotor.class, "rightFront");

//LINEARSLIDE MOTOR

//ARMSERVO
//ArmServo = hardwareMap.get(Servo.class, "ArmServo");

//ClAW SERVO
//ClawServo = hardwareMap.get(Servo.class,"ClawServo");

//BRAKE FUNCTION
leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

//MECANUM DRIVE REVERSE


leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
leftBack.setDirection(DcMotorSimple.Direction.REVERSE);

//IMU LOCALIZATION
imu = hardwareMap.get(IMU.class, "imu");
IMU.Parameters parameters = new IMU.Parameters(new
RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.LEFT,
RevHubOrientationOnRobot.UsbFacingDirection.UP));
imu.initialize(parameters);
imu.resetYaw();
}
@Override
public void loop() {
double y = -gamepad1.left_stick_y; // Remember, Y stick value is reversed
double x = gamepad1.left_stick_x;
double rx = gamepad1.right_stick_x;

// Field Centric Drive Reset


if (gamepad1.dpad_down) {
imu.resetYaw();
}

double botHeading =
imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);

// Rotate the movement direction counter to the bots 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;

// MECANUM DRIVE POWER


rightFront.setPower(frontRightPower * 0.7);
leftFront.setPower(frontLeftPower * 0.7);
rightBack.setPower(backRightPower * 0.7);
leftBack.setPower(backLeftPower * 0.7);

//LINEAR SLIDE MOVEMENT

//ARM MOVEMENT
if (gamepad2.a){
//ArmServo.setPosition(0.2);
}
if (gamepad2.b){
//ArmServo.setPosition(0.5);
}
if (gamepad1.a){
//ClawServo.setPosition(0.2);
}
if (gamepad1.b){
//ClawServo.setPosition(0.8);
}

}
}

You might also like