buggy code

will fix later

src/main/deploy/pathplanner/LowScoreTaxiBump.path

@@ -0,0 +1,49 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 1.92,
+        "y": 0.5
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0042032480370326,
+        "y": 0.5
+      },
+      "holonomicAngle": 180.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 6.371669585444434,
+        "y": 0.5
+      },
+      "prevControl": {
+        "x": 5.336015096499199,
+        "y": 0.5
+      },
+      "nextControl": null,
+      "holonomicAngle": 180.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "markers": []
+}

src/main/deploy/pathplanner/New Path.path

@@ -0,0 +1,49 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 3.599129905026728,
+        "y": 3.264765928028098
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.622136041599105,
+        "y": 3.264765928028098
+      },
+      "holonomicAngle": 180.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 6.341533284570328,
+        "y": 0.924179893472571
+      },
+      "prevControl": {
+        "x": 5.341533284570328,
+        "y": 0.924179893472571
+      },
+      "nextControl": null,
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "markers": []
+}

src/main/java/frc/robot/Robot.java

@@ -35,7 +35,7 @@ public class Robot extends TimedRobot {
     private final RobotState robot_state_ = new RobotState(drive_);
 
     // Superstructure
-    private final Superstructure superstructure_ = new Superstructure(arm_);
+    private final Superstructure superstructure_ = new Superstructure(arm_, intake_);
 
     // Xbox Controller
     private final CommandXboxController driver_controller_ = new CommandXboxController(0);

src/main/java/frc/robot/RobotState.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.subsystems.Drive;
 
 public class RobotState {
@@ -38,6 +39,13 @@ public double getDegree() {
         return pose_estimator_.getEstimatedPosition().getRotation().getDegrees();
     }
 
+    // Get Rotation2d
+    public Rotation2d getRotation2d() {
+        return pose_estimator_.getEstimatedPosition().getRotation();
+    }
+
     // Reset Position
-    public void reset(Pose2d pose) {}
+    public void reset(Pose2d pose) {
+        pose_estimator_.resetPosition(getRotation2d(), drive_.getSwerveModulePositions(), pose);
+    }
 }

src/main/java/frc/robot/Superstructure.java

@@ -5,16 +5,19 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.commands.ArmToPosition;
 import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.Intake;
 
 public class Superstructure {
     // Subsystems
     private final Arm arm_;
+    private final Intake intake_;
 
     // Position State
     public String state = "STOW";
 
-    public Superstructure(Arm arm) {
+    public Superstructure(Arm arm, Intake intake) {
         arm_ = arm;
+        intake_ = intake;
     }
 
 
@@ -31,6 +34,10 @@ public String getState() {
         return state;
     }
 
+    public Command setIntakePercent(double value) {
+        return new InstantCommand(() -> intake_.setPercent(value));
+    }
+
 
 
 

src/main/java/frc/robot/auto/ArmSimTest.java

@@ -1,17 +1,37 @@
 package frc.robot.auto;
 
+import com.pathplanner.lib.PathConstraints;
+import com.pathplanner.lib.PathPlanner;
+import com.pathplanner.lib.PathPlannerTrajectory;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Superstructure;
 import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.Drive;
 
 public class ArmSimTest extends SequentialCommandGroup{
+    // Trajectories
+    private final PathPlannerTrajectory traj1 = PathPlanner.loadPath("LowScoreTaxiBump", new PathConstraints(4, 3));
+
+    private final Drive drive_;
+    private final frc.robot.RobotState robot_state_;
+
+    // private final DriveTrajectory follow = new DriveTrajectory()
+
+
     // Constructor
-    public ArmSimTest(Superstructure superstructure, Arm arm) {
+    public ArmSimTest(Superstructure superstructure, Arm arm, Drive drive, frc.robot.RobotState robot_state) {
+        drive_ = drive;
+        robot_state_ = robot_state;
         addCommands(
             superstructure.setPosition(Superstructure.Position.STOW),
             new WaitCommand(1.0),
-            superstructure.setPosition(Superstructure.Position.CUBE_L2)
+            superstructure.setPosition(Superstructure.Position.CUBE_L1),
+            new WaitCommand(2.5),
+            superstructure.setIntakePercent(0.5),
+            new InstantCommand(() -> new DriveTrajectory(traj1).followTrajectoryCommand(true, robot_state_, drive_))
+
         );
     }
-}
+} 

src/main/java/frc/robot/auto/AutoSelector.java

@@ -1,18 +1,13 @@
 package frc.robot.auto;
 
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.RobotState;
-import frc.robot.Superstructure;
-import frc.robot.subsystems.Arm;
-
 public class AutoSelector {
     // private Command auto_command_ = null;
 
     public AutoSelector() {
         ;
     }
 
-    public Command run(RobotState robot_state, Superstructure superstructure, Arm arm) {
-        return new ArmSimTest(superstructure, arm);
-    }
+    // public Command run(RobotState robot_state, Superstructure superstructure, Arm arm) {
+    //     return new ArmSimTest(superstructure, arm);
+    // }
 }

src/main/java/frc/robot/auto/DriveTrajectory.java

@@ -0,0 +1,79 @@
+package frc.robot.auto;
+
+import com.pathplanner.lib.PathPlannerTrajectory;
+import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
+import com.pathplanner.lib.commands.PPSwerveControllerCommand;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.RobotState;
+import frc.robot.subsystems.Drive;
+
+public class DriveTrajectory extends CommandBase{
+    private final PathPlannerTrajectory traj_;
+    private final Timer timer_;
+    private double t;
+
+    public DriveTrajectory(PathPlannerTrajectory traj) {
+        traj_ = traj;
+        timer_ = new Timer();
+    }
+
+
+    @Override
+    public void initialize() {
+        timer_.start();
+    }
+
+    @Override
+    public void execute() {
+        t = timer_.get();
+    }
+
+    public Pose2d getTrajectoryPose(double time) {
+        final PathPlannerState state = (PathPlannerState) traj_.sample(t);
+        return new Pose2d(new Translation2d(state.poseMeters.getX(), state.poseMeters.getY()), state.holonomicRotation);
+    }
+
+    public Command followTrajectoryCommand(boolean isFirstTrajectory, RobotState robot_state_, Drive drive_) {
+        return new SequentialCommandGroup(
+            new InstantCommand(() -> {
+                if(isFirstTrajectory) {
+                    robot_state_.reset(traj_.getInitialHolonomicPose());
+                    SmartDashboard.putNumber("Trajectory Total Time", traj_.getTotalTimeSeconds());
+                }
+            }),
+            new PPSwerveControllerCommand(
+                traj_,
+                robot_state_::getPosition,
+                drive_.getKinematics(),
+                Constants.kXController,
+                Constants.kYController,
+                Constants.kThetaController,
+                drive_::setModuleStates,
+                true,
+                drive_
+            )
+        );
+    }
+
+    public double getTrajectoryTime() {
+        return traj_.getTotalTimeSeconds();
+    }
+
+    public static class Constants {
+        public static double kP = 0.5;
+
+        public static PIDController kXController = new PIDController(kP, 0, 0);
+        public static PIDController kYController = new PIDController(kP, 0, 0);
+        public static PIDController kThetaController = new PIDController(kP, 0, 0);
+    }
+
+}

src/main/java/frc/robot/subsystems/Arm.java

@@ -193,7 +193,7 @@ private static class Constants {
         public static final int kMotorId = 9;
 
         // Physical Constants
-        public static final double kGearRatio = 22.0 / 34.0;
+        public static final double kGearRatio = 49.0 * 34.0 / 22.0;
         public static final double kMinAngle = Math.toRadians(-40);
         public static final double kMaxAngle = Math.toRadians(170);
         public static final double kArmLength = 0.30;

src/main/java/frc/robot/subsystems/Drive.java

@@ -61,17 +61,7 @@ public void periodic() {
 
         // Calculate outputs
         SwerveModuleState[] module_states = kinematics_.toSwerveModuleStates(io_.speeds);
-        SwerveDriveKinematics.desaturateWheelSpeeds(module_states, Constants.kMaxSpeed);
-
-        // Set outputs and telemetry
-        for (int i = 0; i < modules_.length; i++) {
-            modules_[i].setDesiredState(module_states[i], output_type_);
-
-            SmartDashboard.putNumber(String.format("Module [%d] Speed", i),
-                modules_[i].getDriveVelocity());
-            SmartDashboard.putNumber(String.format("Module [%d] Angle", i),
-                modules_[i].getSteerPosition().getDegrees());
-        }
+        setModuleStates(module_states);
     }
 
     // Get Module Positions
@@ -105,6 +95,19 @@ public void setSpeeds(ChassisSpeeds speeds) {
         setSpeeds(speeds, OutputType.VELOCITY);
     }
 
+    // Set Module States
+    public void setModuleStates(SwerveModuleState[] desired_states) {
+        SwerveDriveKinematics.desaturateWheelSpeeds(desired_states, Constants.kMaxSpeed);
+        for (int i = 0; i < modules_.length; i++){
+            modules_[i].setDesiredState(desired_states[i], output_type_);
+
+            SmartDashboard.putNumber(String.format("Module [%d] Speed", i),
+                modules_[i].getDriveVelocity());
+            SmartDashboard.putNumber(String.format("Module [%d] Angle", i),
+                modules_[i].getSteerPosition().getDegrees());
+        }
+    }
+
     // Puts wheels in X shape for brake
     public void HoldPosition() {
         front_left_.setAngle(45);

src/main/java/frc/robot/subsystems/Intake.java

@@ -1,7 +1,5 @@
 package frc.robot.subsystems;
 
-import javax.lang.model.util.ElementScanner14;
-
 import com.revrobotics.CANSparkMax;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;