Merge branch 'bumblebee_dev' of https://github.com/IronEaglesRobotics/CenterStage into bumblebee_dev
# Conflicts: # TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java
This commit is contained in:
Thomas
commit
3f78d1ec9b
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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());
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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_MAX = 0.75;
|
||||
public static double CLAW_MIN = 0.9;
|
||||
public static double PICKUP_ARM_MIN = 0.185;
|
||||
public static double PICKUP_ARM_MAX = 0.76;
|
||||
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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<Integer> lastTrackingEncPositions = new ArrayList<>();
|
||||
List<Integer> 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
|
||||
));
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<Double> getWheelPositions() {
|
||||
return Arrays.asList(
|
||||
encoderTicksToInches(parallelEncoder.getCurrentPosition()) * X_MULTIPLIER,
|
||||
encoderTicksToInches(perpendicularEncoder.getCurrentPosition()) * Y_MULTIPLIER
|
||||
);
|
||||
}
|
||||
|
||||
@NonNull
|
||||
@Override
|
||||
public List<Double> 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
|
||||
);
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue