SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Fine tuning of Teleop class (untested)

Browse Source
This commit is contained in:
Carlos Rivas
2024-10-31 11:03:00 -07:00
parent 3aed4b8676
commit 1c922f025e
2 changed files with 40 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometDrive.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDrive.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode.cometbots;
package org.firstinspires.ftc.teamcode;
import static org.firstinspires.ftc.teamcode.util.action.FieldConstants.blueBucketStartPose;
@ -9,9 +9,9 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
import org.firstinspires.ftc.teamcode.runmodes.Teleop;
@TeleOp(name="Drive", group="A")
@TeleOp(name="ComeBot Drive", group="Debug")
@Disabled
public class CometDrive extends OpMode {
public class CometBotDrive extends OpMode {
private Teleop teleop;
@ -20,12 +20,7 @@ public class CometDrive extends OpMode {
teleop = new Teleop(hardwareMap,
telemetry,
new Follower(hardwareMap),
blueBucketStartPose,
gamepad1);
}
@Override
public void start() {
teleop.start();
}

62
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/Teleop.java
View File

@ -1,5 +1,16 @@
package org.firstinspires.ftc.teamcode.runmodes;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR;
import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
@ -12,51 +23,36 @@ import org.firstinspires.ftc.teamcode.util.action.RunAction;
public class Teleop {
private ClawSubsystem claw;
private ClawSubsystem.ClawState clawState;
private Follower follower;
private Pose startPose;
private DcMotorEx leftFront;
private DcMotorEx leftRear;
private DcMotorEx rightFront;
private DcMotorEx rightRear;
private Telemetry telemetry;
private Gamepad gamepad1;
private Gamepad currentGamepad1;
private Gamepad previousGamepad1;
public RunAction stopDrive, startDrive;
public Teleop(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, Gamepad gamepad1) {
public double speed = 0.5;
public Teleop(HardwareMap hardwareMap, Telemetry telemetry, Follower follower, Pose startPose, Gamepad gamepad1) {
claw = new ClawSubsystem(hardwareMap, clawState);
claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
initMotors(hardwareMap);
this.follower = follower;
this.startPose = startPose;
this.telemetry = telemetry;
this.gamepad1 = gamepad1;
this.currentGamepad1 = new Gamepad();
this.previousGamepad1 = new Gamepad();
stopDrive = new RunAction(this::stopDrive);
startDrive = new RunAction(this::startDrive);
}
public void start() {
claw.start();
follower.setPose(startPose);
follower.startTeleopDrive();
}
private void startDrive() {
follower.startTeleopDrive();
}
private void stopDrive() {
follower.breakFollowing();
}
public void update() {
previousGamepad1.copy(currentGamepad1);
currentGamepad1.copy(gamepad1);
@ -64,14 +60,30 @@ public class Teleop {
if (currentGamepad1.a && !previousGamepad1.a)
claw.switchState();
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y * speed, -gamepad1.left_stick_x * speed, -gamepad1.right_stick_x * speed);
follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
follower.update();
telemetry.addData("X", follower.getPose().getX());
telemetry.addData("Y", follower.getPose().getY());
telemetry.addData("Heading", Math.toDegrees(follower.getPose().getHeading()));
telemetry.addData("Claw State", clawState);
telemetry.addData("Claw State", claw.getState());
telemetry.update();
}
private void initMotors(HardwareMap hardwareMap) {
rightFront = hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_MOTOR);
rightRear = hardwareMap.get(DcMotorEx.class, BACK_RIGHT_MOTOR);
leftFront = hardwareMap.get(DcMotorEx.class, FRONT_LEFT_MOTOR);
leftRear = hardwareMap.get(DcMotorEx.class, BACK_LEFT_MOTOR);
rightFront.setDirection(FRONT_RIGHT_MOTOR_DIRECTION);
rightRear.setDirection(BACK_RIGHT_MOTOR_DIRECTION);
leftFront.setDirection(FRONT_LEFT_MOTOR_DIRECTION);
leftRear.setDirection(BACK_LEFT_MOTOR_DIRECTION);
leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}
}