Auto heading control inverted HELP

src/main/deploy/pathplanner/autos/He He He Haw.auto

@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.4585613729632017,
+      "y": 7.0398809743488435
+    },
+    "rotation": 180.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "He He He Haw"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Right 2S Center.auto

@@ -0,0 +1,62 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.52,
+      "y": 4.034483282641779
+    },
+    "rotation": 180.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Right Sub"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Charge Shooter"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Right Sub to Right Center Note"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Right Sub from Center"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/He He He Haw.path

@@ -0,0 +1,74 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.4585613729632017,
+        "y": 7.0398809743488435
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.4585613729632003,
+        "y": 7.0398809743488435
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.7856981070087494,
+        "y": 5.98740707452925
+      },
+      "prevControl": {
+        "x": 1.7856981070087494,
+        "y": 5.98740707452925
+      },
+      "nextControl": {
+        "x": 3.7856981070087494,
+        "y": 5.98740707452925
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.69184528112646,
+        "y": 7.051575128791284
+      },
+      "prevControl": {
+        "x": 4.238771694067605,
+        "y": 6.519491101660266
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 90.0,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 88.210089391754,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Left Subwoofer.path

@@ -39,7 +39,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -146.30993247402017,
+    "rotation": 149.21037688376774,
     "rotateFast": false
   },
   "reversed": false,

