diff --git a/MeepMeepTesting/.gitignore b/MeepMeepTesting/.gitignore new file mode 100644 index 0000000..42afabf --- /dev/null +++ b/MeepMeepTesting/.gitignore @@ -0,0 +1 @@ +/build \ No newline at end of file diff --git a/MeepMeepTesting/build.gradle b/MeepMeepTesting/build.gradle new file mode 100644 index 0000000..1fb580a --- /dev/null +++ b/MeepMeepTesting/build.gradle @@ -0,0 +1,17 @@ +plugins { + id 'java-library' +} + +java { + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 +} + +repositories { + maven { url = 'https://jitpack.io' } + maven { url = 'https://maven.brott.dev/' } +} + +dependencies { + implementation 'com.github.rh-robotics:MeepMeep:v1.1.0' +} diff --git a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java new file mode 100644 index 0000000..df3e744 --- /dev/null +++ b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java @@ -0,0 +1,34 @@ +package com.example.meepmeeptesting; + +import com.acmerobotics.roadrunner.geometry.Pose2d; + +import org.rowlandhall.meepmeep.MeepMeep; +import org.rowlandhall.meepmeep.roadrunner.DefaultBotBuilder; +import org.rowlandhall.meepmeep.roadrunner.entity.RoadRunnerBotEntity; + +public class MeepMeepTesting { + public static void main(String[] args) { + MeepMeep meepMeep = new MeepMeep(800); + + RoadRunnerBotEntity myBot = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .followTrajectorySequence(drive -> drive.trajectorySequenceBuilder(new Pose2d(0, 0, 0)) + .forward(30) + .turn(Math.toRadians(90)) + .forward(30) + .turn(Math.toRadians(90)) + .forward(30) + .turn(Math.toRadians(90)) + .forward(30) + .turn(Math.toRadians(90)) + .build()); + + + meepMeep.setBackground(MeepMeep.Background.FIELD_INTOTHEDEEP_JUICE_DARK) + .setDarkMode(true) + .setBackgroundAlpha(0.95f) + .addEntity(myBot) + .start(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRobotController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRobotController.java index da48a67..291d5ec 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRobotController.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoRobotController.java @@ -1,23 +1,32 @@ package org.firstinspires.ftc.teamcode; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.Pose2d; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.subsystems.driveTrain.MecanumDriveSubsystem; -import org.firstinspires.ftc.teamcode.subsystems.driveTrain.commands.mecanumDrive.StrafeInAngleMecanumCommand; +import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive; +import org.firstinspires.ftc.teamcode.subsystems.claw.ClawSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.elevator.ElevatorSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.extender.ExtenderSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.intakejoint.IntakeRollJointSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.scorer.ScorerSubsystem; +import org.firstinspires.ftc.teamcode.trajectories.Trajectories; +import org.firstinspires.ftc.teamcode.util.DataLogger; import org.firstinspires.ftc.teamcode.subsystems.intakejoint.IntakePitchJointSubsystem; import org.firstinspires.ftc.teamcode.util.opModes.SympleCommandOpMode; public class AutoRobotController extends RobotControllerBase { - private final MecanumDriveSubsystem mecanumDriveSubsystem; - private final IntakePitchJointSubsystem intakePitchJointSubsystem; + private final SubsystemContainer subsystemContainer; + private final Trajectories trajectory; - public AutoRobotController(HardwareMap hMap, Telemetry telemetry, Gamepad driverController, Gamepad actionController, String logFilePrefix, boolean logData) { + public AutoRobotController(HardwareMap hMap, Telemetry telemetry, Gamepad driverController, Gamepad actionController, String logFilePrefix, boolean logData, Trajectories trajectory) { super(hMap, telemetry, driverController, actionController, logFilePrefix, logData); - this.mecanumDriveSubsystem = new MecanumDriveSubsystem(this.getHardwareMap(), this.getTelemetry(), this.getDataLogger()); - this.intakePitchJointSubsystem = new IntakePitchJointSubsystem(this.getHardwareMap(), this.getTelemetry(), this.getDataLogger()); + this.subsystemContainer = new SubsystemContainer(getHardwareMap(), getTelemetry(), getDataLogger(), trajectory.getStartingPose()); + this.trajectory = trajectory; } @Override @@ -37,8 +46,9 @@ public void initializeLoop() { @Override public void postInitialize() { - this.intakePitchJointSubsystem.moveToState(RobotConstants.IntakeJointConstants.JointPitchState.BASKET).schedule(); - new StrafeInAngleMecanumCommand(this.mecanumDriveSubsystem, 90, 0.4).schedule(); + this.subsystemContainer.intakeSubsystem.goToState(RobotConstants.IntakeConstants.IntakeState.CLOSE).schedule(); + this.subsystemContainer.intakePitchJointSubsystem.moveToState(RobotConstants.IntakeJointConstants.JointPitchState.BASKET).schedule(); + this.trajectory.followTrajectory(this.subsystemContainer); } @Override @@ -52,6 +62,8 @@ public void postRun() { } public static class Builder extends RobotControllerBase.Builder { + private Trajectories trajectory; + public Builder() { this.logFilePrefix = "Auto"; } @@ -62,9 +74,68 @@ public Builder initializeDefaults(SympleCommandOpMode opMode) { return this; } + public Builder setTrajectory(Trajectories trajectory) { + this.trajectory = trajectory; + return this; + } + @Override public AutoRobotController build() { - return new AutoRobotController(this.hardwareMap, this.telemetry, this.driverController, this.actionController, this.logFilePrefix, this.logData); + return new AutoRobotController(this.hardwareMap, this.telemetry, this.driverController, this.actionController, this.logFilePrefix, this.logData, trajectory); + } + } + + public static class SubsystemContainer { + private final MecanumDrive mecanumDriveSubsystem; + private final ClawSubsystem clawSubsystem; + private final ScorerSubsystem scorerSubsystem; + private final IntakeSubsystem intakeSubsystem; + private final ElevatorSubsystem elevatorSubsystem; + private final ExtenderSubsystem extenderSubsystem; + private final IntakeRollJointSubsystem intakeRollJointSubsystem; + private final IntakePitchJointSubsystem intakePitchJointSubsystem; + + private SubsystemContainer(HardwareMap hardwareMap, MultipleTelemetry telemetry, DataLogger dataLogger, Pose2d pose2d) { + this.mecanumDriveSubsystem = new MecanumDrive(hardwareMap, pose2d); + this.clawSubsystem = new ClawSubsystem(hardwareMap, telemetry, dataLogger); + this.scorerSubsystem = new ScorerSubsystem(hardwareMap, telemetry, dataLogger); + this.intakeSubsystem = new IntakeSubsystem(hardwareMap, telemetry, dataLogger); + this.elevatorSubsystem = new ElevatorSubsystem(hardwareMap, telemetry, dataLogger); + this.extenderSubsystem = new ExtenderSubsystem(hardwareMap, telemetry, dataLogger); + this.intakeRollJointSubsystem = new IntakeRollJointSubsystem(hardwareMap, telemetry, dataLogger); + this.intakePitchJointSubsystem = new IntakePitchJointSubsystem(hardwareMap, telemetry, dataLogger); + } + + public MecanumDrive getMecanumDriveSubsystem() { + return mecanumDriveSubsystem; + } + + public ClawSubsystem getClawSubsystem() { + return clawSubsystem; + } + + public ScorerSubsystem getScorerSubsystem() { + return scorerSubsystem; + } + + public IntakeSubsystem getIntakeSubsystem() { + return intakeSubsystem; + } + + public ElevatorSubsystem getElevatorSubsystem() { + return elevatorSubsystem; + } + + public ExtenderSubsystem getExtenderSubsystem() { + return extenderSubsystem; + } + + public IntakeRollJointSubsystem getIntakeRollJointSubsystem() { + return intakeRollJointSubsystem; + } + + public IntakePitchJointSubsystem getIntakePitchJointSubsystem() { + return intakePitchJointSubsystem; } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java index 22ccb62..a6a5f7e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java @@ -19,6 +19,8 @@ public static class DriveConstants { public static final double METERS_PER_REV = (Math.PI * 2) * WHEEL_RADIUS; public static final double METERS_PER_TICK = (METERS_PER_REV / (TICKS_PER_REV * GEAR_RATIO)); + public static final double MOTOR_POWER_FIX = 0.08; + public static final double Ks = 0; public static final RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIRECTION = RevHubOrientationOnRobot.LogoFacingDirection.LEFT; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opMode/auto/ParkAutoOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opMode/auto/ParkAutoOpMode.java index 80d5993..7072c1a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opMode/auto/ParkAutoOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opMode/auto/ParkAutoOpMode.java @@ -3,6 +3,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.AutoRobotController; +import org.firstinspires.ftc.teamcode.trajectories.Trajectories; import org.firstinspires.ftc.teamcode.util.opModes.SympleCommandOpMode; @Autonomous(name = "Park auto", group = "auto") @@ -11,6 +12,7 @@ public class ParkAutoOpMode extends SympleCommandOpMode { public void initialize() { this.robotController = new AutoRobotController.Builder() .initializeDefaults(this) + .setTrajectory(Trajectories.CloseRed) .build(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opMode/teleOp/SympleTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opMode/teleOp/SympleTankDrive.java new file mode 100644 index 0000000..8fea318 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opMode/teleOp/SympleTankDrive.java @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.teamcode.opMode.teleOp; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.arcrobotics.ftclib.command.CommandOpMode; +import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.subsystems.driveTrain.TankDriveSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.driveTrain.commands.tankDriveBase.TankArcadeDriveCommand; +import org.firstinspires.ftc.teamcode.util.DataLogger; + +@TeleOp(name = "Symple Tank Drive", group = "test") +public class SympleTankDrive extends CommandOpMode { + private TankDriveSubsystem driveTrain; + + @Override + public void initialize() { + this.driveTrain = new TankDriveSubsystem(hardwareMap, new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()), new DataLogger("SympleTankDrive", false)); + this.driveTrain.setDefaultCommand(new TankArcadeDriveCommand(driveTrain, new GamepadEx(gamepad1))); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Drawing.java new file mode 100644 index 0000000..38114d8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Drawing.java @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.teamcode; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; + +public final class Drawing { + private Drawing() {} + + + public static void drawRobot(Canvas c, Pose2d t) { + final double ROBOT_RADIUS = 9; + + c.setStrokeWidth(1); + c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS); + + Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS); + Vector2d p1 = t.position.plus(halfv); + Vector2d p2 = p1.plus(halfv); + c.strokeLine(p1.x, p1.y, p2.x, p2.y); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Localizer.java new file mode 100644 index 0000000..7a19827 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/Localizer.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.teamcode.roadrunner; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.PoseVelocity2d; + +/** + * Interface for localization methods. + */ +public interface Localizer { + void setPose(Pose2d pose); + + /** + * Returns the current pose estimate. + * NOTE: Does not update the pose estimate; + * you must call update() to update the pose estimate. + * @return the Localizer's current pose + */ + Pose2d getPose(); + + /** + * Updates the Localizer's pose estimate. + * @return the Localizer's current velocity estimate + */ + PoseVelocity2d update(); +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/MecanumDrive.java new file mode 100644 index 0000000..a2b9aae --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/MecanumDrive.java @@ -0,0 +1,395 @@ +package org.firstinspires.ftc.teamcode.roadrunner; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.*; +import com.acmerobotics.roadrunner.AngularVelConstraint; +import com.acmerobotics.roadrunner.DualNum; +import com.acmerobotics.roadrunner.HolonomicController; +import com.acmerobotics.roadrunner.MecanumKinematics; +import com.acmerobotics.roadrunner.MinVelConstraint; +import com.acmerobotics.roadrunner.MotorFeedforward; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Pose2dDual; +import com.acmerobotics.roadrunner.ProfileAccelConstraint; +import com.acmerobotics.roadrunner.Time; +import com.acmerobotics.roadrunner.TimeTrajectory; +import com.acmerobotics.roadrunner.TimeTurn; +import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.acmerobotics.roadrunner.TurnConstraints; +import com.acmerobotics.roadrunner.VelConstraint; +import com.acmerobotics.roadrunner.ftc.DownsampledWriter; +import com.acmerobotics.roadrunner.ftc.FlightRecorder; +import com.acmerobotics.roadrunner.ftc.LazyImu; +import com.acmerobotics.roadrunner.ftc.LynxFirmware; +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.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.VoltageSensor; + +import org.firstinspires.ftc.teamcode.Drawing; +import org.firstinspires.ftc.teamcode.RobotConstants; +import org.firstinspires.ftc.teamcode.maps.MotorMap; +import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage; +import org.firstinspires.ftc.teamcode.messages.MecanumCommandMessage; +import org.firstinspires.ftc.teamcode.messages.PoseMessage; +import org.firstinspires.ftc.teamcode.util.MathUtil; + +import java.lang.Math; +import java.util.Arrays; +import java.util.LinkedList; +import java.util.List; + +@Config +public final class MecanumDrive { + public static class Params { + // drive model parameters + public double inPerTick = 0.0014855715871254162; + public double lateralInPerTick = 0.0010132515332920253; + public double trackWidthTicks = 5083.39859898591; + + // feedforward parameters (in tick units) + public double kS = 1.0574569483193246; + public double kV = 0.0002034917399979932; + public double kA = 0.0001; + + // path profile parameters (in inches) + public double maxWheelVel = 50; + public double minProfileAccel = -30; + public double maxProfileAccel = 50; + + // turn profile parameters (in radians) + public double maxAngVel = Math.PI; // shared with path + public double maxAngAccel = Math.PI; + + // path controller gains + public double axialGain = 1.7474412016; + public double lateralGain = 8; + public double headingGain = 21; // shared with turn + + public double axialVelGain = 2.93211; + public double lateralVelGain = 1; + public double headingVelGain = 4; // shared with turn + } + + public static Params PARAMS = new Params(); + + public final MecanumKinematics kinematics = new MecanumKinematics( + PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick); + + public final TurnConstraints defaultTurnConstraints = new TurnConstraints( + PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel); + public final VelConstraint defaultVelConstraint = + new MinVelConstraint(Arrays.asList( + kinematics.new WheelVelConstraint(PARAMS.maxWheelVel), + new AngularVelConstraint(PARAMS.maxAngVel) + )); + public final AccelConstraint defaultAccelConstraint = + new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel); + + public final DcMotorEx leftFront, leftBack, rightBack, rightFront; + + public final VoltageSensor voltageSensor; + + public final LazyImu lazyImu; + + public final Localizer localizer; + private final LinkedList poseHistory = new LinkedList<>(); + + private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000); + private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000); + private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000); + private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000); + + public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) { + LynxFirmware.throwIfModulesAreOutdated(hardwareMap); + + for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { + module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } + + // TODO: make sure your config has motors with these names (or change them) + // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html + leftFront = hardwareMap.get(DcMotorEx.class, MotorMap.LEG_FRONT_LEFT.getId()); + leftBack = hardwareMap.get(DcMotorEx.class, MotorMap.LEG_BACK_LEFT.getId()); + rightBack = hardwareMap.get(DcMotorEx.class, MotorMap.LEG_BACK_RIGHT.getId()); + rightFront = hardwareMap.get(DcMotorEx.class, MotorMap.LEG_FRONT_RIGHT.getId()); + + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // TODO: reverse motor directions if needed + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + leftBack.setDirection(DcMotorSimple.Direction.REVERSE); + + // TODO: make sure your config has an IMU with this name (can be BNO or BHI) + // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html + lazyImu = new LazyImu(hardwareMap, "imu", new RevHubOrientationOnRobot( + RobotConstants.DriveConstants.LOGO_FACING_DIRECTION, RobotConstants.DriveConstants.USB_FACING_DIRECTION)); + + voltageSensor = hardwareMap.voltageSensor.iterator().next(); + + localizer = new ThreeDeadWheelLocalizer(hardwareMap, MathUtil.metersToInch(RobotConstants.DriveConstants.METERS_PER_TICK), pose); + + FlightRecorder.write("MECANUM_PARAMS", PARAMS); + } + + public Pose2d getPose() { + return this.localizer.getPose(); + } + + public void setDrivePowers(PoseVelocity2d powers) { + MecanumKinematics.WheelVelocities