Swerve drive integration (#53)

* Added drive commands and smartdashboard output

* deleted robot config

* Constants, style sanity changes

* Integrating swerve (1/2 way there)

* Standardizes spiral module definition, adds todos

* Changes dimension of robot

* Minor changes to kinematics and offsets

* Inverted motor and minor comment change

* Update motor inversions

* Update button bindings

* Changed top arm constants

* new settings

---------

Co-authored-by: redplover <redploveryt@gmail.com>
Co-authored-by: Honzik <100500945+redPlover@users.noreply.github.com>
Co-authored-by: TheLumpiest <gideon.colliver@gmail.com>

src/main/java/frc/robot/Constants.java

@@ -22,64 +22,66 @@ public final class ArmConstants {
         public static final double stowShootPosition = 0;
         public static final double lowShootVoltage = -0.1;
         public static final double midShootVoltage = -0.2;
-        public static final double highShootVoltage = -0.5;
+        public static final double highShootVoltage = -0.75;
         public static final double intakeVoltage = 0.2;
         public static final double spitVoltage = 0.5;
 
+        public static final double topShootPower = 0.5;
+        public static final double topIntakePower = -0.75;
+
     }
 
     public static final class DriveConstants {
 
-        public static final double trackWidth = Units.inchesToMeters(22.75); //need to measure
-        public static final double wheelBase = Units.inchesToMeters(20.75); //need to measure
-
-        public static final int frontLeftID = 52;
-		public static final int backLeftID = 53;
-		public static final int frontRightID = 14;
-		public static final int backRightID = 16;
-
-        public static final int frontLeftRotationMotorID = 0; //placeholder
-        public static final int frontRightRotationMotorID = 0; //placeholder
-        public static final int backLeftRotationMotorID = 0; //placeholder
-        public static final int backRightRotationMotorID = 0; //placeholder
-
-        public static final int frontLeftRotationEncoderID = 0; //placeholder
-        public static final int frontRightRotationEncoderID = 0; //placeholder
-        public static final int backLeftRotationEncoderID = 0; //placeholder
-        public static final int backRightRotationEncoderID = 0; //placeholder
-
-        public static final double frontLeftAngleOffset = 0.0; //placeholder
-        public static final double frontRightAngleOffset = 0.0; //placeholder
-        public static final double backLeftAngleOffset = 0.0; //placeholder
-        public static final double backRightAngleOffset = 0.0; //placeholder
+        public static final double trackWidth = Units.inchesToMeters(20.4); // TODO: Might have to change
+        public static final double wheelBase = Units.inchesToMeters(22.4); // TODO: Might have to change
+
+        public static final int frontLeftDriveID = 1;
+        public static final int frontRightDriveID = 3;
+		public static final int backRightDriveID = 5;
+		public static final int backLeftDriveID = 7;
+		
+        public static final int frontLeftRotationMotorID = 2;
+        public static final int frontRightRotationMotorID = 4;
+        public static final int backRightRotationMotorID = 6;
+        public static final int backLeftRotationMotorID = 8;
+
+        public static final int frontLeftRotationEncoderID = 1; 
+        public static final int frontRightRotationEncoderID = 2; 
+        public static final int backRightRotationEncoderID = 3;
+        public static final int backLeftRotationEncoderID = 4; 
+
+        public static final double frontLeftAngleOffset = -2.3930100291016;
+        public static final double frontRightAngleOffset = 2.560213934981135; 
+        public static final double backRightAngleOffset = 2.389942067525829; // TODO: Might have to switch these
+        public static final double backLeftAngleOffset = -0.582912699396544; // TODO: Might have to switch these
 
 		public static final double autoDrivePercent = 0.6;
-        public static final int driveWheelGearReduction = 0;
-        public static final int wheelRadiusM = 0;
+        public static final double driveWheelGearReduction = 6.12; // placeholder
+        public static final double wheelRadiusM = Units.inchesToMeters(2);
 
         public static final SwerveDriveKinematics kinematics = 
             new SwerveDriveKinematics(
-                new Translation2d(trackWidth / 2.0, wheelBase / 2.0), //front left placeholder
-                new Translation2d(trackWidth / 2.0, -wheelBase / 2.0), //front right placeholder
-                new Translation2d(-trackWidth / 2.0, wheelBase / 2.0), //rear left placeholder
-                new Translation2d(-trackWidth / 2.0, -wheelBase / 2.0) //rear right placeholder
+                new Translation2d(trackWidth / 2.0, wheelBase / 2.0), //TODO: Test
+                new Translation2d(trackWidth / 2.0, -wheelBase / 2.0), //TODO: Test
+                new Translation2d(-trackWidth / 2.0, wheelBase / 2.0), //TODO: Test
+                new Translation2d(-trackWidth / 2.0, -wheelBase / 2.0) //TODO: Test
         );
 
-        public static final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(0.24, 2.185); //placeholder 
+        public static final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(0.2, 0.3); // TODO: Not used
 
-        public static final int pigeonIMU = 0; //placeholder
+        public static final int pigeonIMU = 5;
 
-        public static final double driveJoystickDeadbandPercent = 0; //placeholder
+        public static final double driveJoystickDeadbandPercent = 0.12;
 
-        public static final double maxDriveSpeed = 0; //placeholder
-        public static final double maxTurnRate = 0; //placeholder
+        public static final double maxDriveSpeed = 5;
+        public static final double maxTurnRate = 2 * Math.PI;
 
-        public static final double driveKps = 0.0; //placeholder
-        public static final double driveKds = 0.0; //placeholder
+        public static final double driveKp = 0.01; //placeholder
+        public static final double driveKd = 0.0; //placeholder
 
-        public static final double rotationKps = 0.0; //placeholder
-        public static final double rotationKds = 0.0; //placeholder
-
+        public static final double rotationKp = 7.0;
+        public static final double rotationKd = 0.0;
     }
 
 

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

@@ -1,19 +1,23 @@
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.commands.ArmMove;
+import frc.robot.commands.DriveWithJoysticks;
 import frc.robot.subsystems.ArmSubsystem;
-import frc.robot.subsystems.BasicDriveSubsystem;
 import frc.robot.subsystems.ArmSubsystem.Mode;
+import frc.robot.subsystems.drive.DriveSubsystem;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
+
 public class RobotContainer {
 
-    private final BasicDriveSubsystem drive = new BasicDriveSubsystem();
+    private final DriveSubsystem drive = new DriveSubsystem();
     private ArmSubsystem arm = new ArmSubsystem();
 
     private Joystick leftJoystick = new Joystick(0);
@@ -30,13 +34,14 @@ public class RobotContainer {
     SendableChooser<String> autoChooser = new SendableChooser<String>();
 
     public RobotContainer() {
-        drive.setDefaultCommand(
-            new InstantCommand(
-                () -> drive.arcadeDrive(
-                        leftJoystick.getY(),
-                        rightJoystick.getX() / 2),
-                drive)
-        );
+
+        drive.setDefaultCommand(new DriveWithJoysticks(
+            drive,
+            () -> -leftJoystick.getRawAxis(1),
+            () -> -leftJoystick.getRawAxis(0),
+            () -> -rightJoystick.getRawAxis(0),
+            true
+        ));
         configureButtonBindings();
 
     }
@@ -52,6 +57,10 @@ private void configureButtonBindings() {
             .whileTrue(armShootLow);
         new Trigger(() -> gamepad.getLeftY() < -0.7)
             .whileTrue(armSpit);
+        new JoystickButton(rightJoystick, 1)
+            .whileTrue(new InstantCommand(
+                () -> {drive.resetHeading();
+                }));
 
     }
 

src/main/java/frc/robot/commands/drive/SwerveDriveCommand.java → src/main/java/frc/robot/commands/DriveWithJoysticks.java

@@ -1,21 +1,23 @@
-package frc.robot.commands.drive;
-
+package frc.robot.commands;
 
 import java.util.function.DoubleSupplier;
 
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import frc.robot.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants.DriveConstants;
 import frc.robot.subsystems.drive.DriveSubsystem;
 
-public class SwerveDriveCommand extends CommandBase{
+
+public class DriveWithJoysticks extends CommandBase{
     private final DriveSubsystem drive;
     private final DoubleSupplier xPercent;
     private final DoubleSupplier yPercent;
     private final DoubleSupplier omegaPercent;
     private final boolean fieldRelative;
 
-    public SwerveDriveCommand(DriveSubsystem drive, DoubleSupplier xPercent, DoubleSupplier yPercent, DoubleSupplier omegaPercent, boolean fieldRelative) {
+    public DriveWithJoysticks(DriveSubsystem drive, DoubleSupplier xPercent, DoubleSupplier yPercent, DoubleSupplier omegaPercent, 
+            boolean fieldRelative){
         this.drive = drive;
         this.xPercent = xPercent;
         this.yPercent = yPercent;
@@ -25,12 +27,10 @@ public SwerveDriveCommand(DriveSubsystem drive, DoubleSupplier xPercent, DoubleS
         addRequirements(drive);
     }
 
-    // Called when the command is initially scheduled.
     @Override
     public void initialize() {
     }
 
-    // Called every time the scheduler runs while the command is scheduled.
     @Override
     public void execute() {
         double xMPerS = processJoystickInputs(xPercent.getAsDouble(), false) * DriveConstants.maxDriveSpeed;
@@ -45,16 +45,6 @@ public void execute() {
         drive.setGoalChassisSpeeds(targetSpeeds);
     }
 
-    // Called once the command ends or is interrupted.
-    @Override
-    public void end(boolean interrupted) {}
-
-    // Returns true when the command should end.
-    @Override
-    public boolean isFinished() {
-        return false;
-    }
-
     private double processJoystickInputs(double value, boolean square) {
         double scaledValue = 0.0;
         double deadband = DriveConstants.driveJoystickDeadbandPercent;
@@ -68,4 +58,11 @@ private double processJoystickInputs(double value, boolean square) {
         }
         return scaledValue;
     }
+
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+
+
 }

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

@@ -1,5 +1,7 @@
 package frc.robot.subsystems;
+
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import frc.robot.Constants.ArmConstants;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -9,7 +11,7 @@ public class Intake {
   private CANSparkMax leftMotor, rightMotor, topIntakeMotor;
   private double intakePower;
   private final double CURRENT_LIMIT = 25.0;
-  private boolean runTopIntakeMotor = false;
+  private double topIntakeMotorPower = 0;
 
   public Intake() {
     leftMotor = new CANSparkMax(ArmConstants.leftIntakeMotorID, MotorType.kBrushless);
@@ -18,56 +20,58 @@ public Intake() {
     leftMotor.follow(rightMotor, true);
     leftMotor.setSmartCurrentLimit(80);
     rightMotor.setSmartCurrentLimit(80);
-    topIntakeMotor.setSmartCurrentLimit(80);
+    topIntakeMotor.setSmartCurrentLimit(10);
+
+    rightMotor.setIdleMode(IdleMode.kCoast);
+    leftMotor.setIdleMode(IdleMode.kCoast);
+    topIntakeMotor.setIdleMode(IdleMode.kCoast);
   }
 
-  public void intake () {
+  public void intake() {
     intakePower = ArmConstants.intakeVoltage;
-    runTopIntakeMotor = true;
+    topIntakeMotorPower = ArmConstants.topIntakePower;
   }
 
-  public void shootLow () {
+  public void shootLow() {
     intakePower = ArmConstants.lowShootVoltage;
-    runTopIntakeMotor = false;
+    topIntakeMotorPower = ArmConstants.topShootPower;
   }
 
-  public void shootMid () {
+  public void shootMid() {
     intakePower = ArmConstants.midShootVoltage;
-    runTopIntakeMotor = false;
+    topIntakeMotorPower = ArmConstants.topShootPower;
   }
 
-  public void shootHigh () {
+  public void shootHigh() {
     intakePower = ArmConstants.highShootVoltage;
-    runTopIntakeMotor = false;
+    topIntakeMotorPower = ArmConstants.topShootPower;
   }
 
-  public void spit () {
+  public void spit() {
     intakePower = -ArmConstants.spitVoltage;
-    runTopIntakeMotor = true;
+    topIntakeMotorPower = ArmConstants.topShootPower;
   }
 
-  public void off () {
+  public void off() {
     intakePower = 0;
-    runTopIntakeMotor = true;
+    topIntakeMotorPower = 0;
   }
 
   public void setIntakeMotorPower(double percent) {
-    rightMotor.set(percent);
-    if(runTopIntakeMotor == true) {
-      topIntakeMotor.set(percent);
-    } else {
-      topIntakeMotor.set(0);
-    }
+    rightMotor.set(-percent);
+    topIntakeMotor.set(topIntakeMotorPower);
   }
+
   public double getIntakeMotorAmps() {
     return rightMotor.getOutputCurrent();
   }
+
   private void checkIntakeAmps() {
-    if(getIntakeMotorAmps() > CURRENT_LIMIT) {
+    if (getIntakeMotorAmps() > CURRENT_LIMIT) {
       setIntakeMotorPower(0);
     }
   }
-  
+
   public void run() {
     setIntakeMotorPower(intakePower);
     checkIntakeAmps();