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

TeleOp started mostly. No buttons assigned.

This commit is contained in:
Scott Barnes 2023-11-04 12:19:49 -05:00
parent ed527be034
commit b5ff67b528
7 changed files with 158 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
TeamCode/src/main/java/opmodes/MainTeleOp.java
View File

@ -1,5 +1,8 @@
package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_DELTA;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_LIFT_DELTA;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
@ -9,8 +12,15 @@ import org.firstinspires.ftc.teamcode.hardware.Robot;
@TeleOp(name = "MainTeleOp", group = "Main")
public class MainTeleOp extends OpMode {
private Robot robot;
private double clawArmPosition = 0;
private boolean screwArmPos = false;
private boolean previousScrewArmToggle = false;
private boolean previousScrewDeposit = false;
private boolean previousScrewIntake = false;
private boolean previousScrewReset = false;
private boolean previousSlideUp = false;
private boolean previousSlideDown = false;
@Override
public void init() {
@ -20,27 +30,77 @@ public class MainTeleOp extends OpMode {
telemetry.addData("Status", "Initialized");
}
// Gamepad 2 - Arm Control
// RT - Hanger Raise
// RB - Hanger Lift Robot
// DPad Left/Right - Gantry Left/Right
// DPad Up/Down - Lift Up/Down
@Override
public void loop() {
// Drive
double x = gamepad1.left_stick_x;
double y = -gamepad1.left_stick_y;
double z = gamepad1.right_stick_x;
// Set the drive input
this.robot.getDrive().setInput(x, y, z);
// Pickup
// this.robot.getClaw().close();
// this.robot.getClaw().open();
// this.robot.getClaw().up();
// this.robot.getClaw().down();
// Claw
boolean openClaw = false;
boolean clawUp = false;
boolean clawDown = false;
if (openClaw) {
this.robot.getClaw().open();
} else {
this.robot.getClaw().close();
}
if (clawUp) {
this.clawArmPosition += CLAW_ARM_DELTA;
this.robot.getClaw().setArmPosition(clawArmPosition);
} else if (clawDown) {
this.clawArmPosition -= CLAW_ARM_DELTA;
this.robot.getClaw().setArmPosition(clawArmPosition);
}
// Robot Lift
boolean raiseRobotLift = false;
boolean liftRobot = false;
if (raiseRobotLift) {
this.robot.getLift().raise();
} else if (liftRobot) {
this.robot.getLift().lift();
}
// Gantry
// this.robot.getGantry().up();
// this.robot.getGantry().down();
// this.robot.getGantry().armIn();
// this.robot.getGantry().armOut();
// this.robot.getGantry().intake();
// this.robot.getGantry().deposit();
boolean screwArmToggle = false;
boolean screwDeposit = false;
boolean screwIntake = false;
boolean screwReset = false;
boolean slideUp = false;
boolean slideDown = false;
if (!previousScrewArmToggle && screwArmToggle) {
if (screwArmPos) {
this.robot.getGantry().armOut();
} else {
this.robot.getGantry().armIn();
}
screwArmPos = !screwArmPos;
}
if (!previousScrewDeposit && screwDeposit) {
this.robot.getGantry().deposit();
} else if (!previousScrewIntake && screwIntake) {
this.robot.getGantry().intake();
} else if (screwReset) {
this.robot.getGantry().resetScrew();
}
if ((!previousSlideUp && slideUp) || (!previousSlideDown && slideDown)) {
int currentPosition = this.robot.getGantry().getSlidePosition();
this.robot.getGantry().setTarget(currentPosition + GANTRY_LIFT_DELTA);
}
this.previousSlideUp = slideUp;
this.previousScrewArmToggle = screwArmToggle;
this.previousScrewDeposit = screwDeposit;
this.previousScrewIntake = screwIntake;
this.previousScrewReset = screwReset;
}
}

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java
View File

@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_MIN;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_CENTER;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_SCREW_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_SCREW_POSITIONS;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_X_NAME;
@ -10,9 +11,7 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_X_NA
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import lombok.Getter;
public class Gantry extends Lift {
public class Gantry extends Slide {
private final Servo xServo;
private final Servo armServo;
private final Servo screwServo;
@ -49,4 +48,8 @@ public class Gantry extends Lift {
public void deposit() {
this.screwServo.setPosition(GANTRY_SCREW_POSITIONS[this.currentScrewIndex--]);
}
public void center() {
this.setX(GANTRY_CENTER);
}
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -17,6 +17,9 @@ public class Robot {
@Getter
private Claw claw;
@Getter
private RobotLift lift;
public Robot(HardwareMap hardwareMap) {
this.init(hardwareMap);
}
@ -25,5 +28,6 @@ public class Robot {
this.drive = new MecanumDrive(hardwareMap);
this.gantry = new Gantry(hardwareMap);
this.claw = new Claw(hardwareMap);
this.lift = new RobotLift(hardwareMap);
}
}

15
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java
View File

@ -4,25 +4,34 @@ import com.acmerobotics.dashboard.config.Config;
@Config
public class RobotConstants {
public static final String LIFT_MOTOR_NAME = "lift";
public static final String SLIDE_MOTOR_NAME = "slide";
public static final String CLAW_ARM_LEFT_NAME = "clawArmLeft";
public static final String CLAW_ARM_RIGHT_NAME = "clawArmRight";
public static final String CLAW_NAME = "claw";
public static final String GANTRY_X_NAME = "gantryX";
public static final String GANTRY_ARM_NAME = "gantryArm";
public static final String GANTRY_SCREW_NAME = "screw";
public static final String ROBOT_LIFT_ROTATION_NAME = "liftRotation";
public static final String ROBOT_LIFT_LIFT_NAME = "liftLift";
// Lift
public static int LIFT_UP = 100;
// Slide
public static int SLIDE_UP = 100;
// Arm
public static double PICKUP_ARM_MIN = 0;
public static double PICKUP_ARM_MAX = 1;
public static double CLAW_MIN = 0;
public static double CLAW_MAX = 1;
public static double CLAW_ARM_DELTA = 0.05;
// Gantry
public static double GANTRY_ARM_MIN = 0;
public static double GANTRY_ARM_MAX = 0;
public static double[] GANTRY_SCREW_POSITIONS = new double[] { 0, 0.1, 0.2, 1.0 };
public static int GANTRY_LIFT_DELTA = 50;
public static double GANTRY_CENTER = 0.5;
// Robot Lift
public static int LIFT_ROTATION_UP = 100;
public static int LIFT_EXTEND_MAX = 100;
}

51
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotLift.java Normal file
View File

@ -0,0 +1,51 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_EXTEND_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_ROTATION_UP;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.ROBOT_LIFT_LIFT_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.ROBOT_LIFT_ROTATION_NAME;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
public class RobotLift {
private DcMotor rotation;
private DcMotor lift;
public RobotLift(HardwareMap hardwareMap) {
this.init(hardwareMap);
}
public void init(HardwareMap hardwareMap) {
this.rotation = hardwareMap.get(DcMotor.class, ROBOT_LIFT_ROTATION_NAME);
this.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.lift = hardwareMap.get(DcMotor.class, ROBOT_LIFT_LIFT_NAME);
this.lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}
public void raise() {
this.rotation.setTargetPosition(LIFT_ROTATION_UP);
this.lift.setTargetPosition(LIFT_EXTEND_MAX);
this.rotation.setPower(1);
this.lift.setPower(1);
}
public void lift() {
this.rotation.setTargetPosition(LIFT_ROTATION_UP);
this.lift.setTargetPosition(0);
this.lift.setPower(1);
this.rotation.setPower(1);
}
public void reset() {
this.rotation.setTargetPosition(0);
this.lift.setTargetPosition(0);
this.lift.setPower(0.25);
this.rotation.setPower(0.25);
}
}

25
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Lift.java → TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slide.java
View File

@ -1,33 +1,30 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_MOTOR_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_UP;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SLIDE_MOTOR_NAME;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import lombok.Getter;
public class Lift {
public class Slide {
protected final DcMotor lift;
protected Lift(HardwareMap hardwareMap) {
this.lift = hardwareMap.get(DcMotor.class, LIFT_MOTOR_NAME);
protected Slide(HardwareMap hardwareMap) {
this.lift = hardwareMap.get(DcMotor.class, SLIDE_MOTOR_NAME);
this.lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.lift.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
public void up() {
public void setTarget(int target) {
this.lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.lift.setTargetPosition(LIFT_UP);
}
public void down() {
this.lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.lift.setTargetPosition(0);
this.lift.setTargetPosition(target);
this.lift.setPower(1);
}
public void setInput(double x) {
this.lift.setPower(x);
}
public int getSlidePosition() {
return this.lift.getCurrentPosition();
}
}

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java
View File

@ -35,20 +35,4 @@ public class Constants {
public static final Point INVALID_POINT = new Point(Double.MIN_VALUE, Double.MIN_VALUE);
public static final double INVALID_AREA = -1;
public static final Detection INVALID_DETECTION = new Detection(new Size(0, 0), 0);
// Hardware Name Constants
public static final String WHEEL_FRONT_LEFT = "frontLeft";
public static final String WHEEL_FRONT_RIGHT = "frontRight";
public static final String WHEEL_BACK_LEFT = "backLeft";
public static final String WHEEL_BACK_RIGHT = "backRight";
public static final String ARM = "wobbler";
public static final String CLAW = "claw";
public static final String INTAKE = "intake";
public static final String INTAKE_SECONDARY = "secondary";
public static final String INTAKE_SHIELD = "shield";
public static final String SHOOTER = "wheel";
public static final String PUSHER = "pusher";
public static final String STACK_WEBCAM = "Stack Webcam";
public static final String TARGETING_WEBCAM = "Targeting Webcam";
public static final String IMU_SENSOR = "imu";
}