correct swerve offsets

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

@@ -49,10 +49,11 @@ public static final class SwerveConstants{
     public static final int PIGEON_ID = 15;
 
     //Drivetrain characteristics
-    public static final double LEFT_FRONT_OFFSET = -0.200;
-    public static final double RIGHT_FRONT_OFFSET = -0.458;
-    public static final double LEFT_BACK_OFFSET = 0.;
-    public static final double RIGHT_BACK_OFFSET = 0.;
+    //TODO: offsets
+    public static final double LEFT_FRONT_OFFSET = -0.197998;
+    public static final double RIGHT_FRONT_OFFSET = 0.041748;
+    public static final double LEFT_BACK_OFFSET = -0.253174;
+    public static final double RIGHT_BACK_OFFSET = -0.341064;
 
     public static final double WHEEL_DIAMETER = Units.inchesToMeters(4);
     public static final double DRIVE_MOTOR_GEAR_RATIO = 5.357; //SDS Mk4i L3+ (60:16 first stage)
@@ -104,63 +105,15 @@ public static final class SwerveConstants{
     public static final int EXPONENT = 3;
   }
 
-  public static final class IntakeConstants{
-    public static final int UTB_ROLLER_ID = 21;
-  }
-
-  public static final class ShooterConstants{
-    public static final int LEFT_SHOOTER_ID = 31;
-    public static final int RIGHT_SHOOTER_ID = 32;
-    public static final int PIVOT_ID = 33;
-
-    public static final double LEFT_SHOOTER_kP = 0.3;
-    public static final double LEFT_SHOOTER_kI = 0;
-    public static final double LEFT_SHOOTER_kD = 0;
-
-    public static final double RIGHT_SHOOTER_kP = 0.3;
-    public static final double RIGHT_SHOOTER_kI = 0;
-    public static final double RIGHT_SHOOTER_kD = 0;
+  public static final class EndEffectorConstants{
+    public static final int END_EFFECTOR_ID = 21;
 
-    public static final double SHOOTER_MIN_OUTPUT = -1.0;
-    public static final double SHOOTER_MAX_OUTPUT = 1.0;
+    public static final double PULL_VOLTAGE = 0.0;
+    public static final double PUSH_VOLTAGE = 0.0;
 
-    //TODO Tune Pivot to Not Oscillate
-    public static final double PIVOT_kP = 0.07;
-    public static final double PIVOT_kI = 0.00008;
-    public static final double PIVOT_kD = 0;
 
-    public static final double PIVOT_MIN_OUTPUT = -0.85;
-    public static final double PIVOT_MAX_OUTPUT = 0.85;
-
-    public static final double AMP_PIVOT_POSITION = 13.8;
-    public static final double PASSING_PIVOT_POSITION = 15.5;
-    public static final double SPEAKER_PIVOT_POSITION = 19.7;
-
-    public static final double FLOOR_TO_SHOOTER = Units.inchesToMeters(7);
+    public static final int END_SENSOR_CHANNEL = 0;
   }
-
-  public static final class TransportConstants{
-    public static final int TOP_TRANSPORT_ID = 34;
-    public static final int BOT_TRANSPORT_ID = 35;
-
-    public static final int IR_SENSOR_CHANNEL = 9;
-  }
-
-  public static final class AmpBarConstants{
-    public static final int AMP_BAR_ID = 41;
-
-    public static final double AMP_BAR_kP = 0.25;
-    public static final double AMP_BAR_kI = 0;
-    public static final double AMP_BAR_kD = 0;
-
-    public static final double AMP_BAR_MIN_OUTPUT = -0.5;
-    public static final double AMP_BAR_MAX_OUTPUT = 0.5;
-
-    // Zero at Top of Shooter at Lowest Pivot
-    public static final double STOWED_ROT = 3.0;
-    public static final double DEPLOYED_ROT = 22.0;
-  }
-
   public static final class FieldConstants{
     public static final double FIELD_LENGTH = 16.54175;
     public static final double FIELD_WIDTH = 8.21055;

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

@@ -48,7 +48,7 @@ public void robotInit() {
     Logger.recordMetadata("ProjectName", "MyProject"); // Set a metadata value
 
     if (isReal()) {
-        //Logger.addDataReceiver(new WPILOGWriter("/media/sda1/")); // Log to a USB stick ("/U/logs")
+        Logger.addDataReceiver(new WPILOGWriter("/media/sda1/")); // Log to a USB stick ("/U/logs")
         Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables 
         // new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
     } 

src/main/java/frc/robot/commands/SwerveDrive.java

@@ -35,8 +35,7 @@ public void execute() {
         -driverController.getRightX(),
         RobotContainer.driverController.getRightTriggerAxis() < 0.9,
         new Translation2d(),
-        true,
-        SwerveConstants.EXPONENT);
+        true);
   }
 
   // Called once the command ends or is interrupted.

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

@@ -47,9 +47,12 @@ public class Drivetrain extends SubsystemBase {
   private GenericEntry rightBackStateEntry;
   private GenericEntry robotAngleEntry;
   private GenericEntry angularSpeedEntry;
+  private GenericEntry exponentEntry;
 
   private static final Drivetrain DRIVETRAIN = new Drivetrain();
 
+  private double exponent = 1;
+
   public static Drivetrain getInstance(){
     return DRIVETRAIN;
   }
@@ -75,15 +78,15 @@ public Drivetrain() {
     rightFront = new SwerveModule(
       SwerveConstants.RIGHT_FRONT_DRIVE_ID, 
       SwerveConstants.RIGHT_FRONT_TURN_ID, 
-      false, 
-      false, 
+      true, 
+      true, 
       SwerveConstants.RIGHT_FRONT_CANCODER_ID, 
       SwerveConstants.RIGHT_FRONT_OFFSET);
 
     leftBack = new SwerveModule(
       SwerveConstants.LEFT_BACK_DRIVE_ID, 
       SwerveConstants.LEFT_BACK_TURN_ID, 
-      false, 
+      true, 
       true, 
       SwerveConstants.LEFT_BACK_CANCODER_ID, 
       SwerveConstants.LEFT_BACK_OFFSET);
@@ -119,6 +122,7 @@ public Drivetrain() {
     rightBackStateEntry = swerveTab.add("Right Back Module State", rightBack.getState().toString()).withSize(4, 1).withPosition(0, 3).getEntry();
     robotAngleEntry = swerveTab.add("Robot Angle", getHeading()).withSize(1, 1).withPosition(4, 1).getEntry();
     angularSpeedEntry = swerveTab.add("Angular Speed", new DecimalFormat("#.00").format((-gyro.getRate() / 180)) + "\u03C0" + " rad/s").withSize(1, 1).withPosition(5, 1).getEntry();
+    exponentEntry = swerveTab.add("Exponent", 1.0).withSize(1, 1).withPosition(4, 2).getEntry();
   }
 
   @Override
@@ -142,19 +146,19 @@ public void periodic() {
   }
 
   public void swerveDrive(double frontSpeed, double sideSpeed, double turnSpeed, 
-    boolean fieldOriented, Translation2d centerOfRotation, boolean deadband, int exponent){ //Drive with rotational speed control w/ joystick
-
-    frontSpeed = Math.pow(frontSpeed, exponent) * exponent % 2 == 0 ? Math.signum(frontSpeed): 1;
-    sideSpeed = Math.pow(sideSpeed, exponent) * exponent % 2 == 0 ? Math.signum(sideSpeed): 1;
-    turnSpeed = Math.pow(turnSpeed, exponent) * exponent % 2 == 0 ? Math.signum(turnSpeed): 1;
-
+    boolean fieldOriented, Translation2d centerOfRotation, boolean deadband){ //Drive with rotational speed control w/ joystick
+    exponent = exponentEntry.getDouble(1.0);
 
     if(deadband){
       frontSpeed = Math.abs(frontSpeed) > 0.15 ? frontSpeed : 0;
       sideSpeed = Math.abs(sideSpeed) > 0.15 ? sideSpeed : 0;
       turnSpeed = Math.abs(turnSpeed) > 0.15 ? turnSpeed : 0;
     }
 
+    frontSpeed = Math.pow(frontSpeed, exponent) * (exponent % 2 == 0 ? Math.signum(frontSpeed): 1);
+    sideSpeed = Math.pow(sideSpeed, exponent) * (exponent % 2 == 0 ? Math.signum(sideSpeed): 1);
+    turnSpeed = Math.pow(turnSpeed, exponent) * (exponent % 2 == 0 ? Math.signum(turnSpeed): 1);
+
     frontSpeed = frontLimiter.calculate(frontSpeed) * SwerveConstants.TELE_DRIVE_MAX_SPEED;
     sideSpeed = sideLimiter.calculate(sideSpeed) * SwerveConstants.TELE_DRIVE_MAX_SPEED;
     turnSpeed = turnLimiter.calculate(turnSpeed) * SwerveConstants.TELE_DRIVE_MAX_ANGULAR_SPEED;
@@ -180,8 +184,21 @@ public void swerveDrive(ChassisSpeeds chassisSpeeds, Translation2d centerOfRotat
     setModuleStates(moduleStates);
   }
 
+  public void setAllIdleMode(boolean brake){
+    if(brake){
+      leftFront.setBrake(true);
+      rightFront.setBrake(true);
+      leftBack.setBrake(true);
+      rightBack.setBrake(true);
+    }
+    else{
+      leftFront.setBrake(false);
+      rightFront.setBrake(false);
+      leftBack.setBrake(false);
+      rightBack.setBrake(false);
+    }
+  }
 
-
   public void resetAllEncoders(){
     leftFront.resetEncoders();
     rightFront.resetEncoders();

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

@@ -0,0 +1,74 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// 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 frc.robot.subsystems;
+
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.lib.drivers.PearadoxSparkMax;
+import frc.robot.Constants.EndEffectorConstants;
+import frc.robot.RobotContainer;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.Debouncer.DebounceType;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class EndEffector extends SubsystemBase {
+
+  private static final EndEffector END_EFFECTOR = new EndEffector();
+
+  public static EndEffector getInstance(){
+    return END_EFFECTOR;
+  }
+
+  private PearadoxSparkMax endEffector;
+  private DigitalInput endSensor;
+  private Debouncer debouncer;
+
+  private boolean rumbled = false;
+  private boolean isExtended = false; //TODO: integrate with arm
+
+  public EndEffector() {
+    endEffector = new PearadoxSparkMax(EndEffectorConstants.END_EFFECTOR_ID, MotorType.kBrushless, IdleMode.kBrake, 50, false);
+    endSensor = new DigitalInput(EndEffectorConstants.END_SENSOR_CHANNEL);
+    debouncer = new Debouncer(0.2, DebounceType.kFalling);
+  }
+
+  @Override
+  public void periodic() {
+    collectCoral();
+
+    SmartDashboard.putBoolean("End Sensor", hasCoral());
+  }
+
+  public void collectCoral() {
+    if(RobotContainer.driverController.getRightTriggerAxis() >= 0.95){
+      coralIn();
+    } else if(RobotContainer.driverController.getLeftTriggerAxis() >= 0.95){
+      coralOut();
+    } else if(hasCoral()){
+      stopEndEffector();
+    } else{
+      stopEndEffector();
+    }
+  }
+
+  public void coralIn(){
+    endEffector.set(EndEffectorConstants.PULL_VOLTAGE);
+  }
+
+  public void coralOut(){
+    endEffector.set(-EndEffectorConstants.PUSH_VOLTAGE);
+  }
+
+  public void stopEndEffector(){
+    endEffector.set(0.0);
+  }
+
+  public boolean hasCoral(){
+    return debouncer.calculate(!endSensor.get());
+  }
+}