Kalman merge

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

@@ -2,9 +2,15 @@
 
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Transform2d;
 import frc.robot.valuetuner.WebConstant;
 import org.apache.commons.lang.math.DoubleRange;
 
+import java.lang.reflect.Field;
+import java.lang.reflect.Modifier;
+import java.util.Arrays;
+import java.util.Optional;
+
 import static org.apache.commons.lang3.ObjectUtils.CONST;
 
 /**
@@ -15,6 +21,7 @@
 public class Constants {
     public static final int TALON_TIMEOUT = 10;
     public static double BACK_BUMPER_TO_CENTER = 0.47; //TODO: Tune for real world
+    public static final double EFFECTIVE_TURN_WIDTH = 0.73; // wheel center to wheel center [m]
 
     public static class Drivetrain {
         //Remember! High gear == High speed!
@@ -36,6 +43,12 @@ public static class Drivetrain {
         public static final double SHIFT_SPEED_TOLERANCE = 0.5; // Stops the robot from shifting while the robot is too fast
         public static final double GRAVITY_ACCELERATION = 9.80665;
 
+        public static final boolean RIGHT_MASTER_INVERTED = true;
+        public static final boolean RIGHT_SLAVE_INVERTED = true;
+        public static final boolean LEFT_MASTER_INVERTED = false;
+        public static final boolean LEFT_SLAVE_INVERTED = false;
+        public static final boolean GYRO_INVERTED = true;
+
         public static final double JOYSTICK_END_THRESHOLD = 0;
 
         public static final double JOYSTICK_MIN_THRESHOLD = 0.04;
@@ -67,12 +80,12 @@ public static class Vision {
     }
 
     public static class FieldGeometry {
-        public static final Pose2d RED_OUTER_POWER_PORT_LOCATION = new Pose2d(15.98, 2.42, new Rotation2d());
-        public static final Pose2d RED_INNER_POWER_PORT_LOCATION = new Pose2d(15.98 + 0.78, 2.42, new Rotation2d());
+        public static final double FIELD_WIDTH = 8.2296 ;
+        public static final Pose2d RED_OUTER_POWER_PORT_LOCATION = new Pose2d(16.15, 2.42, new Rotation2d()); // The opponent location is (x: 0, y: 2.4).
+        public static final Pose2d RED_INNER_POWER_PORT_LOCATION = new Pose2d(16.15 + 0.78, 2.42, new Rotation2d()); // The opponent location is (x: -0.78, y: 2.4).public static final Pose2d RED_OUTER_POWER_PORT_LOCATION = new Pose2d(15.98, 2.42, new Rotation2d()); // The opponent location is (x: 0, y: 2.4).
+        public static final Pose2d BLUE_OUTER_POWER_PORT_LOCATION = new Pose2d(0, 5.79, new Rotation2d()); // The opponent location is (x: 0, y: 2.4).
+        public static final Pose2d BLUE_INNER_POWER_PORT_LOCATION = new Pose2d(-0.78, 5.79, new Rotation2d()); // The opponent location is (x: -0.78, y: 2.4).
         public static final double OUTER_PORT_TO_LINE = 2.28; //TODO: Tune for real world
-//        public static final Pose2d BLUE_OUTER_POWER_PORT_LOCATION = new Pose2d(0, 5.79, new Rotation2d()); // The opponent location is (x: 0, y: 2.4).
-//        public static final Pose2d BLUE_INNER_POWER_PORT_LOCATION = new Pose2d(-0.78, 5.79, new Rotation2d()); // The opponent location is (x: -0.78, y: 2.4).
-
         public static final double PORT_HEIGHT = 2.4;
     }
 
@@ -119,6 +132,8 @@ public static class Turret {
         public static final DoubleRange ALLOWED_ANGLES = new DoubleRange(-42, 264);
         public static final DoubleRange DEAD_ZONE_ANGLES = new DoubleRange(38, 86);
 
+        public static final double TURRET_SWITCHING_TOLERANCE = 20;
+
         public static final double UNREACHABLE_ANGLE = 300; //This is an angle which the turret can't mechanically pass. If the turret passes this angle from either direction before startup, the turret will malfunction.
         public static final int ZERO_POSITION = CONST(1600); //Encoder absolute position when the turret is facing forward. This might change occasionally.
 
@@ -259,7 +274,7 @@ public static class Autonomous {
 
     public static class Turret {
         public static final double KD = 150;
-        public static final int ZERO_POSITION = 777;
+        public static final int ZERO_POSITION = 644;
         public static final DoubleRange ALLOWED_ANGLES = new DoubleRange(-41, 227);
     }
 

src/main/java/frc/robot/KalmanRange/KalmanRangeEstimator.java

@@ -0,0 +1,66 @@
+package frc.robot.KalmanRange;
+
+import frc.robot.subsystems.drivetrain.EKF.KalmanFilter;
+import frc.robot.subsystems.drivetrain.EKF.ObservationModel;
+import frc.robot.subsystems.drivetrain.KalmanLocalization.OdometryInertialObservation;
+import frc.robot.subsystems.drivetrain.KalmanLocalization.OdometryInertialProcess;
+
+public class KalmanRangeEstimator {
+    public  KalmanFilter filter;
+    private Linear1dProcessModel process;
+    private Linear1dObservationModel observation;
+    double  m_proportional_range_err = 0.05; // Initiliaze to 5% of range
+
+    /**
+     * Constructs a KalmanRangeEstimator object.
+     *
+     * @param proportional_range_err         expected vision error relative to range. For example, 0.05 is 5% error.
+     */
+    public KalmanRangeEstimator(double proportional_range_err)
+    {
+        observation = new Linear1dObservationModel();
+        process = new Linear1dProcessModel();
+        filter = new KalmanFilter(process);
+
+    }
+
+
+    /**
+     * Returns the current best estimate of the range
+     */
+    public double GetRange() {return filter.model.state_estimate.data[0][0];}
+
+
+    /**
+     * Resets the filter to initiate state - when range measurements stopped.
+     */
+    public void Reset() {
+        filter.model.initialState(filter.model.state_estimate.data);
+        filter.model.initialStateCovariance(filter.model.estimate_covariance.data);
+    }
+
+    /**
+     * Filters the range
+     *
+     * @param R - measured range in meters
+     * @param time - current time in seconds
+     * @param R_valid - if the measurement is valid. If invalid the filter will continue on prediction
+     */
+    public double FilterRange(double R,double time, boolean R_valid)
+    {
+        if (R_valid) {
+            observation.SetObservationErrorStd(R * m_proportional_range_err);
+        }
+        else
+        {
+            observation.SetObservationErrorStd(1000);
+        }
+        observation.setPosition(R);
+
+        filter.update(time,observation);
+
+        return filter.model.state_estimate.data[0][0];
+    }
+}
+
+

