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.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.PICKUP_ARM_MAX;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@ -21,10 +23,12 @@ public class MainTeleOp extends OpMode {
private boolean previousScrewReset = false;
private boolean previousSlideUp = false;
private boolean previousSlideDown = false;
private double currentScrewPosition = 1f;
@Override
public void init() {
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
this.clawArmPosition = PICKUP_ARM_MAX;
this.robot = new Robot(this.hardwareMap);
telemetry.addData("Status", "Initialized");
@ -33,10 +37,13 @@ public class MainTeleOp extends OpMode {
@Override
public void loop() {
// Drive
double x = gamepad1.left_stick_x;
double x = -gamepad1.left_stick_x;
double y = -gamepad1.left_stick_y;
double z = gamepad1.right_stick_x;
this.robot.getDrive().setInput(x, y, z);
double z = -gamepad1.right_stick_x;
this.robot.getDrive().setInput(0, y, z);
this.telemetry.addLine(this.robot.getDrive().getTelemetry());
this.telemetry.update();
// Button Mappings
boolean openClaw = gamepad2.b; // B
@ -57,23 +64,19 @@ public class MainTeleOp extends OpMode {
// Claw
if (openClaw) {
this.robot.getClaw().open();
} else {
} else if (!clawUp && !clawDown){
this.robot.getClaw().close();
}
if (clawUp) {
this.clawArmPosition += CLAW_ARM_DELTA;
this.clawArmPosition = Math.min(1, this.clawArmPosition + CLAW_ARM_DELTA);
this.robot.getClaw().setArmPosition(clawArmPosition);
} 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);
}
// Robot Lift
if (raiseRobotLift) {
this.robot.getLift().raise();
} else if (liftRobot) {
this.robot.getLift().lift();
}
/*
// Gantry
if (!previousScrewArmToggle && screwArmToggle) {
@ -85,7 +88,8 @@ public class MainTeleOp extends OpMode {
screwArmPos = !screwArmPos;
}
if (!previousScrewDeposit && screwDeposit) {
this.robot.getGantry().deposit();
this.currentScrewPosition += GANTRY_SCREW_DELTA;
this.robot.getGantry().setScrew(currentScrewPosition);
} else if (!previousScrewReset && screwReset) {
this.robot.getGantry().resetScrew();
}
@ -99,10 +103,17 @@ public class MainTeleOp extends OpMode {
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.previousScrewArmToggle = screwArmToggle;
this.previousScrewDeposit = screwDeposit;
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;
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.TARGETING_WEBCAM;
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_WIDTH;
@ -26,7 +26,7 @@ public class Camera {
public void initTargetingCamera() {
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();
targetingCamera.setPipeline(targetingPipeline);
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.armRight = hardwareMap.get(Servo.class, CLAW_ARM_RIGHT_NAME);
this.armRight.setDirection(Servo.Direction.REVERSE);
this.setArmPosition(PICKUP_ARM_MAX);
this.close();
}
public void open() {
@ -40,6 +42,7 @@ public class Claw {
}
public void setArmPosition(double target) {
target = Math.min(PICKUP_ARM_MAX, Math.max(PICKUP_ARM_MIN, target));
this.armLeft.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;
import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_BACK_LEFT;
import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_BACK_RIGHT;
import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_LEFT;
import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_RIGHT;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.BACK_LEFT_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.BACK_RIGHT_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.FRONT_LEFT_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.FRONT_RIGHT_NAME;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import java.util.Locale;
public class Drive {
private DcMotor frontLeft;
private DcMotor frontRight;
private DcMotor backLeft;
private DcMotor backRight;
private final DcMotor frontLeft;
private final DcMotor frontRight;
private final DcMotor backLeft;
private final DcMotor backRight;
public Drive(HardwareMap hardwareMap) {
// Drive
this.frontLeft = hardwareMap.get(DcMotor.class, WHEEL_FRONT_LEFT);
this.frontRight = hardwareMap.get(DcMotor.class, WHEEL_FRONT_RIGHT);
this.backLeft = hardwareMap.get(DcMotor.class, WHEEL_BACK_LEFT);
this.backRight = hardwareMap.get(DcMotor.class, WHEEL_BACK_RIGHT);
this.frontLeft = hardwareMap.get(DcMotor.class, FRONT_LEFT_NAME);
this.frontRight = hardwareMap.get(DcMotor.class, FRONT_RIGHT_NAME);
this.backLeft = hardwareMap.get(DcMotor.class, BACK_LEFT_NAME);
this.backRight = hardwareMap.get(DcMotor.class, BACK_RIGHT_NAME);
this.frontLeft.setDirection(DcMotor.Direction.REVERSE);
this.frontRight.setDirection(DcMotor.Direction.FORWARD);
this.backLeft.setDirection(DcMotor.Direction.REVERSE);
@ -55,6 +57,15 @@ public class Drive {
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) {
double x = gamepad1.left_stick_x;
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.armServo = hardwareMap.get(Servo.class, GANTRY_ARM_NAME);
this.screwServo = hardwareMap.get(Servo.class, GANTRY_SCREW_NAME);
this.screwServo.setPosition(1);
}
public void setX(double x) {
@ -41,12 +42,12 @@ public class Gantry extends Slide {
}
public void resetScrew() {
this.currentScrewIndex = 0;
this.screwServo.setPosition(GANTRY_SCREW_POSITIONS[currentScrewIndex]);
this.screwServo.setPosition(0);
}
public void deposit() {
this.screwServo.setPosition(GANTRY_SCREW_POSITIONS[this.currentScrewIndex--]);
public void setScrew(double target) {
double clamped = Math.min(1, Math.max(0, target));
this.screwServo.setPosition(clamped);
}
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 {
// @Getter
// private MecanumDrive drive;
@Getter
private MecanumDrive drive;
private Drive drive;
@Getter
private Gantry gantry;
@ -25,9 +28,10 @@ public class Robot {
}
private void init(HardwareMap hardwareMap) {
this.drive = new MecanumDrive(hardwareMap);
this.gantry = new Gantry(hardwareMap);
// this.drive = new MecanumDrive(hardwareMap);
this.drive = new Drive(hardwareMap);
// this.gantry = new Gantry(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
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_RIGHT_NAME = "clawArmRight";
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 ROBOT_LIFT_ROTATION_NAME = "liftRotation";
public static final String ROBOT_LIFT_LIFT_NAME = "liftLift";
public static final String WEBCAM_NAME = "webcam";
// 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;
public static double PICKUP_ARM_MIN = 0.19;
public static double PICKUP_ARM_MAX = 0.75;
public static double CLAW_MIN = 0.9;
public static double CLAW_MAX = 0.6;
public static double CLAW_ARM_DELTA = 0.005;
// Gantry
public static double GANTRY_ARM_MIN = 0;
@ -31,6 +37,7 @@ public class RobotConstants {
public static int GANTRY_LIFT_DELTA = 50;
public static double GANTRY_X_DELTA = 0.01;
public static double GANTRY_CENTER = 0.5;
public static double GANTRY_SCREW_DELTA = 0.01;
// Robot Lift
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;
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.HardwareMap;
public class Slide {
protected final DcMotor lift;
protected final DcMotor left;
protected final DcMotor right;
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);
this.left = hardwareMap.get(DcMotor.class, LEFT_SLIDE_MOTOR_NAME);
this.left.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
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) {
this.lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.lift.setTargetPosition(target);
this.lift.setPower(1);
this.left.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.left.setTargetPosition(target);
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) {
this.lift.setPower(x);
}
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.HardwareMap;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.Encoder;
import java.util.Arrays;
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 mecHeadingD = MecanumDrive.HEADING_PID.kD;
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 trackingWheelRadius = StandardTrackingWheelLocalizer.WHEEL_RADIUS;
public double trackingGearRatio = StandardTrackingWheelLocalizer.GEAR_RATIO;