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

Drive works(?) and claw works

This commit is contained in:
Scott Barnes 2023-11-04 15:51:28 -05:00
parent 08d57fccff
commit 2961264246
10 changed files with 100 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -2,7 +2,9 @@ package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_DELTA; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_DELTA;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_LIFT_DELTA; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_LIFT_DELTA;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_SCREW_DELTA;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_X_DELTA; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_X_DELTA;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MAX;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@ -21,10 +23,12 @@ public class MainTeleOp extends OpMode {
private boolean previousScrewReset = false; private boolean previousScrewReset = false;
private boolean previousSlideUp = false; private boolean previousSlideUp = false;
private boolean previousSlideDown = false; private boolean previousSlideDown = false;
private double currentScrewPosition = 1f;
@Override @Override
public void init() { public void init() {
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
this.clawArmPosition = PICKUP_ARM_MAX;
this.robot = new Robot(this.hardwareMap); this.robot = new Robot(this.hardwareMap);
telemetry.addData("Status", "Initialized"); telemetry.addData("Status", "Initialized");
@ -33,10 +37,13 @@ public class MainTeleOp extends OpMode {
@Override @Override
public void loop() { public void loop() {
// Drive // Drive
double x = gamepad1.left_stick_x; double x = -gamepad1.left_stick_x;
double y = -gamepad1.left_stick_y; double y = -gamepad1.left_stick_y;
double z = gamepad1.right_stick_x; double z = -gamepad1.right_stick_x;
this.robot.getDrive().setInput(x, y, z); this.robot.getDrive().setInput(0, y, z);
this.telemetry.addLine(this.robot.getDrive().getTelemetry());
this.telemetry.update();
// Button Mappings // Button Mappings
boolean openClaw = gamepad2.b; // B boolean openClaw = gamepad2.b; // B
@ -57,23 +64,19 @@ public class MainTeleOp extends OpMode {
// Claw // Claw
if (openClaw) { if (openClaw) {
this.robot.getClaw().open(); this.robot.getClaw().open();
} else { } else if (!clawUp && !clawDown){
this.robot.getClaw().close(); this.robot.getClaw().close();
} }
if (clawUp) { if (clawUp) {
this.clawArmPosition += CLAW_ARM_DELTA; this.clawArmPosition = Math.min(1, this.clawArmPosition + CLAW_ARM_DELTA);
this.robot.getClaw().setArmPosition(clawArmPosition); this.robot.getClaw().setArmPosition(clawArmPosition);
} else if (clawDown) { } else if (clawDown) {
this.clawArmPosition -= CLAW_ARM_DELTA; this.robot.getClaw().open();
this.clawArmPosition = Math.max(0, this.clawArmPosition - CLAW_ARM_DELTA);
this.robot.getClaw().setArmPosition(clawArmPosition); this.robot.getClaw().setArmPosition(clawArmPosition);
} }
// Robot Lift /*
if (raiseRobotLift) {
this.robot.getLift().raise();
} else if (liftRobot) {
this.robot.getLift().lift();
}
// Gantry // Gantry
if (!previousScrewArmToggle && screwArmToggle) { if (!previousScrewArmToggle && screwArmToggle) {
@ -85,7 +88,8 @@ public class MainTeleOp extends OpMode {
screwArmPos = !screwArmPos; screwArmPos = !screwArmPos;
} }
if (!previousScrewDeposit && screwDeposit) { if (!previousScrewDeposit && screwDeposit) {
this.robot.getGantry().deposit(); this.currentScrewPosition += GANTRY_SCREW_DELTA;
this.robot.getGantry().setScrew(currentScrewPosition);
} else if (!previousScrewReset && screwReset) { } else if (!previousScrewReset && screwReset) {
this.robot.getGantry().resetScrew(); this.robot.getGantry().resetScrew();
} }
@ -99,10 +103,17 @@ public class MainTeleOp extends OpMode {
this.robot.getGantry().setX(this.robot.getGantry().getX() - GANTRY_X_DELTA); this.robot.getGantry().setX(this.robot.getGantry().getX() - GANTRY_X_DELTA);
} }
// Robot Lift
if (raiseRobotLift) {
this.robot.getLift().raise();
} else if (liftRobot) {
this.robot.getLift().lift();
}
*/
this.previousSlideUp = slideUp; this.previousSlideUp = slideUp;
this.previousScrewArmToggle = screwArmToggle; this.previousScrewArmToggle = screwArmToggle;
this.previousScrewDeposit = screwDeposit; this.previousScrewDeposit = screwDeposit;
this.previousScrewReset = screwReset; this.previousScrewReset = screwReset;
} }
} }

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

@ -1,7 +1,7 @@
package org.firstinspires.ftc.teamcode.hardware; package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.WEBCAM_NAME;
import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION; import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION;
import static org.firstinspires.ftc.teamcode.util.Constants.TARGETING_WEBCAM;
import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_HEIGHT; import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_HEIGHT;
import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_ROTATION; import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_ROTATION;
import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_WIDTH; import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_WIDTH;
@ -26,7 +26,7 @@ public class Camera {
public void initTargetingCamera() { public void initTargetingCamera() {
int targetingCameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); int targetingCameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
this.targetingCamera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, TARGETING_WEBCAM), targetingCameraMonitorViewId); this.targetingCamera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, WEBCAM_NAME), targetingCameraMonitorViewId);
this.targetingPipeline = new TargetingPipeline(); this.targetingPipeline = new TargetingPipeline();
targetingCamera.setPipeline(targetingPipeline); targetingCamera.setPipeline(targetingPipeline);
targetingCamera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { targetingCamera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java
View File

@ -21,6 +21,8 @@ public class Claw {
this.armLeft = hardwareMap.get(Servo.class, CLAW_ARM_LEFT_NAME); this.armLeft = hardwareMap.get(Servo.class, CLAW_ARM_LEFT_NAME);
this.armRight = hardwareMap.get(Servo.class, CLAW_ARM_RIGHT_NAME); this.armRight = hardwareMap.get(Servo.class, CLAW_ARM_RIGHT_NAME);
this.armRight.setDirection(Servo.Direction.REVERSE); this.armRight.setDirection(Servo.Direction.REVERSE);
this.setArmPosition(PICKUP_ARM_MAX);
this.close();
} }
public void open() { public void open() {
@ -40,6 +42,7 @@ public class Claw {
} }
public void setArmPosition(double target) { public void setArmPosition(double target) {
target = Math.min(PICKUP_ARM_MAX, Math.max(PICKUP_ARM_MIN, target));
this.armLeft.setPosition(target); this.armLeft.setPosition(target);
this.armRight.setPosition(target); this.armRight.setPosition(target);
} }

35
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java
View File

@ -1,26 +1,28 @@
package org.firstinspires.ftc.teamcode.hardware; package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_BACK_LEFT; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.BACK_LEFT_NAME;
import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_BACK_RIGHT; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.BACK_RIGHT_NAME;
import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_LEFT; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.FRONT_LEFT_NAME;
import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_RIGHT; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.FRONT_RIGHT_NAME;
import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.HardwareMap;
import java.util.Locale;
public class Drive { public class Drive {
private DcMotor frontLeft; private final DcMotor frontLeft;
private DcMotor frontRight; private final DcMotor frontRight;
private DcMotor backLeft; private final DcMotor backLeft;
private DcMotor backRight; private final DcMotor backRight;
public Drive(HardwareMap hardwareMap) { public Drive(HardwareMap hardwareMap) {
// Drive // Drive
this.frontLeft = hardwareMap.get(DcMotor.class, WHEEL_FRONT_LEFT); this.frontLeft = hardwareMap.get(DcMotor.class, FRONT_LEFT_NAME);
this.frontRight = hardwareMap.get(DcMotor.class, WHEEL_FRONT_RIGHT); this.frontRight = hardwareMap.get(DcMotor.class, FRONT_RIGHT_NAME);
this.backLeft = hardwareMap.get(DcMotor.class, WHEEL_BACK_LEFT); this.backLeft = hardwareMap.get(DcMotor.class, BACK_LEFT_NAME);
this.backRight = hardwareMap.get(DcMotor.class, WHEEL_BACK_RIGHT); this.backRight = hardwareMap.get(DcMotor.class, BACK_RIGHT_NAME);
this.frontLeft.setDirection(DcMotor.Direction.REVERSE); this.frontLeft.setDirection(DcMotor.Direction.REVERSE);
this.frontRight.setDirection(DcMotor.Direction.FORWARD); this.frontRight.setDirection(DcMotor.Direction.FORWARD);
this.backLeft.setDirection(DcMotor.Direction.REVERSE); this.backLeft.setDirection(DcMotor.Direction.REVERSE);
@ -55,6 +57,15 @@ public class Drive {
backRight.setPower(brPower); backRight.setPower(brPower);
} }
public String getTelemetry() {
double flPower = this.frontLeft.getPower();
double frPower = this.frontRight.getPower();
double blPower = this.backLeft.getPower();
double brPower = this.backRight.getPower();
return String.format("FL: %f, FR: %f, BL: %f, BR: %f", flPower, frPower, blPower, brPower);
}
public void setInput(Gamepad gamepad1, Gamepad gamepad2) { public void setInput(Gamepad gamepad1, Gamepad gamepad2) {
double x = gamepad1.left_stick_x; double x = gamepad1.left_stick_x;
double y = -gamepad1.left_stick_y; double y = -gamepad1.left_stick_y;

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

@ -22,6 +22,7 @@ public class Gantry extends Slide {
this.xServo = hardwareMap.get(Servo.class, GANTRY_X_NAME); this.xServo = hardwareMap.get(Servo.class, GANTRY_X_NAME);
this.armServo = hardwareMap.get(Servo.class, GANTRY_ARM_NAME); this.armServo = hardwareMap.get(Servo.class, GANTRY_ARM_NAME);
this.screwServo = hardwareMap.get(Servo.class, GANTRY_SCREW_NAME); this.screwServo = hardwareMap.get(Servo.class, GANTRY_SCREW_NAME);
this.screwServo.setPosition(1);
} }
public void setX(double x) { public void setX(double x) {
@ -41,12 +42,12 @@ public class Gantry extends Slide {
} }
public void resetScrew() { public void resetScrew() {
this.currentScrewIndex = 0; this.screwServo.setPosition(0);
this.screwServo.setPosition(GANTRY_SCREW_POSITIONS[currentScrewIndex]);
} }
public void deposit() { public void setScrew(double target) {
this.screwServo.setPosition(GANTRY_SCREW_POSITIONS[this.currentScrewIndex--]); double clamped = Math.min(1, Math.max(0, target));
this.screwServo.setPosition(clamped);
} }
public void center() { public void center() {

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

@ -8,8 +8,11 @@ import lombok.Getter;
public class Robot { public class Robot {
// @Getter
// private MecanumDrive drive;
@Getter @Getter
private MecanumDrive drive; private Drive drive;
@Getter @Getter
private Gantry gantry; private Gantry gantry;
@ -25,9 +28,10 @@ public class Robot {
} }
private void init(HardwareMap hardwareMap) { private void init(HardwareMap hardwareMap) {
this.drive = new MecanumDrive(hardwareMap); // this.drive = new MecanumDrive(hardwareMap);
this.gantry = new Gantry(hardwareMap); this.drive = new Drive(hardwareMap);
// this.gantry = new Gantry(hardwareMap);
this.claw = new Claw(hardwareMap); this.claw = new Claw(hardwareMap);
this.lift = new RobotLift(hardwareMap); // this.lift = new RobotLift(hardwareMap);
} }
} }

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

@ -4,7 +4,12 @@ import com.acmerobotics.dashboard.config.Config;
@Config @Config
public class RobotConstants { public class RobotConstants {
public static final String SLIDE_MOTOR_NAME = "slide"; public static final String FRONT_LEFT_NAME = "frontLeft";
public static final String FRONT_RIGHT_NAME = "frontRight";
public static final String BACK_LEFT_NAME = "backLeft";
public static final String BACK_RIGHT_NAME = "backRight";
public static final String LEFT_SLIDE_MOTOR_NAME = "slideLeft";
public static final String RIGHT_SLIDE_MOTOR_NAME = "slideRight";
public static final String CLAW_ARM_LEFT_NAME = "clawArmLeft"; public static final String CLAW_ARM_LEFT_NAME = "clawArmLeft";
public static final String CLAW_ARM_RIGHT_NAME = "clawArmRight"; public static final String CLAW_ARM_RIGHT_NAME = "clawArmRight";
public static final String CLAW_NAME = "claw"; public static final String CLAW_NAME = "claw";
@ -13,16 +18,17 @@ public class RobotConstants {
public static final String GANTRY_SCREW_NAME = "screw"; public static final String GANTRY_SCREW_NAME = "screw";
public static final String ROBOT_LIFT_ROTATION_NAME = "liftRotation"; public static final String ROBOT_LIFT_ROTATION_NAME = "liftRotation";
public static final String ROBOT_LIFT_LIFT_NAME = "liftLift"; public static final String ROBOT_LIFT_LIFT_NAME = "liftLift";
public static final String WEBCAM_NAME = "webcam";
// Slide // Slide
public static int SLIDE_UP = 100; public static int SLIDE_UP = 100;
// Arm // Arm
public static double PICKUP_ARM_MIN = 0; public static double PICKUP_ARM_MIN = 0.19;
public static double PICKUP_ARM_MAX = 1; public static double PICKUP_ARM_MAX = 0.75;
public static double CLAW_MIN = 0; public static double CLAW_MIN = 0.9;
public static double CLAW_MAX = 1; public static double CLAW_MAX = 0.6;
public static double CLAW_ARM_DELTA = 0.05; public static double CLAW_ARM_DELTA = 0.005;
// Gantry // Gantry
public static double GANTRY_ARM_MIN = 0; public static double GANTRY_ARM_MIN = 0;
@ -31,6 +37,7 @@ public class RobotConstants {
public static int GANTRY_LIFT_DELTA = 50; public static int GANTRY_LIFT_DELTA = 50;
public static double GANTRY_X_DELTA = 0.01; public static double GANTRY_X_DELTA = 0.01;
public static double GANTRY_CENTER = 0.5; public static double GANTRY_CENTER = 0.5;
public static double GANTRY_SCREW_DELTA = 0.01;
// Robot Lift // Robot Lift
public static int LIFT_ROTATION_UP = 100; public static int LIFT_ROTATION_UP = 100;

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slide.java
View File

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

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/StandardTrackingWheelLocalizer.java
View File

@ -8,7 +8,7 @@ import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.util.Encoder; import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.Encoder;
import java.util.Arrays; import java.util.Arrays;
import java.util.List; import java.util.List;

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java
View File

@ -73,17 +73,6 @@ public final class LogFiles {
public double mecHeadingI = MecanumDrive.HEADING_PID.kI; public double mecHeadingI = MecanumDrive.HEADING_PID.kI;
public double mecHeadingD = MecanumDrive.HEADING_PID.kD; public double mecHeadingD = MecanumDrive.HEADING_PID.kD;
public double mecLateralMultiplier = MecanumDrive.LATERAL_MULTIPLIER; public double mecLateralMultiplier = MecanumDrive.LATERAL_MULTIPLIER;
public double tankAxialP = SampleTankDrive.AXIAL_PID.kP;
public double tankAxialI = SampleTankDrive.AXIAL_PID.kI;
public double tankAxialD = SampleTankDrive.AXIAL_PID.kD;
public double tankCrossTrackP = SampleTankDrive.CROSS_TRACK_PID.kP;
public double tankCrossTrackI = SampleTankDrive.CROSS_TRACK_PID.kI;
public double tankCrossTrackD = SampleTankDrive.CROSS_TRACK_PID.kD;
public double tankHeadingP = SampleTankDrive.HEADING_PID.kP;
public double tankHeadingI = SampleTankDrive.HEADING_PID.kI;
public double tankHeadingD = SampleTankDrive.HEADING_PID.kD;
public double trackingTicksPerRev = StandardTrackingWheelLocalizer.TICKS_PER_REV; public double trackingTicksPerRev = StandardTrackingWheelLocalizer.TICKS_PER_REV;
public double trackingWheelRadius = StandardTrackingWheelLocalizer.WHEEL_RADIUS; public double trackingWheelRadius = StandardTrackingWheelLocalizer.WHEEL_RADIUS;
public double trackingGearRatio = StandardTrackingWheelLocalizer.GEAR_RATIO; public double trackingGearRatio = StandardTrackingWheelLocalizer.GEAR_RATIO;