Merge branch 'master' into arm

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

@@ -0,0 +1,36 @@
+package frc4388.robot.subsystems;
+
+//import edu.wpi.first.apriltag.AprilTag;
+//import edu.wpi.first.math.geometry.Pose3d;
+//import edu.wpi.first.math.geometry.Rotation3d;
+//import edu.wpi.first.networktables.NetworkTable;
+//import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class Apriltags {
+    public static class Tag {
+        public boolean visible = true;
+        public double x,  y,  z  = 0;
+        public double ry, rp, rr = 0;
+    }
+
+    public Tag getTagPosRot() {
+        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+
+        final Tag tag = new Tag();
+        tag.visible = isAprilTag();
+        tag.x  = tagTable.getEntry("TagPosX").getDouble(0);
+        tag.y  = tagTable.getEntry("TagPosY").getDouble(0);
+        tag.z  = tagTable.getEntry("TagPosZ").getDouble(0);
+        tag.ry = tagTable.getEntry("TagRotY").getDouble(0);
+        tag.rp = tagTable.getEntry("TagRotP").getDouble(0);
+        tag.rr = tagTable.getEntry("TagRotR").getDouble(0);
+
+        return tag;
+    }
+
+    public boolean isAprilTag() {
+        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+        return tagTable.getEntry("IsTag").getBoolean(false);
+    }
+}

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

@@ -7,7 +7,6 @@ import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.CANCoderConfiguration;
-
 import frc4388.robot.Constants.ArmConstants;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;

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

@@ -0,0 +1,23 @@
+package frc4388.robot.subsystems;
+import edu.wpi.first.wpilibj.PWM;
+
+public class Claw {
+	private PWM m_clawMotor;
+	private boolean m_open = false;
+
+    // Opens claw
+	public Claw(PWM m_clawMotor) {
+		 this.m_clawMotor = m_clawMotor;
+		 setClaw(true);
+	}
+
+    public void setClaw(boolean open) {
+        // Open claw
+		m_open = open;
+		m_clawMotor.setRaw(open ? 0 : 2000);
+    }
+
+	public boolean isClawOpen() {
+		return m_open;
+	}
+}

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

@@ -0,0 +1,27 @@
+package frc4388.robot.subsystems;
+
+import frc4388.robot.subsystems.Apriltags.Tag;
+
+public class Location {
+    final Apriltags apriltag = new Apriltags();
+
+    private boolean isLimelight = false;
+    private boolean isApriltag = false;
+
+    //Determines which source to get pos and rot from and also resets
+    private void reoderPrio(){
+        isLimelight = false; //If limelight gets position and if within a certain range of poles
+        isApriltag = apriltag.isAprilTag();
+    }
+
+    public Tag getPosRot() {
+        reoderPrio();
+        if(isApriltag){
+            return apriltag.getTagPosRot();
+        } else if (isLimelight) {
+            return null;
+        }
+
+        return null;
+    }
+}

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

@@ -4,15 +4,16 @@
 
 package frc4388.robot.subsystems;
 
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.RobotGyro;
 
@@ -25,27 +26,23 @@ public class SwerveDrive extends SubsystemBase {
 
   private SwerveModule[] modules;
 
-  private RobotGyro gyro;
-
   private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
   private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
   private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
   private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
   
-  private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); 
+  private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
 
-  private SwerveDriveOdometry odometry = new SwerveDriveOdometry(
-    kinematics, 
-    gyro.getRotation2d(),
-    new SwerveModulePosition[] {
-      leftFront.getPosition(),
-      rightFront.getPosition(),
-      leftBack.getPosition(),
-      rightBack.getPosition()
-    }
-  );
+  private RobotGyro gyro;
+
+  // private SwerveDriveOdometry odometry;
+
+  private SwerveDrivePoseEstimator poseEstimator;
 
   public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
+  
+  public Rotation2d rotTarget = new Rotation2d();
+  public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
   /** Creates a new SwerveDrive. */
   public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
@@ -53,23 +50,55 @@ public class SwerveDrive extends SubsystemBase {
     this.rightFront = rightFront;
     this.leftBack = leftBack;
     this.rightBack = rightBack;
-
-    this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
-
+    
     this.gyro = gyro;
-  }
 
-  public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
-    double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
-    double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
+    // this.odometry = new SwerveDriveOdometry(
+    //   kinematics, 
+    //   gyro.getRotation2d(),
+    //   new SwerveModulePosition[] {
+    //     leftFront.getPosition(),
+    //     rightFront.getPosition(),
+    //     leftBack.getPosition(),
+    //     rightBack.getPosition()
+    //   },
+    //   getOdometry()
+    // );
 
