diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index 24f7a9d..ef6ae0e 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/opmodes/MainTeleOp.java @@ -35,13 +35,8 @@ public class MainTeleOp extends OpMode { @Override public void loop() { // Drive - double x = -gamepad1.left_stick_x; - double y = -gamepad1.left_stick_y; - double z = -gamepad1.right_stick_x; - this.robot.getDrive().setInput(0, y, z); - - this.telemetry.addLine(this.robot.getDrive().getTelemetry()); - this.telemetry.update(); + boolean slowmode = gamepad1.right_bumper; + this.robot.getDrive().setInput(gamepad1, gamepad2, slowmode); // Button Mappings boolean openClaw = gamepad2.b; // B @@ -133,11 +128,12 @@ public class MainTeleOp extends OpMode { this.robot.getLift().stopReset(); } - this.robot.update(); this.previousSlideUp = slideUp; this.previousScrewArmToggle = screwArmToggle; this.previousRobotLiftReset = robotLiftReset; this.previousRobotLiftExtend = robotLiftExtend; + + this.robot.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java index e978035..b4112c1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.hardware; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_LEFT_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_KP; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MAX; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MIN; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_RIGHT_NAME; @@ -12,12 +13,12 @@ import com.arcrobotics.ftclib.controller.PController; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; -import org.firstinspires.ftc.robotcore.external.Telemetry; - public class Claw { private final Servo claw; private final Servo armLeft; private final Servo armRight; + PController clawController = new PController(CLAW_KP); + private double clawControllerTarget; public Claw(HardwareMap hardwareMap) { this.claw = hardwareMap.get(Servo.class, CLAW_NAME); @@ -29,11 +30,11 @@ public class Claw { } public void open() { - this.claw.setPosition(CLAW_MAX); + this.clawControllerTarget = CLAW_MAX; } public void close() { - this.claw.setPosition(CLAW_MIN); + this.clawControllerTarget = CLAW_MIN; } public void setArmPosition(double target) { @@ -41,4 +42,16 @@ public class Claw { this.armLeft.setPosition(target); this.armRight.setPosition(target); } + + public void update() { + this.clawController.setSetPoint(this.clawControllerTarget); + this.clawController.setTolerance(0.001); + this.clawController.setP(CLAW_KP); + + double output = 0; + if (!this.clawController.atSetPoint()) { + output = Math.max(-1 * CLAW_MAX, Math.min(CLAW_MAX, this.clawController.calculate(claw.getPosition()))); + this.claw.setPosition(this.claw.getPosition() + output); + } + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index caf6a7b..affa31e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.hardware; +import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -9,11 +10,8 @@ import lombok.Getter; public class Robot { -// @Getter -// private MecanumDrive drive; - @Getter - private Drive drive; + private MecanumDrive drive; @Getter private Gantry gantry; @@ -32,8 +30,7 @@ public class Robot { } private void init(HardwareMap hardwareMap) { -// this.drive = new MecanumDrive(hardwareMap); - this.drive = new Drive(hardwareMap); + this.drive = new MecanumDrive(hardwareMap); this.gantry = new Gantry(hardwareMap, telemetry); this.claw = new Claw(hardwareMap); this.lift = new RobotLift(hardwareMap, telemetry); @@ -43,5 +40,11 @@ public class Robot { 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()); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java index 1754fb2..73b7e4c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java @@ -19,34 +19,43 @@ public class RobotConstants { 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; + + // Drive + public static double SLOW_MODE_SPEED_PCT = 0.25; + public static double SLOW_MODE_TURN_PCT = 0.25; + public static double SPEED = 1f; + public static double TURN = 1f; // Slide public static int SLIDE_UP = 100; // Arm - public static double PICKUP_ARM_MIN = 0.17; + public static double PICKUP_ARM_MIN = 0.185; public static double PICKUP_ARM_MAX = 0.76; - public static double CLAW_MIN = 0.9; + public static double CLAW_MIN = 0.92; public static double CLAW_MAX = 0.6; - public static double CLAW_ARM_DELTA = 0.008; + public static double CLAW_ARM_DELTA = 0.03; // Gantry - public static double GANTRY_ARM_MIN = 0.43; - public static double GANTRY_ARM_MAX = 0.95; + public static double GANTRY_ARM_MIN = 0.435; + public static double GANTRY_ARM_MAX = 0.92; 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.03; + 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 GANTRY_ARM_DELTA_MAX = 0.0025; + public static double GANTRY_ARM_DELTA_MAX = 0.013; // 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.1; - public static double LIFT_ARM_KP = 0.038; + public static double LIFT_RETRACT_PCT = 0.3; + public static double LIFT_ARM_KP = 0.1; public static double LIFT_POWER = 1f; + public static double CLAW_KP = 0.3; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java index 4296787..775c2d4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java @@ -1,5 +1,13 @@ package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive; +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 static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SLOW_MODE_SPEED_PCT; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SLOW_MODE_TURN_PCT; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SPEED; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.TURN; import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_ACCEL; import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_ANG_ACCEL; import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_ANG_VEL; @@ -29,6 +37,7 @@ import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.PIDFCoefficients; @@ -40,15 +49,16 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.Tra import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceRunner; import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.LynxModuleUtil; + import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV; import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA; import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic; + import java.util.ArrayList; import java.util.Arrays; import java.util.List; - /* * Simple mecanum drive hardware implementation for REV hardware. */ @@ -99,10 +109,10 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive DriveConstants.LOGO_FACING_DIR, DriveConstants.USB_FACING_DIR)); imu.initialize(parameters); - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftFront = hardwareMap.get(DcMotorEx.class, FRONT_LEFT_NAME); + leftRear = hardwareMap.get(DcMotorEx.class, BACK_LEFT_NAME); + rightRear = hardwareMap.get(DcMotorEx.class, BACK_RIGHT_NAME); + rightFront = hardwareMap.get(DcMotorEx.class, FRONT_RIGHT_NAME); motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront); @@ -124,11 +134,16 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive // TODO: reverse any motors using DcMotor.setDirection() + this.leftFront.setDirection(DcMotor.Direction.REVERSE); + this.rightFront.setDirection(DcMotor.Direction.FORWARD); + this.leftRear.setDirection(DcMotor.Direction.REVERSE); + this.rightRear.setDirection(DcMotor.Direction.FORWARD); + List lastTrackingEncPositions = new ArrayList<>(); List lastTrackingEncVels = new ArrayList<>(); // TODO: if desired, use setLocalizer() to change the localization method - // setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels)); + setLocalizer(new TwoWheelTrackingLocalizer(hardwareMap, this)); trajectorySequenceRunner = new TrajectorySequenceRunner( follower, HEADING_PID, batteryVoltageSensor, @@ -309,9 +324,15 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive return new ProfileAccelerationConstraint(maxAccel); } - public void setInput(double x, double y, double z) { - Pose2d vel = new Pose2d(x, y, z); - setDrivePower(vel); - update(); + public void setInput(Gamepad gamepad1, Gamepad gamepad2, boolean slowmode) { + double speedScale = slowmode ? SLOW_MODE_SPEED_PCT : SPEED; + double turnScale = slowmode ? SLOW_MODE_TURN_PCT : TURN; + + this.setWeightedDrivePower( + new Pose2d( + gamepad1.left_stick_y * -1 * speedScale, + gamepad1.left_stick_x * -1 * speedScale, + -gamepad1.right_stick_x * turnScale + )); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java new file mode 100644 index 0000000..b3277de --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java @@ -0,0 +1,110 @@ +package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive; + +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PARALLEL_DRIVE_ODOMETRY; +import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PERPENDICULAR_DRIVE_ODOMETRY; + +import androidx.annotation.NonNull; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.localization.TwoTrackingWheelLocalizer; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.Encoder; + +import java.util.Arrays; +import java.util.List; + +/* + * Sample tracking wheel localizer implementation assuming the standard configuration: + * + * ^ + * | + * | ( x direction) + * | + * v + * <----( y direction )----> + + * (forward) + * /--------------\ + * | ____ | + * | ---- | <- Perpendicular Wheel + * | || | + * | || | <- Parallel Wheel + * | | + * | | + * \--------------/ + * + */ +public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer { + public static double TICKS_PER_REV = 2000; + 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 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 + // Parallel/Perpendicular to the forward axis + // Parallel wheel is parallel to the forward axis + // Perpendicular is perpendicular to the forward axis + private final Encoder parallelEncoder; + private final Encoder perpendicularEncoder; + + private final MecanumDrive drive; + + public TwoWheelTrackingLocalizer(HardwareMap hardwareMap, MecanumDrive drive) { + super(Arrays.asList( + new Pose2d(PARALLEL_X, PARALLEL_Y, 0), + new Pose2d(PERPENDICULAR_X, PERPENDICULAR_Y, Math.toRadians(90)) + )); + + this.drive = drive; + + parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, PARALLEL_DRIVE_ODOMETRY)); + 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); + } + + public static double encoderTicksToInches(double ticks) { + return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; + } + + @Override + public double getHeading() { + return drive.getRawExternalHeading(); + } + + @Override + public Double getHeadingVelocity() { + return drive.getExternalHeadingVelocity(); + } + + @NonNull + @Override + public List getWheelPositions() { + return Arrays.asList( + encoderTicksToInches(parallelEncoder.getCurrentPosition()) * X_MULTIPLIER, + encoderTicksToInches(perpendicularEncoder.getCurrentPosition()) * Y_MULTIPLIER + ); + } + + @NonNull + @Override + public List getWheelVelocities() { + // TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other + // competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a + // compensation method + + return Arrays.asList( + encoderTicksToInches(parallelEncoder.getCorrectedVelocity()) * X_MULTIPLIER, + encoderTicksToInches(perpendicularEncoder.getCorrectedVelocity()) * Y_MULTIPLIER + ); + } +} \ No newline at end of file