created arm PID, changed IDs, fixed swerve (im tweakin) (pushkar has a mental disability)

src/main/java/frc4388/robot/Constants.java

@@ -159,8 +159,8 @@ public final class Constants {
     }
 
     public static final class IntakeConstants {
-        public static final int INTAKE_MOTOR_ID = 17; //TODO:
-        public static final int PIVOT_MOTOR_ID = 18; //TODO:
+        public static final int INTAKE_MOTOR_ID = 18; //TODO:
+        public static final int PIVOT_MOTOR_ID = 17; //TODO:
         public static final double INTAKE_SPEED = 0.2; //TODO:
         public static final double HANDOFF_SPEED = 0.2; //TODO:
         public static final double PIVOT_SPEED = 0.2; //TODO:

src/main/java/frc4388/robot/RobotContainer.java

@@ -16,6 +16,7 @@ import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.commands.Autos.PlaybackChooser;
 import frc4388.robot.commands.Swerve.JoystickPlayback;
 import frc4388.robot.commands.Swerve.JoystickRecorder;
+import frc4388.robot.commands.Intake.RotateIntakeToPosition;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Shooter;
@@ -46,7 +47,7 @@ public class RobotContainer {
      private final Shooter m_robotShooter = new Shooter(m_robotMap.leftShooter, m_robotMap.rightShooter);
 
      private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor);
-    
+
     //private final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor, m_robotMap.pivotMotor);
 
     /* Controllers */
@@ -115,11 +116,14 @@ public class RobotContainer {
         
         
         /* Operator Buttons */
-         new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
+         new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
              .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter))
              .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter));
-
-        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+             
+         new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
+             .onTrue(new InstantCommand(() ->  new RotateIntakeToPosition(m_robotIntake, 360).execute(), m_robotIntake))
+             .onFalse(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 0).execute(), m_robotShooter));
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor()))
             .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors()));
         

src/main/java/frc4388/robot/commands/Intake/RotateIntakeToPosition.java

@@ -0,0 +1,38 @@
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+package frc4388.robot.commands.Intake;
+
+
+import com.revrobotics.CANSparkMax;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc4388.robot.commands.PID;
+import frc4388.robot.subsystems.SwerveDrive;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+
+import frc4388.robot.subsystems.Intake;
+public class RotateIntakeToPosition extends PID {
+
+  Intake intake;
+  double targetAngle;
+
+  /** Creates a new PIDSparkMax. */
+  public RotateIntakeToPosition(Intake intake, double targetAngle) {
+    super(0.3, 0.0, 0.0, 0.0, 1);
+    
+    this.intake = intake;
+    this.targetAngle = targetAngle;
+
+    addRequirements(intake);
+  }
+
+  @Override
+  public double getError() {
+    return targetAngle - (((intake.getEncoder().getPosition()) * (360))%360);
+  }
+
+  @Override
+  public void runWithOutput(double output) {
+    intake.setVoltage(output / Math.abs(getError()));    
+  }
+}

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

@@ -6,7 +6,7 @@ package frc4388.robot.subsystems;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.SparkLimitSwitch;
-
+import com.revrobotics.RelativeEncoder;
 import edu.wpi.first.wpilibj.CAN;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -16,7 +16,7 @@ public class Intake extends SubsystemBase {
   
   private CANSparkMax intakeMotor;
   private CANSparkMax pivot;
-  
+
   /** Creates a new Intake. */
   public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) {
     this.intakeMotor = intakeMotor;
@@ -46,8 +46,14 @@ public class Intake extends SubsystemBase {
   public void stopIntakeMotors() {
     intakeMotor.set(0);
   }
-
+  public RelativeEncoder getEncoder() {
+    return pivot.getEncoder();
+  }
+  public void setVoltage(double voltage) {
+   pivot.setVoltage(voltage);
+  }
   public void rotateArm() {
+
   }
 
   @Override

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -77,9 +77,9 @@ public class  SwerveDrive extends SubsystemBase {
       // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
 
       // Convert field-relative speeds to robot-relative speeds.
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
     } else {      // Create robot-relative speeds.
-      chassisSpeeds = new ChassisSpeeds(leftStick.getX(), leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
+      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
     }
     setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
   }