[AUTO] Roadrunner

MeepMeepTesting/.gitignore

@@ -0,0 +1 @@
+/build

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'
+}

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();
+    }
+}

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;
         }
     }
 }

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;

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();
     }
 }

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)));
+    }
+}

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);
+    }
+}

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();
+}