src/main/java/frc/robot/KalmanRange/Linear1dObservationModel.java

@@ -0,0 +1,46 @@
+package frc.robot.KalmanRange;
+
+import frc.robot.subsystems.drivetrain.EKF.ObservationModel;
+
+public class Linear1dObservationModel extends ObservationModel {
+
+	private double mx;
+	private double m_err_cov = 1;
+
+	public void SetObservationErrorStd(double err_std) { m_err_cov = err_std*err_std;}
+
+	public void setPosition(double x) {
+		this.mx = x;
+	}
+
+	@Override
+	public int observationDimension() {
+		return 1;
+	}
+
+	@Override
+	public int stateDimension() {
+		return 2;
+	}
+
+	@Override
+	public void observationMeasurement(double[][] y) {
+		y[0][0] = mx;
+	}
+
+	@Override
+	public void observationModel(double[][] x, double[][] h) {
+		h[0][0] = x[0][0];
+	}
+
+	@Override
+	public void observationModelJacobian(double[][] x, double[][] j) {
+		j[0][0] = 1;
+		j[0][1] = 0;
+	}
+
+	@Override
+	public void observationNoiseCovariance(double[][] cov) {
+		cov[0][0] = m_err_cov;
+	}
+}

src/main/java/frc/robot/KalmanRange/Linear1dProcessModel.java

@@ -0,0 +1,55 @@
+package frc.robot.KalmanRange;
+
+import frc.robot.subsystems.drivetrain.EKF.ProcessModel;
+
+public class Linear1dProcessModel extends ProcessModel {
+
+	@Override
+	public int stateDimension() {
+		return 2;
+	}
+
+	@Override
+	public void initialState(double[][] x) {
+		x[0][0] = 0;
+		x[1][0] = 0;
+	}
+
+	@Override
+	public void initialStateCovariance(double[][] cov) {
+		cov[0][0] = 1000;
+		cov[0][1] = 0;
+		cov[1][0] = 0;
+		cov[1][1] = 1000;
+	}
+
+	@Override
+	public void stateFunction(double[][] x, double[][] f) {
+		f[0][0] = x[1][0];
+		f[1][0] = 0;
+	}
+
+	@Override
+	public void stateFunctionJacobian(double[][] x, double[][] j) {
+		j[0][0] = 0;
+		j[0][1] = 1;
+		j[1][0] = 0;
+		j[1][1] = 0;
+	}
+
+	@Override
+	public void processNoiseCovariance(double[][] cov) {
+		cov[0][0] = 0; // No noise on position
+		cov[0][1] = 0;
+		cov[1][0] = 0;
+		cov[1][1] = 2*2; // assume typical acceleration in  (m/s^2)^2
+	}
+
+	public double getX() {
+		return getState()[0][0];
+	}
+
+	public double getV() {
+		return getState()[1][0];
+	}
+}

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