-    SwerveModuleState[] states = kinematics.toSwerveModuleStates(
-      fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d()) 
-                    : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot)
+    this.poseEstimator = new SwerveDrivePoseEstimator(
+      kinematics, 
+      gyro.getRotation2d(), 
+      new SwerveModulePosition[] {
+        leftFront.getPosition(),
+        rightFront.getPosition(),
+        leftBack.getPosition(),
+        rightBack.getPosition()
+      },
+      new Pose2d(0,0, new Rotation2d(0))
     );
 
-    SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
-    setModuleStates(states);
+    this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
+  }
+
+  public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
+    if (fieldRelative) {
+      if (rightStick.getNorm() > 0.1) {
+        rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1));
+      }
+
+      double rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
+
+      // Use the left joystick to set speed. Apply a quadratic curve and the set max speed.
+      Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
+
+      // Convert field-relative speeds to robot-relative speeds.
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
+
+    } else {
+      // Create robot-relative speeds.
+      chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
+    }
+    setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
   }
 
   /**
@@ -77,6 +106,7 @@ public class SwerveDrive extends SubsystemBase {
    * @param desiredStates Array of module states to set.
    */
   public void setModuleStates(SwerveModuleState[] desiredStates) {
+    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
     for (int i = 0; i < desiredStates.length; i++) {
       SwerveModule module = modules[i];
       SwerveModuleState state = desiredStates[i];
@@ -84,11 +114,33 @@ public class SwerveDrive extends SubsystemBase {
     }
   }
 
+  public double getGyroAngle() {
+    return gyro.getAngle();
+  }
+
+  public void resetGyro() {
+    gyro.reset();
+    // setOdometry(getOdometry());
+    rotTarget = new Rotation2d(0);
+  }
+
   /**
    * Updates the odometry of the SwerveDrive.
    */
-  public void updateOdometry() {
-    odometry.update(
+  // public void updateOdometry() {
+  //   odometry.update(
+  //     gyro.getRotation2d(), 
+  //     new SwerveModulePosition[] {
+  //       leftFront.getPosition(),
+  //       rightFront.getPosition(),
+  //       leftBack.getPosition(),
+  //       rightBack.getPosition()
+  //     }
+  //   );
+  // }
+
+  public void updatePoseEstimator() {
+    poseEstimator.update(
       gyro.getRotation2d(), 
       new SwerveModulePosition[] {
         leftFront.getPosition(),
@@ -103,16 +155,33 @@ public class SwerveDrive extends SubsystemBase {
    * Gets the odometry of the SwerveDrive.
    * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta).
    */
-  public Pose2d getOdometry() {
-    return odometry.getPoseMeters();
+  // public Pose2d getOdometry() {
+  //   return odometry.getPoseMeters();
+  // }
+
+  public Pose2d getPoseEstimator() {
+    return poseEstimator.getEstimatedPosition();
   }
 
   /**
    * Sets the odometry of the SwerveDrive.
    * @param pose Pose to set the odometry to.
    */
-  public void setOdometry(Pose2d pose) {
-    odometry.resetPosition(
+  // public void setOdometry(Pose2d pose) {
+  //   odometry.resetPosition(
+  //     gyro.getRotation2d(), 
+  //     new SwerveModulePosition[] {
+  //       leftFront.getPosition(),
+  //       rightFront.getPosition(),
+  //       leftBack.getPosition(),
+  //       rightBack.getPosition()
+  //     },
+  //     pose
+  //   );
+  // }
+
+  public void setPoseEstimator(Pose2d pose) {
+    poseEstimator.resetPosition(
       gyro.getRotation2d(), 
       new SwerveModulePosition[] {
         leftFront.getPosition(),
@@ -124,27 +193,41 @@ public class SwerveDrive extends SubsystemBase {
     );
   }
 
+  public void resetPoseEstimator() {
+    setPoseEstimator(new Pose2d());
+  }
+  
+  public void stopModules() {
+    for (SwerveModule module : this.modules) {
+      module.stop();
+    }
+  }
+
   /**
    * Resets the odometry of the SwerveDrive to 0.
-   * *NOTE: If you reset your gyroscope or wheel encoders, this method MUST be called with the new gyro angle and wheel encoder positions.
+   * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions.
    */
-  public void resetOdometry() {
-    odometry.resetPosition(
-      gyro.getRotation2d(), 
-      new SwerveModulePosition[] {
-        leftFront.getPosition(),
-        rightFront.getPosition(),
-        leftBack.getPosition(),
-        rightBack.getPosition()
-      },
-      new Pose2d()
-    );
+  // public void resetOdometry() {
+  //   setOdometry(new Pose2d());
+  // }
+
+  public SwerveDriveKinematics getKinematics() {
+    return this.kinematics;
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    updateOdometry();
+
+    // updateOdometry();
+    updatePoseEstimator();
+
+    // SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX()));
+    // SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY()));
+    // SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees());
+
+    // SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
+    // SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees());
   }
 
   /**

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

@@ -10,7 +10,6 @@ import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
 import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
-import com.ctre.phoenix.sensors.CANCoderConfiguration;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -23,15 +22,15 @@ import frc4388.utility.Gains;
 public class SwerveModule extends SubsystemBase {
     private WPI_TalonFX driveMotor;
     private WPI_TalonFX angleMotor;
-    private CANCoder canCoder;
+    private CANCoder encoder;
 
     public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
   
     /** Creates a new SwerveModule. */
-    public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
+    public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
         this.driveMotor = driveMotor;
         this.angleMotor = angleMotor;
-        this.canCoder = canCoder;
+        this.encoder = encoder;
 
         TalonFXConfiguration angleConfig = new TalonFXConfiguration();
         angleConfig.slot0.kP = swerveGains.kP;
@@ -39,14 +38,15 @@ public class SwerveModule extends SubsystemBase {
         angleConfig.slot0.kD = swerveGains.kD;
 
         // use the CANcoder as the remote sensor for the primary TalonFX PID
-        angleConfig.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID();
+        angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
         angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
         angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
         angleMotor.configAllSettings(angleConfig);
 
-        CANCoderConfiguration canCoderConfig = new CANCoderConfiguration();
-        canCoderConfig.magnetOffsetDegrees = offset;
-        canCoder.configAllSettings(canCoderConfig);
+        encoder.configMagnetOffset(offset);
+        
+        driveMotor.setSelectedSensorPosition(0);
+        driveMotor.config_kP(0, 0.2);
     }
 
     /**
@@ -70,7 +70,7 @@ public class SwerveModule extends SubsystemBase {
      * @return the CANcoder of the SwerveModule
      */
     public CANCoder getEncoder() {
-        return this.canCoder;
+        return this.encoder;
     }
 
     /**
@@ -78,8 +78,20 @@ public class SwerveModule extends SubsystemBase {
      * @return the angle of a SwerveModule as a Rotation2d
      */
     public Rotation2d getAngle() {
-        // Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees. 
-        return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
+        // * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
+        return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
+    }
+    
+    public double getAngularVel() {
+        return this.angleMotor.getSelectedSensorVelocity();
+    }
+
+    public double getDrivePos() {
+        return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV;
+    }
+
+    public double getDriveVel() {
+        return this.driveMotor.getSelectedSensorVelocity(0);
     }
 
     public void stop() {
@@ -126,15 +138,17 @@ public class SwerveModule extends SubsystemBase {
         double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
 
         // convert the CANCoder from its position reading to ticks
-        double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient();
+        double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
+
         angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
 
         double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
-        driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND);
+
+        driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
     }
 
-    public void reset() {
-        canCoder.setPositionToAbsolute();
+    public void reset(double position) {
+        encoder.setPositionToAbsolute();
     }
 
     public double getCurrent() {
@@ -144,4 +158,4 @@ public class SwerveModule extends SubsystemBase {
     public double getVoltage() {
         return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
     }
-}
+}

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

@@ -0,0 +1,39 @@
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class Vision {
+    private final NetworkTableEntry m_isTags;
+    private final NetworkTableEntry m_xPoses;
+    private final NetworkTableEntry m_yPoses;
+    private final NetworkTableEntry m_zPoses;
+
+    public Vision() {
+        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
+
+        m_isTags = tagTable.getEntry("IsTag");
+        m_xPoses = tagTable.getEntry("TagPosX");
+        m_yPoses = tagTable.getEntry("TagPosY");
+        m_zPoses = tagTable.getEntry("TagPosZ");
+    }
+
+    public AprilTag[] getAprilTags() {
+        if (!m_isTags.getBoolean(false)) return new AprilTag[0];
+
+        double xarr[] = m_xPoses.getDoubleArray(new double[] {});
+        double yarr[] = m_yPoses.getDoubleArray(new double[] {});
+        double zarr[] = m_zPoses.getDoubleArray(new double[] {});
+
+        AprilTag tags[] = new AprilTag[xarr.length];
+        for (int i = 0; i < tags.length; i++) {
+            tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d()));
+        }
+
+        return tags;
+    }
+}