Scribd
Upload
9 views6 pages

Message

The document contains a Java class for controlling a robot in a TeleOp mode using the FTC SDK. It includes methods for initializing motors and servos, controlling their positions, and handling input from gamepads to manage robot movements and functions. The code is structured to allow for various operational modes and adjustments based on user input during the robot's operation.

Uploaded by

lucasallen624
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
9 views6 pages

Message

The document contains a Java class for controlling a robot in a TeleOp mode using the FTC SDK. It includes methods for initializing motors and servos, controlling their positions, and handling input from gamepads to manage robot movements and functions. The code is structured to allow for various operational modes and adjustments based on user input during the robot's operation.

Uploaded by

lucasallen624
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/ 6

package org.firstinspires.ftc.

teamcode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;

@TeleOp(name = "sasha's tip control", group = "Linear OpMode")


public class KingstonDriver extends LinearOpMode {
private final ElapsedTime runtime = new ElapsedTime();
private DcMotor LF, LB, RF, RB;
private DcMotor slideLeft, slideRight, verticalSlide;
private Servo pitchServo, yawServo, tipServo;
private Servo hYaw, hPitch, hR, hL;

private void homePos(){


hPitch.setPosition(0.05);
hYaw.setPosition(0.05);
pitchServo.setPosition(0.5);
yawServo.setPosition(0.7);

hR.setPosition(0.6);
hL.setPosition(0.475);
}

private void onlyHorZero(){


slideLeft.setTargetPosition(0);
slideLeft.setPower(1);
slideLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
slideRight.setTargetPosition(0);
slideRight.setPower(1);
slideRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}

private void onlyVerZero(){


verticalSlide.setTargetPosition(0);
verticalSlide.setPower(1);
verticalSlide.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
private void zero_slides(){
onlyHorZero();
onlyVerZero();
}

private void initializeAll() {


slideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
slideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
verticalSlide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);

// Ensure motors hold position when power is zero


slideRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
slideLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
verticalSlide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

homePos();
zero_slides();
//reset everything
}

private void initIntakePos(){


pitchServo.setPosition(0.4);
yawServo.setPosition(0.7);
tipServo.setPosition(0.5);
}

@Override
public void runOpMode() {
// wheels
LF = hardwareMap.get(DcMotor.class, "leftFrontDrive");
LB = hardwareMap.get(DcMotor.class, "leftBackDrive");
RF = hardwareMap.get(DcMotor.class, "rightFrontDrive");
RB = hardwareMap.get(DcMotor.class, "rightBackDrive");

// slides
verticalSlide = hardwareMap.get(DcMotor.class, "verticalSlide");
slideLeft = hardwareMap.get(DcMotor.class, "slideLeft");
slideRight = hardwareMap.get(DcMotor.class, "slideRight");

// servos
pitchServo = hardwareMap.get(Servo.class, "pitchIntake");
yawServo = hardwareMap.get(Servo.class, "yawIntake");
tipServo = hardwareMap.get(Servo.class, "tipIntake");

hYaw = hardwareMap.get(Servo.class, "hYaw");


hPitch = hardwareMap.get(Servo.class, "hPitch");
hR = hardwareMap.get(Servo.class, "hR");
hL = hardwareMap.get(Servo.class, "hL");

LF.setDirection(DcMotor.Direction.REVERSE);
LB.setDirection(DcMotor.Direction.REVERSE);
RF.setDirection(DcMotor.Direction.FORWARD);
RB.setDirection(DcMotor.Direction.FORWARD);
verticalSlide.setDirection(DcMotor.Direction.REVERSE);

slideLeft.setDirection(DcMotorSimple.Direction.REVERSE);
slideRight.setDirection(DcMotorSimple.Direction.FORWARD);

initializeAll();

telemetry.addData("Status", "Initialized");
telemetry.update();

waitForStart();
runtime.reset();

while (opModeIsActive()) {
double max;
double axial = -gamepad1.left_stick_y;
double lateral = gamepad1.left_stick_x;
double yaw = gamepad1.right_stick_x;

double LFpw = axial + lateral + yaw;


double RFpw = axial - lateral - yaw;
double LBpw = axial - lateral + yaw;
double RBpw = axial + lateral - yaw;

max = Math.max(Math.abs(LFpw), Math.abs(RFpw));


max = Math.max(max, Math.abs(LBpw));
max = Math.max(max, Math.abs(RBpw));

if (max > 1.0) {


LFpw /= max;
RFpw /= max;
LBpw /= max;
RBpw /= max;
}

//GAMEPAD
1-----------------------------------------------------------------------------

double speed_factor=0.5;

if (gamepad1.right_bumper) {
speed_factor = 1;
}
if(gamepad1.left_bumper){
speed_factor = 0.2;
}

//slide manual control


float slideOut=gamepad1.right_trigger;
float slideIn=gamepad1.left_trigger;
if(gamepad1.right_trigger>=0.2){
slideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
slideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
slideLeft.setPower(0.5);
slideRight.setPower(0.5);
}
if(gamepad1.left_trigger>=0.2){
slideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
slideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
slideLeft.setPower(-0.5);
slideRight.setPower(-0.5);
// slideLeft.setTargetPosition(0);
// slideLeft.setPower(0.7);
// slideLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// slideRight.setTargetPosition(0);
// slideRight.setPower(0.7);
// slideRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
else if(slideOut<0.2&&slideIn<0.2){
slideLeft.setPower(0);
slideRight.setPower(0);
slideLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
slideRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}

// bring slide in to handoff pos


if (gamepad1.dpad_down) {
slideLeft.setTargetPosition(325);
slideLeft.setPower(0.7);
slideLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
slideRight.setTargetPosition(325);
slideRight.setPower(0.7);
slideRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
sleep(250);
pitchServo.setPosition(0.96);
yawServo.setPosition(0.7);
tipServo.setPosition(0.15);
}
//commit grab
if (gamepad1.x) {
pitchServo.setPosition(0.25);
sleep(500);
tipServo.setPosition(0.15);
}

if(gamepad1.b){
tipServo.setPosition(0.5);
}

if (gamepad1.a) {
hR.setPosition(0.3);
hL.setPosition(0.65);
}

//intake yaw control 1 and 2


if (gamepad1.dpad_right) {
pitchServo.setPosition(0.275);
yawServo.setPosition(0.7);
tipServo.setPosition(0.5);
}
if (gamepad1.dpad_left) {
pitchServo.setPosition(0.275);
yawServo.setPosition(0.35);
tipServo.setPosition(0.5);
}

if(gamepad1.y){
initIntakePos();
}

//GAMEPAD
2-----------------------------------------------------------------------------
//handoff sequence and get position for basket
if (gamepad2.dpad_up) {
hR.setPosition(0.3);
hL.setPosition(0.65);
sleep(250);
tipServo.setPosition(0.5);
sleep(250);
hPitch.setPosition(0.85);
verticalSlide.setTargetPosition(3000);
verticalSlide.setPower(1);
verticalSlide.setMode(DcMotor.RunMode.RUN_TO_POSITION);
sleep(2000);
}

if (gamepad2.dpad_down) {
slideLeft.setTargetPosition(1000);
slideLeft.setPower(0.7);
slideLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
slideRight.setTargetPosition(1000);
slideRight.setPower(0.7);
slideRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}

if(gamepad2.b){
hR.setPosition(0.15);
hL.setPosition(0.8);
}

if(gamepad2.y){
homePos();
onlyVerZero();
}

if (gamepad2.a) {
hPitch.setPosition(0.9);
}

if (gamepad2.x) {
verticalSlide.setTargetPosition(100);
verticalSlide.setPower(1);
verticalSlide.setMode(DcMotor.RunMode.RUN_TO_POSITION);
hYaw.setPosition(0);
hPitch.setPosition(0.15);
hR.setPosition(0.6);
hL.setPosition(0.475);
sleep(100);
pitchServo.setPosition(1);
}

//ready to hang specimen


if(gamepad2.dpad_right){
verticalSlide.setTargetPosition(3000);
verticalSlide.setPower(1);
verticalSlide.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}

//commit hang
if(gamepad2.dpad_left){
verticalSlide.setTargetPosition(2500);
verticalSlide.setPower(0.8);
verticalSlide.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}

LF.setPower(LFpw * speed_factor);
RF.setPower(RFpw * speed_factor);
LB.setPower(LBpw * speed_factor);
RB.setPower(RBpw * speed_factor);

telemetry.addData("Status", "Run Time: " + runtime.toString());


telemetry.addData("Horizontal Slide Pos", "%7d, %7d",
slideLeft.getCurrentPosition(), slideRight.getCurrentPosition());
telemetry.addData("VertSlide Pos", "%7d",
verticalSlide.getCurrentPosition());
telemetry.update();
}
verticalSlide.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
verticalSlide.setPower(0.2);
}
}

You might also like