@@ -8,6 +8,7 @@
 package frc.robot;
 
 import edu.wpi.cscore.UsbCamera;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.*;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -124,6 +125,7 @@ public void robotPeriodic() {
 
         rainbow();
         m_led.setData(m_ledBuffer);
+        NetworkTableInstance.getDefault().flush();
     }
 
     private void rainbow() {
@@ -192,12 +194,12 @@ public void autonomousInit() {
         }
     }
 
-    /**
-     * This function is called periodically during autonomous.
-     */
-    @Override
-    public void autonomousPeriodic() {
-    }
+  /**
+   * This function is called periodically during autonomous.
+   */
+  @Override
+  public void autonomousPeriodic() {
+  }
 
     @Override
     public void teleopInit() {
@@ -211,6 +213,7 @@ public void teleopInit() {
         m_robotContainer.drivetrain.setBrake(false);
     }
 
+
     /**
      * This function is called periodically during operator control.
      */

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

@@ -9,13 +9,18 @@
 
 import com.kauailabs.navx.frc.AHRS;
 import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.*;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.autonomous.ResetOnly;
 import frc.robot.autonomous.ShootAndDriveForward;
 import frc.robot.autonomous.ShootAndDriveToPickup;
 import frc.robot.autonomous.TrenchPickup;
+import frc.robot.commandgroups.OuttakeBalls;
 import frc.robot.commandgroups.PickupBalls;
 import frc.robot.subsystems.climb.Climber;
 import frc.robot.subsystems.climb.commands.PIDClimbAndBalance;
@@ -28,11 +33,11 @@
 import frc.robot.subsystems.conveyor.Conveyor;
 import frc.robot.subsystems.conveyor.commands.FeedTurret;
 import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.drivetrain.FullLocalization;
 import frc.robot.subsystems.drivetrain.commands.GearShift;
 import frc.robot.subsystems.drivetrain.commands.JoystickDrive;
 import frc.robot.subsystems.drivetrain.commands.ResetLocalization;
 import frc.robot.subsystems.intake.Intake;
-import frc.robot.commandgroups.OuttakeBalls;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.commands.SpeedUp;
 import frc.robot.subsystems.turret.Turret;
@@ -41,28 +46,39 @@
 import frc.robot.utilities.CustomDashboard;
 import frc.robot.utilities.VisionModule;
 import frc.robot.valuetuner.ValueTuner;
+import org.ghrobotics.lib.debug.FalconDashboard;
 import org.techfire225.webapp.Webserver;
 
+import static frc.robot.Constants.Drivetrain.GRAVITY_ACCELERATION;
+import static frc.robot.Constants.Drivetrain.GYRO_INVERTED;
+import static frc.robot.Constants.EFFECTIVE_TURN_WIDTH;
+import static frc.robot.subsystems.drivetrain.Drivetrain.localization;
+
 /**
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
  * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
  * (including subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
+
     // The robot's subsystems and commands are defined here...
     public static AHRS navx = new AHRS(SPI.Port.kMXP);
-    public final VisionModule visionModule = new VisionModule();
-    public final CustomDashboard customDashboard = new CustomDashboard();
-    public final Drivetrain drivetrain = new Drivetrain();
-    public final ColorWheel colorWheel = new ColorWheel();
+    private final VisionModule visionModule = new VisionModule();
+    private final CustomDashboard customDashboard = new CustomDashboard();
+    public static final Drivetrain drivetrain = new Drivetrain();
+    private final ColorWheel colorWheel = new ColorWheel();
     public final Shooter shooter = new Shooter();
-    public final Intake intake = new Intake();
-    public final Conveyor conveyor = new Conveyor(intake);
+    private final Intake intake = new Intake();
+    private final Conveyor conveyor = new Conveyor(intake);
     public static final Climber climber = new Climber();
     public static final Turret turret = new Turret();
+
     private final Command m_autoCommand = null;
 
+
+
+
     /**
      * The container for the robot.  Contains subsystems, OI devices, and commands.
      */
@@ -94,7 +110,7 @@ private void configureButtonBindings() {
         OI.y.whileHeld(new PickupBalls(intake, conveyor));
         OI.back.whenPressed(new InstantCommand(CommandScheduler.getInstance()::cancelAll));
         OI.rs.toggleWhenPressed(new RotationControl(colorWheel));
-        OI.start.toggleWhenPressed(new PositionControl(colorWheel));
+        OI.start.toggleWhenPressed(new ResetLocalization(drivetrain));
         OI.ls.whenHeld(new SequentialCommandGroup(
                 new WaitCommand(0.7),
                 new ResetLocalization(drivetrain)
@@ -116,6 +132,7 @@ private void configureButtonBindings() {
         }
     }
 
+
     /**
      * Initiates the value tuner.
      */
@@ -158,4 +175,5 @@ public Command getAutonomousCommand() {
         return null;
     }
 
-}
+}
+