src/main/deploy/pathplanner/paths/Right Sub from Center.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.335023775460655,
+        "y": 1.1577212898015548
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.803089543501023,
+        "y": 0.9472265098376356
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.0432690950851984,
+        "y": 4.478861151454497
+      },
+      "prevControl": {
+        "x": 2.013883913807713,
+        "y": 1.5436283864020721
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 143.130102354156,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 180.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Right Sub to Right Center Note.path

@@ -0,0 +1,76 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.335622956146197,
+        "y": 4.315142989260338
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.8735540604984342,
+        "y": 1.4149926875352343
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.603689737294005,
+        "y": 0.8068966565283563
+      },
+      "prevControl": {
+        "x": 6.603689737294005,
+        "y": 0.8068966565283563
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.6,
+      "rotationDegrees": 180.0,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [
+    {
+      "name": "Note Pickup",
+      "waypointRelativePos": 0.6,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "Note Pickup"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 139.21417852273407,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Right Sub.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.52,
+        "y": 4.034483282641779
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.0783515584179242,
+        "y": 3.5199404871715854
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.0783515584179242,
+        "y": 4.53733192366386
+      },
+      "prevControl": {
+        "x": 1.85016575161896,
+        "y": 4.034483282638943
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 143.6955028774248,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": -178.76146677965983,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/java/frc/robot/RobotContainer.java

@@ -9,6 +9,8 @@ import java.util.Optional;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.util.PPLibTelemetry;
+import com.pathplanner.lib.util.PathPlannerLogging;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -97,6 +99,7 @@ public class RobotContainer {
         // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration
         NamedCommands.registerCommand("Set Drivetrain X", drivetrain.setXCommand());
 
+
         // TODO Specify a default auto string once we have one
         autoChooser = AutoBuilder.buildAutoChooser();
 
@@ -134,8 +137,8 @@ public class RobotContainer {
 
         drivetrain.setDefaultCommand(
             drivetrain.teleopCommand(
-                driver::getLeftY, 
+                () -> { return -driver.getLeftY(); }, 
-                driver::getLeftX, 
+                () -> { return -driver.getLeftX(); }, 
                 () -> { return -driver.getRightX(); }, 
                 OIConstants.kTeleopDriveDeadband
             )
@@ -154,9 +157,7 @@ public class RobotContainer {
                     }
                 },
                 () -> {
-                    if (driver.getLeftTriggerAxis() > .25) {
+                    if(driver.getRightTriggerAxis() > 0.25){
-                        return -1.0;
-                    }else if(driver.getRightTriggerAxis() > 0.25){
                         return 1.0;
                     } else {
                         return 0;
@@ -271,6 +272,11 @@ public class RobotContainer {
 
         operator.start().onTrue(shooter.zeroEncoder());
 
+        operator.povDown().whileTrue(intake.intakeDownCommand());
+        operator.povUp().whileTrue(intake.intakeUpCommand());
+
+        driver.leftTrigger().whileTrue(Commands.parallel( intake.manualPivot(() -> 0.0, () -> 1.0), indexer.advanceNote()));
+        
   }
 
   private void shuffleboardSetup() {
@@ -293,7 +299,10 @@ public class RobotContainer {
     teleopTab.addBoolean("indexer beam break", indexer::getBeamBreak);
     teleopTab.addBoolean("shooter beam break", shooter::getBeamBreak);
     teleopTab.addDouble("shooter angle", shooter::getShooterAngle);
-    teleopTab.addDouble("intake angle", intake::getIntakeAngle);
+    teleopTab.addDouble("intake angle", intake::getIntakeDegrees);
+    teleopTab.addDouble("intake pid", intake::getIntakePID);
+    teleopTab.addDouble("heading swerve", drivetrain::getHeading);
+    teleopTab.addDouble("steering encoder", drivetrain::getEncoderSteering);
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/constants/AutoConstants.java

@@ -13,9 +13,9 @@ public final class AutoConstants {
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 
-    public static final double kPXController = 1.05;
+    public static final double kPXController = 5;
-    public static final double kPYController = 1.05;
+    public static final double kPYController = 5;
-    public static final double kPThetaController = 0.95; // needs to be separate from heading control
+    public static final double kPThetaController = 0.5; // needs to be separate from heading control
 
     public static final double kPHeadingController = 0.02; // for heading control NOT PATHING
     public static final double kDHeadingController = 0.0025;

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -26,10 +26,16 @@ public final class DrivetrainConstants {
         new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
 
     // Angular offsets of the modules relative to the chassis in radians
+    /* 
     public static final double kFrontLeftChassisAngularOffset = Math.PI;
     public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
     public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
     public static final double kBackRightChassisAngularOffset = 0;
+*/
+    public static final double kFrontLeftChassisAngularOffset = -2*Math.PI;
+    public static final double kFrontRightChassisAngularOffset = -2*Math.PI;
+    public static final double kBackLeftChassisAngularOffset = -2*Math.PI;
+    public static final double kBackRightChassisAngularOffset = -2*Math.PI-(4/180.0);
 
     // SPARK MAX CAN IDs
     public static final int kFrontLeftDrivingCanId = 8;

src/main/java/frc/robot/constants/IntakeConstants.java

@@ -4,19 +4,20 @@ public class IntakeConstants {
     public static final int kIntakePivotID = 10;
     public static final int kIntakeRollerID = 12;
 
-    public static final double kIntakePivotConversionFactor = 1/(20.0*(28.0/15.0));
+    public static final double kIntakePivotConversionFactor = (20.0*(28.0/15.0));
 
     public static final int kPivotCurrentLimit = 40;
 
-    public static final double kPIntake = 0;
+    public static final double kPIntake = 2.5;
     public static final double kIIntake = 0;
     public static final double kDIntake = 0;
 
     public static final double kSFeedForward = 0;
-    public static final double kGFeedForward = 1.11;
+    public static final double kGFeedForward = 0;//1.11;
-    public static final double kVFeedForward = 0.73;
+    public static final double kVFeedForward = 0;//0.73;
 
     public static final double kStartingAngle = Math.toRadians(105.0);
     public static final double kUpAngle = Math.toRadians(90.0);
     public static final double kDownAngle = Math.toRadians(-34.0);
+
 }

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

@@ -451,4 +451,8 @@ public class Drivetrain extends SubsystemBase {
     public double getTurnRate() {
         return m_gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
     }
+
+    public double getEncoderSteering(){
+        return m_frontLeft.steeringEncoder();
+    }
 }

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

@@ -41,9 +41,9 @@ public class Indexer extends SubsystemBase{
     public Command advanceNote(){
         return run(() -> {
             if(indexerBeamBreak.get()){
-                indexerMotor.set(0.75);
+                indexerMotor.set(1);
             }else{
-                indexerMotor.set(0.75);
+                indexerMotor.set(0.0);
             }
         });
     }

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

@@ -1,5 +1,6 @@
 package frc.robot.subsystems;
 
+import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import com.revrobotics.CANSparkMax;
@@ -9,6 +10,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType;
 
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.IntakeConstants;
@@ -23,7 +25,11 @@ public class Intake extends SubsystemBase{
 
     private ArmFeedforward intakeFeedForward;
 
+    private double armOffset;
+
     public Intake(){
+        armOffset = 0;
+
         intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless);
         
         intakeRoller.setSmartCurrentLimit(60);
@@ -43,8 +49,8 @@ public class Intake extends SubsystemBase{
         );
 
         intakeEncoder = intakePivot.getEncoder();
-        intakeEncoder.setPosition(IntakeConstants.kStartingAngle);
+        intakeEncoder.setPosition(Units.radiansToRotations( IntakeConstants.kStartingAngle));
-        intakeEncoder.setPositionConversionFactor(IntakeConstants.kIntakePivotConversionFactor);
+       // intakeEncoder.setPositionConversionFactor(IntakeConstants.kIntakePivotConversionFactor);
 
         intakeAnglePID = new PIDController(
             IntakeConstants.kPIntake, 
@@ -52,24 +58,54 @@ public class Intake extends SubsystemBase{
             IntakeConstants.kDIntake
         );
 
+        armOffset = getIntakeAngle()-IntakeConstants.kStartingAngle;
     }
 
     public Command intakeDownCommand() {
         return run(() -> {
-            intakeRoller.set(1.0);
+            intakeRoller.set(0.0);
         
             intakePivot.setVoltage(
                 intakeAnglePID.calculate(
-                    intakeEncoder.getPosition(), 
+                    getIntakeAngle(), 
                     IntakeConstants.kDownAngle
                 ) + intakeFeedForward.calculate(
                     IntakeConstants.kDownAngle, 
-                    intakeEncoder.getVelocity()
+                    0.0
                 )
             );
         });
     }
 
+    public Command autoIntaking(BooleanSupplier beamBreak){
+        return run(() -> {
+            if(beamBreak.getAsBoolean()){
+                intakeRoller.set(1.0);
+
+                intakePivot.setVoltage(
+                intakeAnglePID.calculate(
+                    getIntakeAngle(), 
+                    IntakeConstants.kUpAngle
+                ) + intakeFeedForward.calculate(
+                    IntakeConstants.kUpAngle, 
+                    0.0
+                )
+                );
+            }else{
+                intakeRoller.set(0);
+                intakePivot.setVoltage(
+                intakeAnglePID.calculate(
+                    getIntakeAngle(), 
+                    IntakeConstants.kDownAngle
+                ) + intakeFeedForward.calculate(
+                    IntakeConstants.kDownAngle, 
+                    0.0
+                )
+                );
+            }
+        });
+    }
+
     public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier rollerSpinny){
         return run(() ->{
             intakePivot.set(pivotPower.getAsDouble());
@@ -83,11 +119,11 @@ public class Intake extends SubsystemBase{
 
             intakePivot.setVoltage(
                 intakeAnglePID.calculate(
-                    intakeEncoder.getPosition(), 
+                    getIntakeAngle(), 
                     IntakeConstants.kDownAngle
                 ) + intakeFeedForward.calculate(
                     IntakeConstants.kDownAngle, 
-                    intakeEncoder.getVelocity()
+                    0.0
                 )
             );
         });
@@ -99,17 +135,31 @@ public class Intake extends SubsystemBase{
 
             intakePivot.setVoltage(
                 intakeAnglePID.calculate(
-                    intakeEncoder.getPosition(), 
+                    getIntakeAngle(), 
                     IntakeConstants.kUpAngle
                 ) + intakeFeedForward.calculate(
                     IntakeConstants.kUpAngle, 
-                    intakeEncoder.getVelocity()
+                    0.0
                 )
             );
         });
     }
 
     public double getIntakeAngle(){
-        return intakeEncoder.getPosition();
+        return Units.rotationsToRadians(intakeEncoder.getPosition()/IntakeConstants.kIntakePivotConversionFactor)-armOffset;
+    }
+
+    public double getIntakePID(){
+        return intakeAnglePID.calculate(
+                    getIntakeAngle(), 
+                    IntakeConstants.kDownAngle
+                ) + intakeFeedForward.calculate(
+                    IntakeConstants.kDownAngle, 
+                    0.0
+                );
+    }
+
+    public double getIntakeDegrees(){
+        return Math.toDegrees(getIntakeAngle());
     }
 }

src/main/java/frc/robot/utilities/MAXSwerveModule.java

@@ -157,4 +157,8 @@ public class MAXSwerveModule {
 	public void resetEncoders() {
 		m_drivingEncoder.setPosition(0);
 	}
+
+	public double steeringEncoder(){
+		return m_turningEncoder.getPosition();
+	}
 }