Add toml stuff

- Weston J.

Author: PhyXTGears-1720

src/main/deploy/config.toml

@@ -7,3 +7,31 @@ backRightAbsEncoderOffset  =   -30.6
 swerve.turn.kP = 2.0
 swerve.turn.kI = 0.01
 swerve.turn.kD = 3.0
+
+[vision]
+
+strafeSpeed = 0.22
+turnSpeed = 0.58
+forwardSpeed = 1.0
+
+maxLinearSpeed = 1.9
+
+maxAngularSpeed = 1.2
+
+minMoveSpeedLinear = 0.03
+
+minMoveSpeedAngular = 0.1
+
+distanceMultiplierForwards = 0.65
+distanceMultiplierStafing = 0.07
+distanceMultiplierTurning = 0.4
+
+LIMELIGHT_INIT_FAILED_TIME = 0.4
+
+STOP_MOVING_THERSHOLD = 0.58
+STAFE_THERSHOLD = 1.5
+TURNING_THERSHOLD = 5.0
+
+DISTANCE_WANTED_FOR_ROBOT = 0.19
+
+DEBUG_MODE = false

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

@@ -71,9 +71,18 @@ public void robotInit() {
 
         kDrivetrain = new Drivetrain(table.get());
 
-        kVisionSubsystem = new Vision();
+        // init vision
+
+        table = config.getTable("vision");
+
+        if (table.isEmpty()) {
+            throw new Error("Unable to find config table 'vision' ");
+        }
+
+        kVisionSubsystem = new Vision(table.get());
         kVisionAutoCommand = new VisionAutoCommand(kVisionSubsystem, kDrivetrain, 10, Units.Milliseconds.of(20.0));
         mVisionTeleopCommand = new VisionTeleopCommand(kVisionSubsystem, kDrivetrain, -1, Units.Milliseconds.of(20.0));
+        
         kDriveTeleopCommand = new DriveTeleopCommand(kDrivetrain, kDriveController, Units.Milliseconds.of(20.0));
 
         m_chooser.setDefaultOption("Default Auto", kDefaultAuto);

src/main/java/frc/robot/commands/visionDriveCommand/VisionAutoCommand.java

@@ -1,69 +1,48 @@
 package frc.robot.commands.visionDriveCommand;
 
-
 import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 
+
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.units.Units;
-import  edu.wpi.first.units.measure.*;
+import edu.wpi.first.units.measure.*;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.drive.*;
 import frc.robot.subsystems.vision.*;
 
 public class VisionAutoCommand extends Command {
-    
+
     // config
-    // for now instead of a toml FIXME:
-    public static final double STOP_MOVING_THERSHOLD = 0.58;
-    public static final double STAFE_THERSHOLD = 1.5;
-    public static final double TURNING_THERSHOLD = 5.0;
-    
+
     private final Vision kVisionSubsystem;
     private final Drivetrain kDrivetrain;
     private final Time kRobotPeriod;
-    
-    // config
-    
-    private final LinearVelocity kStrafeSpeed = Units.MetersPerSecond.of(0.22);
-    private final AngularVelocity kTurnSpeed = Units.RadiansPerSecond.of(0.58);
-    private final LinearVelocity kForwardSpeed = Units.MetersPerSecond.of(1.0);
-    
-    
-    private final LinearVelocity kMaxLinearSpeed = Units.MetersPerSecond.of(1.9);
-    
-    private final AngularVelocity kMaxAngularSpeed = Units.RadiansPerSecond.of(1.2);
-    
-    private final double kDistanceMultiplierForwards = 0.65;
-    private final double kDistanceMultiplierStafing = 0.07;
-    private final double kDistanceMultiplierTurning = 0.4;
-
-    private final LinearVelocity kMinMoveSpeedLinear = Units.MetersPerSecond.of(0.03);
-    private final AngularVelocity kMinMoveSpeedAngular = Units.RadiansPerSecond.of(0.1);
 
     private boolean mIsDoneStafing = false;
     private boolean mIsDoneMoving = false;
-    
-    
-    private int mTargetDriveAprilTagId;
+
+    private final int mTargetDriveAprilTagId;
 
     private AngularVelocity mAngularVelocity = Units.RadiansPerSecond.zero();
     private LinearVelocity mStafeVelocity = Units.MetersPerSecond.zero();
     private LinearVelocity mForwardVelocity = Units.MetersPerSecond.zero();
 
-    
+    private Vision.Config mConfig;
+
     public VisionAutoCommand(
-        Vision visionSubsystem,
-        Drivetrain drivetrain,
-        int targetDriveAprilTagId,
-        Time period
-    ) {
+            Vision visionSubsystem,
+            Drivetrain drivetrain,
+            int targetDriveAprilTagId,
+            Time period) {
         mTargetDriveAprilTagId = targetDriveAprilTagId;
         kVisionSubsystem = visionSubsystem;
         kDrivetrain = drivetrain;
         kRobotPeriod = period;
 
+        mConfig = kVisionSubsystem.getConfigCopy();
+
         addRequirements(kDrivetrain, kVisionSubsystem);
     }
 
@@ -72,112 +51,135 @@ public void initialize() {
         // reset some vars
         mIsDoneMoving = false;
         mIsDoneStafing = false;
+
     }
 
     @Override
     public void execute() {
-        
+
+        if (mTargetDriveAprilTagId == -1)
+            return;
+
         Vision.VisionTagData data = kVisionSubsystem.robotDriveToAprilTag(mTargetDriveAprilTagId, 0.0);
 
         double heading = kDrivetrain.getHeading().in(Units.Degree);
-        double targetHeading = fmod(Vision.rotations[mTargetDriveAprilTagId-1], 360.0, 180.0);
+        double targetHeading = fmod(Vision.rotations[mTargetDriveAprilTagId - 1], 360.0, 180.0);
         double distanceToTargetHeading = fmod(targetHeading - heading, 360.0, 180.0);
 
         mAngularVelocity = Units.RadiansPerSecond.zero();
         mStafeVelocity = Units.MetersPerSecond.zero();
         mForwardVelocity = Units.MetersPerSecond.zero();
 
-        //System.out.println("[drive progress] " + Boolean.toString(mIsDoneMoving) + ", " + Boolean.toString(mIsDoneStafing));
-        
-        // System.out.println( heading + "-> " + targetHeading + " [DISTANCE] " + distanceToTargetHeading + ", " + Boolean.toString(distanceToTargetHeading > Vision.TURNING_THERSHOLD));
-        
+        // init teleop
+
+        kVisionSubsystem.setFilter(-1);
+
+        // System.out.println("[drive progress] " + Boolean.toString(mIsDoneMoving) + ",
+        // " + Boolean.toString(mIsDoneStafing));
+
+        // System.out.println( heading + "-> " + targetHeading + " [DISTANCE] " +
+        // distanceToTargetHeading + ", " + Boolean.toString(distanceToTargetHeading >
+        // Vision.TURNING_THERSHOLD));
+
         // turn code
-        // detect if we are not done stafing and moving then try and rotate the robot to face the april tag
-        if (!(mIsDoneMoving && mIsDoneStafing) && Math.abs(distanceToTargetHeading) > TURNING_THERSHOLD) {
+        // detect if we are not done stafing and moving then try and rotate the robot to
+        // face the april tag
+        if (!(mIsDoneMoving && mIsDoneStafing) && Math.abs(distanceToTargetHeading) > mConfig.TURNING_THERSHOLD) {
             if (distanceToTargetHeading > 0.0) {
-                mAngularVelocity = kTurnSpeed.times(distanceToTargetHeading * kDistanceMultiplierTurning).plus(kMinMoveSpeedAngular);
-            }
-            else {
-                mAngularVelocity = kTurnSpeed.times(distanceToTargetHeading * kDistanceMultiplierTurning).minus(kMinMoveSpeedAngular);
+                mAngularVelocity = mConfig.kTurnSpeed.times(distanceToTargetHeading * mConfig.kDistanceMultiplierTurning)
+                        .plus(mConfig.kMinMoveSpeedAngular);
+            } else {
+                mAngularVelocity = mConfig.kTurnSpeed.times(distanceToTargetHeading * mConfig.kDistanceMultiplierTurning)
+                        .minus(mConfig.kMinMoveSpeedAngular);
             }
         }
 
         SmartDashboard.putNumber("target heading", targetHeading);
         SmartDashboard.putNumber("heading error", distanceToTargetHeading);
-        
+
         // see if we can see THE april tag
         if (kVisionSubsystem.isAprilTagDetected()) {
-            //System.out.printf("[drive progress] fwd dist: %f, strf dist: %f\n", data.mDistanceToAprilTag, data.mTurningDistance);
+            // System.out.printf("[drive progress] fwd dist: %f, strf dist: %f\n",
+            // data.mDistanceToAprilTag, data.mTurningDistance);
             if (!(mIsDoneStafing && mIsDoneMoving)) {
 
                 SmartDashboard.putNumber("Strafe Distance", data.mTurningDistance);
                 SmartDashboard.putNumber("Forward Distance", data.mDistanceToAprilTag);
 
                 // if we are not in the turning thershold
-                if (Math.abs(data.mTurningDistance) > STAFE_THERSHOLD) {
+                if (Math.abs(data.mTurningDistance) > mConfig.STAFE_THERSHOLD) {
                     if (data.mTurningDistance > 0.0) {
-                        //System.out.println("Turn right");
-                        
-                        mStafeVelocity = kStrafeSpeed.times(Math.abs(data.mTurningDistance * kDistanceMultiplierStafing)).plus(kMinMoveSpeedLinear);
-                        
+                        // System.out.println("Turn right");
+
+                        mStafeVelocity = mConfig.kStrafeSpeed
+                                .times(Math.abs(data.mTurningDistance * mConfig.kDistanceMultiplierStafing))
+                                .plus(mConfig.kMinMoveSpeedLinear);
+
                     } else {
-                        //System.out.println("Turn left");
-                        
-                        mStafeVelocity = kStrafeSpeed.times(Math.abs(data.mTurningDistance * kDistanceMultiplierStafing)).plus(kMinMoveSpeedLinear).negate();
+                        // System.out.println("Turn left");
+
+                        mStafeVelocity = mConfig.kStrafeSpeed
+                                .times(Math.abs(data.mTurningDistance * mConfig.kDistanceMultiplierStafing))
+                                .plus(mConfig.kMinMoveSpeedLinear).negate();
 
                     }
-                    
-                }  
+
+                }
                 // this means that we are in the STAFE_THERSHOLD so we can stop stafing
                 else {
                     mIsDoneStafing = true;
                 }
-                
-                
+
                 // see if we are in the STOP_MOVING_THERSHOLD if so then stop
-                if (Math.abs(0.0 - data.mDistanceToAprilTag) > STOP_MOVING_THERSHOLD) {
+                if (Math.abs(0.0 - data.mDistanceToAprilTag) > mConfig.STOP_MOVING_THERSHOLD) {
                     // detect if we are too far from the april tag if so then have forwards
                     if (data.mDistanceToAprilTag > 0.0) {
-                        //System.out.println("backwards");
-                        
-                        mForwardVelocity = kForwardSpeed.times(Math.abs(data.mDistanceToAprilTag * kDistanceMultiplierForwards)).plus(kMinMoveSpeedLinear).negate();
-                        
-                    }
-                    else {
-                        //System.out.println("forwards");
-                        
-                        mForwardVelocity = kForwardSpeed.times(Math.abs(data.mDistanceToAprilTag * kDistanceMultiplierForwards)).plus(kMinMoveSpeedLinear);
+                        // System.out.println("backwards");
+
+                        mForwardVelocity = mConfig.kForwardSpeed
+                                .times(Math.abs(data.mDistanceToAprilTag * mConfig.kDistanceMultiplierForwards))
+                                .plus(mConfig.kMinMoveSpeedLinear).negate();
+
+                    } else {
+                        // System.out.println("forwards");
+
+                        mForwardVelocity = mConfig.kForwardSpeed
+                                .times(Math.abs(data.mDistanceToAprilTag * mConfig.kDistanceMultiplierForwards))
+                                .plus(mConfig.kMinMoveSpeedLinear);
                     }
-                }
-                else{
+                } else {
                     mIsDoneMoving = true;
                 }
-            } 
+            }
         }
-        
-        
+
         // clamp just in case
-        
-        mForwardVelocity = clamp(mForwardVelocity, kMaxLinearSpeed.negate(), kMaxLinearSpeed);
-        mStafeVelocity = clamp(mStafeVelocity, kMaxLinearSpeed.negate(), kMaxLinearSpeed);
-        mAngularVelocity = clamp(mAngularVelocity, kMaxAngularSpeed.negate(), kMaxAngularSpeed);
+
+        mForwardVelocity = clamp(mForwardVelocity, mConfig.kMaxLinearSpeed.negate(), mConfig.kMaxLinearSpeed);
+        mStafeVelocity = clamp(mStafeVelocity, mConfig.kMaxLinearSpeed.negate(), mConfig.kMaxLinearSpeed);
+        mAngularVelocity = clamp(mAngularVelocity, mConfig.kMaxAngularSpeed.negate(), mConfig.kMaxAngularSpeed);
 
         // now drive
-        
-        kDrivetrain.Drive(
-            mForwardVelocity,
-            mStafeVelocity,
-            mAngularVelocity,
-            false,
-            kRobotPeriod
-        );
 
+        kDrivetrain.Drive(
+                mForwardVelocity,
+                mStafeVelocity,
+                mAngularVelocity,
+                false,
+                kRobotPeriod);
 
     }
 
     @Override
     public boolean isFinished() {
+        if (mIsDoneMoving && !mIsDoneStafing) {
+            System.out.println("stafing is holding us up");
+        } else if (mIsDoneStafing && !mIsDoneMoving) {
+            System.out.println("moving is holding us up");
+        }
+
         if ((mIsDoneStafing && mIsDoneMoving)) {
+
             System.out.println("Done");
 
             return true;
@@ -188,32 +190,29 @@ public boolean isFinished() {
     @Override
     public void end(boolean i) {
         kDrivetrain.Drive(
-            Units.MetersPerSecond.zero(),
-            Units.MetersPerSecond.zero(),
-            Units.RadiansPerSecond.zero(),
-            false,
-            kRobotPeriod
-        );
+                Units.MetersPerSecond.zero(),
+                Units.MetersPerSecond.zero(),
+                Units.RadiansPerSecond.zero(),
+                false,
+                kRobotPeriod);
     }
 
     private static LinearVelocity clamp(LinearVelocity _value, LinearVelocity _min, LinearVelocity _max) {
         double value = _value.in(MetersPerSecond);
         double min = _min.in(MetersPerSecond);
         double max = _max.in(MetersPerSecond);
-        
+
         return MetersPerSecond.of(MathUtil.clamp(value, min, max));
     }
 
     private static AngularVelocity clamp(AngularVelocity _value, AngularVelocity _min, AngularVelocity _max) {
         double value = _value.in(RadiansPerSecond);
         double min = _min.in(RadiansPerSecond);
         double max = _max.in(RadiansPerSecond);
-        
+
         return RadiansPerSecond.of(MathUtil.clamp(value, min, max));
     }
 
-    
-
     private static double fmod(double a, double b, double bias) {
         double absA = a + bias;
         double absB = absA % b;