I'm a little confused, might work

frc-862 · Oct 12, 2023 · 37c033c · 37c033c
1 parent 3a33978
commit 37c033c
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 7 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -158,5 +158,8 @@ public static final class AutonomousConstants {
         public static final PIDConstants POSE_PID_CONSTANTS = new PIDConstants(0, 0, 0); // X and Y position PID
 
         public static final PathConstraints CUBE_CHASE_CONSTRAINTS = new PathConstraints(2, 2); // TODO TEST FOR MAX 
+
+        public static final double MAX_VELOCITY = 2;
+        public static final double MAX_ACCELERATION = 1;
     }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -3,9 +3,11 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.lib.auto.Autonomous;
+import frc.lib.auto.AutonomousCommandFactory;
 
 public class Robot extends TimedRobot {
-    private Command m_autonomousCommand;
+    private Command autonomousCommand;
 
     private RobotContainer m_robotContainer;
 
@@ -18,6 +20,8 @@ public void robotInit() {
         // Instantiate our RobotContainer.    This will perform all our button bindings, and put our
         // autonomous chooser on the dashboard.
         m_robotContainer = new RobotContainer();
+        Autonomous.load();
+        AutonomousCommandFactory.connectToServer(5811);
     }
 
     /**
@@ -46,11 +50,11 @@ public void disabledPeriodic() {}
     /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
     @Override
     public void autonomousInit() {
-        m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+        autonomousCommand = Autonomous.getAutonomous();
 
         // schedule the autonomous command (example)
-        if (m_autonomousCommand != null) {
-            m_autonomousCommand.schedule();
+        if (autonomousCommand != null) {
+            autonomousCommand.schedule();
         }
     }
 
@@ -64,8 +68,8 @@ public void teleopInit() {
         // teleop starts running. If you want the autonomous to
         // continue until interrupted by another command, remove
         // this line or comment it out.
-        if (m_autonomousCommand != null) {
-            m_autonomousCommand.cancel();
+        if (autonomousCommand != null) {
+            autonomousCommand.cancel();
         }
     }
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -1,12 +1,14 @@
 package frc.robot;
 
+import java.util.HashMap;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.lib.auto.AutonomousCommandFactory;
+import frc.lib.pathplanner.com.pathplanner.lib.PathConstraints;
 import frc.lib.pathplanner.com.pathplanner.lib.PathPoint;
 import frc.lib.util.Limelight;
 import frc.robot.Constants.AutonomousConstants;
@@ -44,5 +46,15 @@ private void configureDefaultCommands() {
         drivetrain.setDefaultCommand(new TeleopSwerve(drivetrain, () -> driver.getLeftY(), () -> driver.getLeftX(), () -> driver.getRightX(), () -> (driver.getRightTriggerAxis() > 0.75)));
     }
 
-    public Command getAutonomousCommand() { return autoFactory}
+    private void configureAutonomousCommands(){
+        autoFactory.makeTrajectory("B2[1]-M-C-HIGH", new HashMap<>(), 
+            new PathConstraints(AutonomousConstants.MAX_VELOCITY, AutonomousConstants.MAX_ACCELERATION));
+
+    }
+
+    protected AutonomousCommandFactory getCommandFactory(){
+        return autoFactory;
+    }
+
+    public Command getAutonomousCommand() { return null;}
 }