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

Merge branch 'bumblebee_dev' of https://github.com/IronEaglesRobotics/CenterStage into bumblebee_dev

This commit is contained in:
Thomas 2023-11-14 16:18:13 -06:00
parent 3f78d1ec9b dcdb25632f
commit 21f1a10bcb
9 changed files with 105 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -2,12 +2,15 @@ package opmodes;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_DELTA;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_MIN;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.hardware.Robot;
@TeleOp(name = "MainTeleOp", group = "Main")
@ -22,10 +25,11 @@ public class MainTeleOp extends OpMode {
private boolean previousRobotLiftExtend = false;
private boolean liftArmShouldBeUp = false;
private boolean screwArmIsMoving = false;
private Telemetry dashboard;
@Override
public void init() {
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
this.dashboard = FtcDashboard.getInstance().getTelemetry();
this.clawArmPosition = PICKUP_ARM_MAX;
this.robot = new Robot(this.hardwareMap, telemetry);
@ -35,18 +39,22 @@ public class MainTeleOp extends OpMode {
@Override
public void loop() {
// Drive
boolean slowmode = gamepad1.right_bumper;
boolean slowmode = gamepad1.right_bumper || gamepad1.y;
this.robot.getDrive().setInput(gamepad1, gamepad2, slowmode);
// Button Mappings
// Claw / Pickup
boolean openClaw = gamepad2.b; // B
boolean clawUp = gamepad2.y; // Y
boolean clawDown = gamepad2.a; // A
boolean clawDownSafe = gamepad2.dpad_down; // dpad-down
boolean clawDown = gamepad2.a || clawDownSafe; // A
// Robot Lift
boolean robotLiftRotation = gamepad2.right_trigger > 0.05; // RT
boolean robotLiftExtend = gamepad2.right_trigger > 0.5; // RT
boolean robotLiftReset = gamepad2.right_stick_button;
// Gantry
boolean screwArmToggle = gamepad2.x; // X
boolean screwDeposit = gamepad2.left_trigger > 0.25; // LT
boolean screwIntake = gamepad2.left_bumper; // LB
@ -68,7 +76,9 @@ public class MainTeleOp extends OpMode {
this.robot.getClaw().setArmPosition(clawArmPosition);
} else if (clawDown) {
this.screwArmIsMoving = false;
this.robot.getClaw().open();
if (!clawDownSafe) {
this.robot.getClaw().open();
}
this.clawArmPosition = Math.max(0, this.clawArmPosition - CLAW_ARM_DELTA);
this.robot.getClaw().setArmPosition(clawArmPosition);
}
@ -97,11 +107,13 @@ public class MainTeleOp extends OpMode {
// this.robot.getGantry().setTarget(currentPosition + GANTRY_LIFT_DELTA);
// }
//
// if (gantryLeft) {
// this.robot.getGantry().setX(this.robot.getGantry().getX() + GANTRY_X_DELTA);
// } else if (gantryRight) {
// this.robot.getGantry().setX(this.robot.getGantry().getX() - GANTRY_X_DELTA);
// }
if (gantryRight) {
this.robot.getGantry().setX(X_MIN);
} else if (gantryLeft) {
this.robot.getGantry().setX(X_MAX);
} else {
this.robot.getGantry().center();
}
// Robot Lift

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

@ -13,10 +13,13 @@ import com.arcrobotics.ftclib.controller.PController;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
public class Claw {
import org.firstinspires.ftc.robotcore.external.Telemetry;
public class Claw implements Updatable {
private final Servo claw;
private final Servo armLeft;
private final Servo armRight;
private Telemetry telemetry;
PController clawController = new PController(CLAW_KP);
private double clawControllerTarget;
@ -29,6 +32,11 @@ public class Claw {
this.close();
}
public Claw(HardwareMap hardwareMap, Telemetry telemetry) {
this(hardwareMap);
this.telemetry = telemetry;
}
public void open() {
this.clawControllerTarget = CLAW_MAX;
}

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

@ -5,33 +5,51 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_
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.X_CENTER;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_SCREW_NAME;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_X_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 static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_KP;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_MAX;
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_MIN;
import com.arcrobotics.ftclib.controller.PController;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.Telemetry;
public class Gantry extends Slide {
public class Gantry {
private final Servo xServo;
private final Servo armServo;
private final CRServo screwServo;
private final DcMotor liftLeft;
private final DcMotor right;
PController armController = new PController(GANTRY_ARM_KP);
private double armControllerTarget;
PController xController = new PController(X_KP);
private double xControllerTarget;
private Telemetry telemetry;
public Gantry(HardwareMap hardwareMap) {
super(hardwareMap);
this.xServo = hardwareMap.get(Servo.class, GANTRY_X_NAME);
this.armServo = hardwareMap.get(Servo.class, GANTRY_ARM_NAME);
this.screwServo = hardwareMap.get(CRServo.class, GANTRY_SCREW_NAME);
this.armServo.setPosition(GANTRY_ARM_MIN);
this.liftLeft = hardwareMap.get(DcMotor.class, LEFT_SLIDE_MOTOR_NAME);
this.liftLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.liftLeft.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);
this.xControllerTarget = X_MIN;
}
public Gantry(HardwareMap hardwareMap, Telemetry telemetry) {
@ -39,8 +57,19 @@ public class Gantry extends Slide {
this.telemetry = telemetry;
}
public void setX(double x) {
this.xServo.setPosition(x);
public void setSlideTarget(int target) {
this.liftLeft.setTargetPosition(target);
this.liftLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.liftLeft.setPower(1);
this.right.setTargetPosition(target);
this.right.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.right.setPower(1);
}
public void setX(double x)
{
this.xControllerTarget = Math.max(X_MIN, Math.min(x, X_MAX));
}
public double getX() {
@ -70,7 +99,7 @@ public class Gantry extends Slide {
}
public void center() {
this.setX(GANTRY_CENTER);
this.setX(X_CENTER);
}
public boolean isIn() {
@ -83,14 +112,20 @@ public class Gantry extends Slide {
this.armController.setTolerance(0.001);
this.armController.setP(GANTRY_ARM_KP);
double output = 0;
this.xController.setSetPoint(this.xControllerTarget);
this.xController.setTolerance(0.001);
this.xController.setP(X_KP);
double armOutput = 0;
if (!this.armController.atSetPoint()) {
output = Math.max(-1 * GANTRY_ARM_DELTA_MAX, Math.min(GANTRY_ARM_DELTA_MAX, this.armController.calculate(armServo.getPosition())));
this.armServo.setPosition(this.armServo.getPosition() + output);
armOutput = Math.max(-1 * GANTRY_ARM_DELTA_MAX, Math.min(GANTRY_ARM_DELTA_MAX, this.armController.calculate(armServo.getPosition())));
this.armServo.setPosition(this.armServo.getPosition() + armOutput);
}
this.telemetry.addData("Arm P Controller", output);
this.telemetry.addData("Arm P Setpoint", this.armControllerTarget);
this.telemetry.addData("Arm In", this.isIn());
double xOutput = 0;
if (!this.xController.atSetPoint()) {
xOutput = this.xController.calculate(this.xServo.getPosition());
this.xServo.setPosition(this.xServo.getPosition() + xOutput);
}
}
}

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

@ -1,6 +1,7 @@
package org.firstinspires.ftc.teamcode.hardware;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.Telemetry;
@ -22,7 +23,7 @@ public class Robot {
@Getter
private RobotLift lift;
private Telemetry telemetry;
private final Telemetry telemetry;
public Robot(HardwareMap hardwareMap, Telemetry telemetry) {
this.telemetry = telemetry;
@ -31,20 +32,18 @@ public class Robot {
private void init(HardwareMap hardwareMap) {
this.drive = new MecanumDrive(hardwareMap);
this.drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
this.gantry = new Gantry(hardwareMap, telemetry);
this.claw = new Claw(hardwareMap);
this.claw = new Claw(hardwareMap, telemetry);
this.lift = new RobotLift(hardwareMap, telemetry);
}
public void update() {
this.gantry.update();
this.lift.update();
this.telemetry.update();
this.drive.update();
this.claw.update();
// Pose2d pose = this.drive.getLocalizer().getPoseEstimate();
// this.telemetry.addData("x", pose.getX());
// this.telemetry.addData("y", pose.getY());
this.telemetry.update();
}
}

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

@ -13,14 +13,14 @@ public class RobotConstants {
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_X_NAME = "gantry_x";
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";
public static final String WEBCAM_NAME = "webcam";
public static final String PARALLEL_DRIVE_ODOMETRY = BACK_RIGHT_NAME;
public static final String PERPENDICULAR_DRIVE_ODOMETRY = FRONT_LEFT_NAME;
public static final String PERPENDICULAR_DRIVE_ODOMETRY = FRONT_RIGHT_NAME;
// Drive
public static double SLOW_MODE_SPEED_PCT = 0.25;
@ -29,31 +29,32 @@ public class RobotConstants {
public static double TURN = 1f;
// Slide
public static int SLIDE_UP = 100;
// Arm
public static double PICKUP_ARM_MIN = 0.185;
public static double PICKUP_ARM_MAX = 0.76;
public static double PICKUP_ARM_MAX = 0.755;
public static double CLAW_MIN = 0.92;
public static double CLAW_MAX = 0.6;
public static double CLAW_ARM_DELTA = 0.03;
// Gantry
public static double GANTRY_ARM_MIN = 0.435;
public static double GANTRY_ARM_MAX = 0.92;
public static double GANTRY_ARM_MAX = 0.94;
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_ARM_KP = 0.06;
public static double GANTRY_ARM_KI = 0f;
public static double GANTRY_ARM_KD = 0f;
public static double X_KP = 0.1;
public static double X_MAX = 0.84;
public static double X_MIN = 0.16;
public static double X_CENTER = 0.54;
public static double GANTRY_ARM_DELTA_MAX = 0.013;
public static int SLIDE_UP = 100;
// Robot Lift
public static double LIFT_ROTATION_UP = 0.4;
public static double LIFT_ROTATION_DOWN = 0;
public static int LIFT_EXTEND_MAX = 13100;
public static double LIFT_RETRACT_PCT = 0.3;
public static double LIFT_RETRACT_PCT = 0.4;
public static double LIFT_ARM_KP = 0.1;
public static double LIFT_POWER = 1f;

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

@ -15,7 +15,7 @@ import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.robotcore.external.Telemetry;
public class RobotLift {
public class RobotLift implements Updatable{
private Servo rotation;
private DcMotor lift;
PController armController = new PController(LIFT_ARM_KP);
@ -78,8 +78,6 @@ public class RobotLift {
this.armController.setSetPoint(this.armControllerTarget);
double output = this.armController.calculate(this.rotation.getPosition());
this.rotation.setPosition(this.rotation.getPosition() + output);
this.telemetry.addData("Lift P Controller", output);
}
public boolean isUp() {

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

@ -1,40 +0,0 @@
package org.firstinspires.ftc.teamcode.hardware;
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 left;
protected final DcMotor right;
protected Slide(HardwareMap hardwareMap) {
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.left.setTargetPosition(target);
this.left.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.left.setPower(1);
this.right.setTargetPosition(target);
this.right.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.right.setPower(1);
}
public void setInput(double x) {
}
public int getSlidePosition() {
return this.left.getCurrentPosition();
}
}

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Updatable.java Normal file
View File

@ -0,0 +1,5 @@
package org.firstinspires.ftc.teamcode.hardware;
public interface Updatable {
void update();
}

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java
View File

@ -41,11 +41,11 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer {
public static double WHEEL_RADIUS = 0.944882; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
public static double PARALLEL_X = 0.269685; // X is the up and down direction
public static double PARALLEL_Y = 4.409449; // Y is the strafe direction
public static double PARALLEL_X = -0.269685; // X is the up and down direction
public static double PARALLEL_Y = -4.409449; // Y is the strafe direction
public static double PERPENDICULAR_X = -0.6299213;
public static double PERPENDICULAR_Y = 0.1122047;
public static double PERPENDICULAR_X = 0.6299213;
public static double PERPENDICULAR_Y = -0.1122047;
public static double X_MULTIPLIER = 1; // Multiplier in the X direction
public static double Y_MULTIPLIER = 1; // Multiplier in the Y direction
@ -69,7 +69,7 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer {
perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, PERPENDICULAR_DRIVE_ODOMETRY));
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
parallelEncoder.setDirection(Encoder.Direction.REVERSE);
parallelEncoder.setDirection(Encoder.Direction.FORWARD);
}
public static double encoderTicksToInches(double ticks) {