Merge pull request #32 from Team4388/vision-april-lime

Merge Vision / Odometry branch

simgui.json

@@ -3,5 +3,73 @@
     "types": {
       "/FMSInfo": "FMSInfo"
     }
+  },
+  "NetworkTables": {
+    "Persistent Values": {
+      "open": false
+    },
+    "Retained Values": {
+      "open": false
+    },
+    "Transitory Values": {
+      "open": false
+    },
+    "retained": {
+      "apriltag": {
+        "open": true
+      }
+    },
+    "transitory": {
+      "LiveWindow": {
+        ".status": {
+          "open": true
+        }
+      },
+      "Shuffleboard": {
+        ".recording": {
+          "open": true
+        },
+        "open": true
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "Clients": {
+      "open": true
+    },
+    "NT3@192.168.1.132:40708": {
+      "Publishers": {
+        "open": true
+      },
+      "Subscribers": {
+        "open": true
+      },
+      "open": true
+    },
+    "NT3@192.168.1.132:49546": {
+      "Publishers": {
+        "open": true
+      },
+      "Subscribers": {
+        "open": true
+      },
+      "open": true
+    },
+    "NT3@192.168.1.132:52724": {
+      "Publishers": {
+        "open": true
+      },
+      "Subscribers": {
+        "open": true
+      },
+      "open": true
+    },
+    "shuffleboard": {
+      "Subscribers": {
+        "open": true
+      },
+      "open": true
+    },
+    "visible": true
   }
 }

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

@@ -63,8 +63,8 @@ public final class Constants {
     public static final class Conversions {
       public static final int CANCODER_TICKS_PER_ROTATION = 4096;
 
-      public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value
+      public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 5.8;
-      public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 1.0; // TODO: find the actual value
+      public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8;
 
       public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
       public static final double MOTOR_REV_PER_STEER_REV = 12.8;
@@ -86,22 +86,6 @@ public final class Constants {
       public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
       public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
       public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
-
-      // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // * 2022 SwerveDrive values
-      // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // * 2022 SwerveDrive values
-      // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // * 2022 SwerveDrive values
-      // public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // * 2022 SwerveDrive values
-      
-      // public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // * 2023 translated values (don't work)
-      // public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // * 2023 translated values (don't work)
-      // public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // * 2023 translated values (don't work)
-      // public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // * 2023 translated values (don't work)
-      
-      // public static final double LEFT_FRONT_ENCODER_OFFSET = 0.0 + 90.0; // * 2023 experimentally-derived values (mostly work) 
-      // public static final double RIGHT_FRONT_ENCODER_OFFSET = 0.0; // * 2023 experimentally-derived values (mostly work) 
-      // public static final double LEFT_BACK_ENCODER_OFFSET = 0.0 + 24.0; // * 2023 experimentally-derived values (mostly work) 
-      // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0 + 45.0 + 180.0; // * 2023 experimentally-derived values (mostly work)  
-      
     }
 
     public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value
@@ -137,10 +121,8 @@ public final class Constants {
   public static final class OIConstants {
     public static final int XBOX_DRIVER_ID = 0;
     public static final int XBOX_OPERATOR_ID = 1;
-    
     public static final double LEFT_AXIS_DEADBAND = 0.1;
     public static final double RIGHT_AXIS_DEADBAND = 0.6;
-
+    public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing
-    public static final boolean SKEW_STICKS = true; 
   }
 }

src/main/java/frc4388/robot/Robot.java

@@ -7,6 +7,9 @@
 
 package frc4388.robot;
 
+import java.lang.System;
+import java.lang.reflect.Array;
+import java.util.Arrays;
 import java.io.File;
 import java.io.IOException;
 import java.io.PrintWriter;
@@ -18,6 +21,10 @@ import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc4388.utility.RobotTime;
 
+import frc4388.robot.subsystems.Location;
+import frc4388.robot.subsystems.Apriltags.Tag;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
 /**
  * The VM is configured to automatically run this class, and to call the
  * functions corresponding to each mode, as described in the TimedRobot
@@ -31,6 +38,8 @@ public class Robot extends TimedRobot {
   private RobotTime m_robotTime = RobotTime.getInstance();
   private RobotContainer m_robotContainer;
 
+  private Location location = new Location();
+
 
   /**
    * This function is run when the robot is first started up and should be
@@ -61,6 +70,13 @@ public class Robot extends TimedRobot {
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
+
+    final Tag pos = location.getPosRot();
+    if (pos != null) {
+      SmartDashboard.putNumber("x position", pos.x);
+    }
+
+    //ystem.out.print(apriltagPos[0]);
   }
 
   /**

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

@@ -35,6 +35,8 @@ import frc4388.robot.Constants.*;
 import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
 import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants;
 import frc4388.robot.commands.AutoBalance;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.controller.IHandController;
 import frc4388.robot.commands.JoystickPlayback;
 import frc4388.robot.commands.JoystickRecorder;
 import frc4388.robot.commands.PlaybackChooser;

src/main/java/frc4388/robot/RobotMap.java

@@ -7,8 +7,8 @@
 
 package frc4388.robot;
 
-import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.WPI_Pigeon2;
 

src/main/java/frc4388/robot/commands/AutoBalance.java

@@ -34,6 +34,7 @@ public class AutoBalance extends PelvicInflammatoryDisease {
 	@Override
 	public void runWithOutput(double output) {
 		double out2 = MathUtil.clamp(output / 40, -.5, .5);
+
 		if (Math.abs(getError()) < 3) out2 = 0;
 		drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false);
 	}

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/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,19 +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.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.RobotGyro;
 
@@ -43,6 +40,7 @@ public class SwerveDrive extends SubsystemBase {
   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();
 
@@ -52,6 +50,7 @@ public class SwerveDrive extends SubsystemBase {
     this.rightFront = rightFront;
     this.leftBack = leftBack;
     this.rightBack = rightBack;
+    
     this.gyro = gyro;
 
     // this.odometry = new SwerveDriveOdometry(
@@ -81,7 +80,6 @@ public class SwerveDrive extends SubsystemBase {
     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) {
@@ -112,7 +110,7 @@ public class SwerveDrive extends SubsystemBase {
     for (int i = 0; i < desiredStates.length; i++) {
       SwerveModule module = modules[i];
       SwerveModuleState state = desiredStates[i];
-      module.setDesiredState(state, false);
+      module.setDesiredState(state);
     }
   }
 
@@ -230,7 +228,6 @@ public class SwerveDrive extends SubsystemBase {
 
     // SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
     // SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees());
-
   }
 
   /**

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

@@ -126,7 +126,7 @@ public class SwerveModule extends SubsystemBase {
      * Set the speed and rotation of the SwerveModule from a SwerveModuleState object
      * @param desiredState a SwerveModuleState representing the desired new state of the module
      */
-    public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
+    public void setDesiredState(SwerveModuleState desiredState) {
         Rotation2d currentRotation = this.getAngle();
 
         SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
@@ -140,18 +140,11 @@ public class SwerveModule extends SubsystemBase {
         // convert the CANCoder from its position reading to ticks
         double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
 
-        if (!ignoreAngle) {
+        angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
-            angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
-        }
 
         double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
-        // double inchesPerSecond = Units.metersToFeet(state.speedMetersPerSecond) * 12;
 
-        // driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME);
-        // driveMotor.set(0.1);
-        // double angleCorrection = getAngularVel() * 2.69;
         driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
-        // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME);
     }
 
     public void reset(double position) {

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;
+    }
+}