COMITTINGGG

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

MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java

@@ -0,0 +1,30 @@
+package com.example.meepmeeptesting;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+
+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(10.53, -68.65, Math.toRadians(90.00)))
+                        .splineToConstantHeading(new Vector2d(9.59, -33.44), Math.toRadians(91.52))
+                        .lineTo(new Vector2d(46.85, -67.34))
+                        .build());
+
+
+
+        meepMeep.setBackground(MeepMeep.Background.FIELD_INTOTHEDEEP_JUICE_DARK)
+                .setDarkMode(true)
+                .setBackgroundAlpha(0.95f)
+                .addEntity(myBot)
+                .start();
+    }
+}

TeamCode/build.gradle

@@ -24,5 +24,12 @@ android {
 }
 
 dependencies {
+    implementation 'org.ftclib.ftclib:core:2.1.1'
     implementation project(':FtcRobotController')
-}
+    annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
+
+
+    implementation 'org.apache.commons:commons-math3:3.6.1'
+    implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
+    implementation 'com.acmerobotics.roadrunner:core:0.5.6'
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/RedObsSideStrafe.java

@@ -0,0 +1,53 @@
+package org.firstinspires.ftc.teamcode.auton;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.arcrobotics.ftclib.command.CommandOpMode;
+import com.arcrobotics.ftclib.hardware.motors.Motor;
+//
+//import org.firstinspires.ftc.teamcode.commands.ShooterCom;
+import org.firstinspires.ftc.teamcode.commands.TrajectoryFollowerCommand;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem;
+
+@Autonomous(name = "RedObsSide")
+public class RedObsSideStrafe extends CommandOpMode {
+    private MecanumDriveSubsystem drive;
+    private Motor frontLeft, frontRight, backLeft, backRight;
+
+    private Pose2d startPose = new Pose2d(12.2, -62.0, Math.toRadians(0.0));
+
+    @Override
+    public void initialize() {
+        frontLeft = new Motor(hardwareMap, "fL");
+        frontRight = new Motor(hardwareMap, "fR");
+        backLeft = new Motor(hardwareMap, "bL");
+        backRight = new Motor(hardwareMap, "bR");
+        frontLeft.motor.setDirection(DcMotor.Direction.FORWARD);
+        frontRight.motor.setDirection(DcMotor.Direction.FORWARD);
+        backLeft.motor.setDirection(DcMotor.Direction.FORWARD);
+        backRight.motor.setDirection(DcMotor.Direction.FORWARD);
+
+        drive = new MecanumDriveSubsystem(new SampleMecanumDrive(hardwareMap), true);
+
+
+//        Trajectory center = drive.trajectoryBuilder(new Pose2d(12.55, -64.31, Math.toRadians(90.00)))
+//                .back(28)
+//                .build();
+        Trajectory right = drive.trajectoryBuilder(new Pose2d(12.55, -64.31), Math.toRadians(90))
+                .strafeLeft(200)
+                .build();
+
+//        TrajectoryFollowerCommand autonomous = new TrajectoryFollowerCommand(drive, center);
+        TrajectoryFollowerCommand autonomous2 = new TrajectoryFollowerCommand(drive, right);
+
+        if(isStopRequested()){
+
+        }
+
+        schedule(autonomous2);
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/ClawCommand.java

@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.commands;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+
+public class ClawCommand extends CommandBase {
+    private ClawSubsystem subsystem;
+    private int closeDeg, openDeg;
+
+    public ClawCommand(ClawSubsystem sub, int close, int open){
+        subsystem = sub;
+        closeDeg = close;
+        openDeg = open;
+    }
+
+    @Override
+    public void initialize(){
+        subsystem.close(closeDeg);
+    }
+    public void end(boolean interrupted){
+        subsystem.open(openDeg);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/IntakeCommand.java

@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.commands;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+import org.firstinspires.ftc.teamcode.subsystems.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem;
+
+public class IntakeCommand extends CommandBase {
+    private IntakeSubsystem subsystem;
+    private int closeDeg, openDeg;
+    public IntakeCommand(IntakeSubsystem sub, int close, int open){
+        subsystem = sub;
+        closeDeg = close;
+        openDeg = open;
+    }
+
+    @Override
+    public void initialize(){
+        subsystem.angleDown(closeDeg);
+    }
+    public void end(boolean interrupted){
+        subsystem.angleUp(openDeg);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/TestCommand.java

@@ -0,0 +1,41 @@
+package org.firstinspires.ftc.teamcode.commands;
+
+import com.arcrobotics.ftclib.command.CommandBase;
+
+
+import org.firstinspires.ftc.teamcode.subsystems.TestSubsystem;
+
+import java.util.function.DoubleSupplier;
+
+public class TestCommand extends CommandBase {
+    private final TestSubsystem mecDrive;
+    private final DoubleSupplier m_strafe, m_forward, m_turn;
+    private double multiplier;
+
+
+    public TestCommand(TestSubsystem subsystem, DoubleSupplier strafe, DoubleSupplier forward, DoubleSupplier turn, double mult){
+        mecDrive = subsystem;
+        m_strafe = strafe;
+        m_forward = forward;
+        m_turn = turn;
+        multiplier = mult;
+
+        addRequirements(subsystem);
+    }
+    public TestCommand(TestSubsystem subsystem, DoubleSupplier strafe, DoubleSupplier forward, DoubleSupplier turn){
+        mecDrive = subsystem;
+        m_strafe = strafe;
+        m_forward = forward;
+        m_turn = turn;
+        multiplier = 1.0;
+
+        addRequirements(subsystem);
+    }
+
+    @Override
+    public void execute(){
+        mecDrive.drive(m_strafe.getAsDouble() * 0.9 * multiplier,
+                m_forward.getAsDouble() * 0.9 * multiplier,
+                m_turn.getAsDouble() * 0.88 * multiplier);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/commands/TrajectoryFollowerCommand.java

@@ -0,0 +1,43 @@
+package org.firstinspires.ftc.teamcode.commands;
+
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.arcrobotics.ftclib.command.CommandBase;
+
+
+import org.firstinspires.ftc.teamcode.subsystems.MecanumDriveSubsystem;
+
+public class TrajectoryFollowerCommand extends CommandBase {
+
+    private final MecanumDriveSubsystem drive;
+    private final Trajectory trajectory;
+
+    public TrajectoryFollowerCommand(MecanumDriveSubsystem drive, Trajectory trajectory) {
+        this.drive = drive;
+        this.trajectory = trajectory;
+
+        addRequirements(drive);
+    }
+
+    @Override
+    public void initialize() {
+        drive.followTrajectory(trajectory);
+    }
+
+    @Override
+    public void execute() {
+        drive.update();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        if (interrupted) {
+            drive.stop();
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Thread.currentThread().isInterrupted() || !drive.isBusy();
+    }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java

@@ -0,0 +1,126 @@
+package org.firstinspires.ftc.teamcode.drive;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+
+/*
+ * Constants shared between multiple drive types.
+ *
+ * Constants generated by LearnRoadRunner.com/drive-constants
+ *
+ * TODO: Tune or adjust the following constants to fit your robot. Note that the non-final
+ * fields may also be edited through the dashboard (connect to the robot's WiFi network and
+ * navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you
+ * adjust them in the dashboard; **config variable changes don't persist between app restarts**.
+ *
+ * These are not the only parameters; some are located in the localizer classes, drive base classes,
+ * and op modes themselves.
+ */
+@Config
+public class DriveConstants {
+
+    /*
+     * These are motor constants that should be listed online for your motors.
+     */
+    public static final double TICKS_PER_REV = 145.6;
+    public static final double MAX_RPM = 1150;
+
+    /*
+     * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders.
+     * Set this flag to false if drive encoders are not present and an alternative localization
+     * method is in use (e.g., tracking wheels).
+     *
+     * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients
+     * from DriveVelocityPIDTuner.
+     */
+    public static final boolean RUN_USING_ENCODER = false;
+    public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
+      getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
+
+    /*
+     * These are physical constants that can be determined from your robot (including the track
+     * width; it will be tune empirically later although a rough estimate is important). Users are
+     * free to chose whichever linear distance unit they would like so long as it is consistently
+     * used. The default values were selected with inches in mind. Road runner uses radians for
+     * angular distances although most angular parameters are wrapped in Math.toRadians() for
+     * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
+     */
+    public static double WHEEL_RADIUS = 1.8898; // in
+    public static double GEAR_RATIO = 0.5; // output (wheel) speed / input (motor) speed
+    public static double TRACK_WIDTH = 18; // in
+
+    /*
+     * These are the feedforward parameters used to model the drive motor behavior. If you are using
+     * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive
+     * motor encoders or have elected not to use them for velocity control, these values should be
+     * empirically tuned.
+     */
+    public static double kV = 1.0 / rpmToVelocity(MAX_RPM);
+    public static double kA = 0;
+    public static double kStatic = 0;
+
+    public static RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIR =
+            RevHubOrientationOnRobot.LogoFacingDirection.UP;
+    public static RevHubOrientationOnRobot.UsbFacingDirection USB_FACING_DIR =
+            RevHubOrientationOnRobot.UsbFacingDirection.RIGHT;
+
+    /*
+     * These values are used to generate the trajectories for you robot. To ensure proper operation,
+     * the constraints should never exceed ~80% of the robot's actual capabilities. While Road
+     * Runner is designed to enable faster autonomous motion, it is a good idea for testing to start
+     * small and gradually increase them later after everything is working. All distance units are
+     * inches.
+     */
+    /*
+     * Note from LearnRoadRunner.com:
+     * The velocity and acceleration constraints were calculated based on the following equation:
+     * ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85
+     * Resulting in 96.72332843878378 in/s.
+     * This is only 85% of the theoretical maximum velocity of the bot, following the recommendation above.
+     * This is capped at 85% because there are a number of variables that will prevent your bot from actually
+     * reaching this maximum velocity: voltage dropping over the game, bot weight, general mechanical inefficiencies, etc.
+     * However, you can push this higher yourself if you'd like. Perhaps raise it to 90-95% of the theoretically 
+     * max velocity. The theoretically maximum velocity is 113.7921511044515 in/s.
+     * Just make sure that your bot can actually reach this maximum velocity. Path following will be detrimentally
+     * affected if it is aiming for a velocity not actually possible.
+     * 
+     * The maximum acceleration is somewhat arbitrary and it is recommended that you tweak this yourself based on
+     * actual testing. Just set it at a reasonable value and keep increasing until your path following starts
+     * to degrade. As of now, it simply mirrors the velocity, resulting in 96.72332843878378 in/s/s
+     *
+     * Maximum Angular Velocity is calculated as: maximum velocity / trackWidth * (180 / Math.PI) but capped at 360°/s.
+     * You are free to raise this on your own if you would like. It is best determined through experimentation.
+     *
+     * WARNING: LearnRoadRunner.com's constant generator has capped the calculated recommended velocity at 90in/s.
+     * This message is showing because your gear ratio/motor RPM/etc. configuration, results in a recommended
+     * velocity (85% of max velocity) exceeding 90in/s.
+     * (Your recommended velocity was 96.72332843878378in/s)
+     * This is simply insanely fast for an FTC bot and chances are your bot cannot properly reach these speeds.
+     * 
+     * Just to be safe, LearnRoadRunner.com has arbitrarily limited your velocity to 90in/s.
+     * You are free to increase it yourself. If you do run into issues, please lower the maximum velocity.
+     * 
+     * A documented case of a similar error which served as an impetus for this reasoning can be found here:
+     * https://github.com/acmerobotics/road-runner-quickstart/issues/91
+     
+     */
+    public static double MAX_VEL = 90;
+    public static double MAX_ACCEL = 90;
+    public static double MAX_ANG_VEL = Math.toRadians(286.4788975654116);
+    public static double MAX_ANG_ACCEL = Math.toRadians(286.4788975654116);
+
+
+    public static double encoderTicksToInches(double ticks) {
+        return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
+    }
+
+    public static double rpmToVelocity(double rpm) {
+        return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0;
+    }
+
+    public static double getMotorVelocityF(double ticksPerSecond) {
+      // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
+      return 32767 / ticksPerSecond;
+    }
+}