From a4bf56b1d0b93dae5e1762952cb00a7896d6b09e Mon Sep 17 00:00:00 2001 From: Sergey Vichik Date: Sat, 8 Feb 2020 21:12:11 +0200 Subject: [PATCH 01/22] Add localization from vision --- .../drivetrain/FullLocalization.java | 21 +++++- .../OdometryInertialObservation.java | 72 ++++++++++++++++++- .../OdometryInertialProcess.java | 11 ++- 3 files changed, 97 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java b/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java index ff1a6ab1..d24489e4 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java @@ -22,7 +22,9 @@ /*----------------------------------------------------------------------------*/ import static frc.robot.Constants.Drivetrain.GYRO_INVERTED; +import static frc.robot.Constants.FieldGeometry.OUTER_POWER_PORT_LOCATION; import static frc.robot.RobotContainer.drivetrain; +import static frc.robot.RobotContainer.turret; import static frc.robot.RobotContainer.navx; import static java.lang.Math.abs; @@ -65,6 +67,12 @@ public class FullLocalization { private NetworkTableEntry encoderLeft = localizationTable.getEntry("left-encoder"); private NetworkTableEntry encoderRight = localizationTable.getEntry("right-encoder"); + private NetworkTable visionTable = NetworkTableInstance.getDefault().getTable("chameleon-vision").getSubTable("turret"); + private NetworkTableEntry visionAngle = visionTable.getEntry("targetYaw"); + private NetworkTableEntry visionDistance = visionTable.getEntry("distance"); + private NetworkTableEntry visionValid = visionTable.getEntry("isValid"); + + /** * Constructs a DifferentialDriveOdometry object. * @@ -152,21 +160,30 @@ public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters, m_prevLeftDistance = leftDistanceMeters; m_prevRightDistance = rightDistanceMeters; - var angle = gyroAngle; // .plus(m_gyroOffset); TODO think how to use offset + var angle = gyroAngle; // .plus(m_gyroOffset); TODO think how to use offset + double target_angle = turret.getAngle() + visionAngle.getDouble(0); + double target_range = visionDistance.getDouble(1); + + final Pose2d target_POS = OUTER_POWER_PORT_LOCATION; + double dt = time - m_prev_time; // Observation object holds the new measurements - observation.SetMeasurement(gyroAngle.getRadians(), deltaLeftDistance, deltaRightDistance, dt); + observation.SetMeasurement(gyroAngle.getRadians(), deltaLeftDistance, deltaRightDistance,target_range, + Math.toRadians(target_angle), target_POS , dt); // Acceleration enters the process and not the observation process.setAcc(acc); // Check if encoders are valid or slipping: observation.setEncoderValid(EncoderValid(angle, deltaLeftDistance, deltaRightDistance)); + observation.setTargetValid(visionValid.getBoolean(false)); + m_previousAngle = gyroAngle; + // The main estimate step: filter.update(time, observation); diff --git a/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialObservation.java b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialObservation.java index 6a206f92..e3aa4c63 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialObservation.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialObservation.java @@ -1,6 +1,8 @@ package frc.robot.subsystems.drivetrain.KalmanLocalization; +import edu.wpi.first.wpilibj.geometry.Pose2d; import frc.robot.subsystems.drivetrain.EKF.ObservationModel; +import org.opencv.core.Mat; // Class that implements the observation model for localization Kalman filter public class OdometryInertialObservation extends ObservationModel { @@ -14,11 +16,17 @@ public class OdometryInertialObservation extends ObservationModel { private double m_Rl; // distacne from robot IMU to left wheel private boolean m_encoderValid = true; // Encoder valid flag + private double m_Range; + private double m_TargetAngle; + private Pose2d m_TargetPose; + private boolean m_targetValid; public void setEncoderValid(boolean valid){ m_encoderValid = valid; } + public void setTargetValid(boolean valid) { m_targetValid = valid;} + // Constructor must receive the geometry of the robot public OdometryInertialObservation(double Rr, double Rl) { @@ -26,21 +34,36 @@ public OdometryInertialObservation(double Rr, double Rl) m_Rr = Rr; } - public void SetMeasurement(double yaw, double LeftTrack, double RightTrack, double dt){ + /** + * + * @param yaw - body angles in radians + * @param LeftTrack + * @param RightTrack + * @param Range + * @param TargetAngle - view angle of target in radians. + * @param TargetPose + * @param dt + */ + public void SetMeasurement(double yaw, double LeftTrack, double RightTrack, double Range, double TargetAngle, + Pose2d TargetPose, double dt){ m_yaw = yaw; m_LeftTrackSpeed = LeftTrack/dt; m_RightTrackSpeed = RightTrack/dt; + m_Range = Range; + m_TargetAngle = TargetAngle; + m_TargetPose = TargetPose; + } @Override public int observationDimension() { - return 3; + return 5; } @Override public int stateDimension() { - return 6; + return 7; } @Override @@ -48,6 +71,8 @@ public void observationMeasurement(double[][] y) { y[0][0] = m_yaw; y[1][0] = m_LeftTrackSpeed; y[2][0] = m_RightTrackSpeed; + y[3][0] = m_Range*m_Range; + y[4][0] = m_TargetAngle; } @Override @@ -58,19 +83,46 @@ public void observationModel(double[][] x, double[][] h) { double phi = x[3][0]; double omega = x[4][0]; + double dx = m_TargetPose.getTranslation().getX() - X; + double dy = m_TargetPose.getTranslation().getY() - Y; + + h[0][0] = phi; // First measurement is the gyro angle h[1][0] = v + m_Rl*omega; // Second measurement is the left encoder h[2][0] = v - m_Rr*omega; // Third measurement is the right encoder + + h[3][0] = dx*dx + dy*dy; + h[4][0] = Math.IEEEremainder( Math.atan2(dy,dx) - phi,2*Math.PI); + } @Override public void observationModelJacobian(double[][] x, double[][] j) { + double X = x[0][0]; + double Y = x[1][0]; + double v = x[2][0]; + double phi = x[3][0]; + double omega = x[4][0]; + + double dx = m_TargetPose.getTranslation().getX() - X; + double dy = m_TargetPose.getTranslation().getY() - Y; + + double r_sqr = dx*dx + dy*dy; + + j[0][3] = 1; j[1][2] = 1; j[2][2] = 1; j[1][4] = m_Rl; j[2][4] = -m_Rr; // TODO: check if the direction of encoders is correct in Kalman tuning + j[3][0] = -2*dx; + j[3][1] = -2*dy; + + j[4][0] = dy/r_sqr; + j[4][1] = -dx/r_sqr; + + j[4][3] = -1; } @Override @@ -86,6 +138,20 @@ public void observationNoiseCovariance(double[][] cov) { System.out.println("encoders invalid"); } + + if (m_targetValid) + { + // TODO: tune vision and turret errors + cov[3][3] = 1 ; // 10 cm range error for 5 m range (5+0.1)^2 = 25 + 2*0.5 + 0.01; => std = 1 => cov = 1 + cov[4][4] = 1e-4 ; // about 0.5 degrees + } + else + { + cov[3][3] = 1e6; // garbage measurement + cov[4][4] = 1e6; // garbage measurement + + } + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java index 11086f48..412176fa 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java @@ -19,7 +19,7 @@ public void setAcc(double acc) @Override public int stateDimension() { - return 6; + return 7; } @Override @@ -30,6 +30,7 @@ public void initialState(double[][] x) { x[3][0] = 0; x[4][0] = 0; x[5][0] = 0; + x[6][0] = 0; } @@ -43,6 +44,7 @@ public void initialStateCovariance(double[][] cov) { cov[3][3] = 8e-3; // 5 deg sqrd in rad for phi cov[4][4] = 1e-5; // assume not moving : 0.2 deg/s for omega cov[5][5] = 1e0; // assume 1 m/s^2 + cov[6][6] = 100; // assume 10 rad } @Override @@ -53,6 +55,7 @@ public void stateFunction(double[][] x, double[][] f) { double phi = x[3][0]; double omega = x[4][0]; double acc_bias = x[5][0]; + double angle_bias = x[6][0]; // The main system dynamics: f[0][0] = v * cos(phi); // 2 D motion @@ -60,6 +63,8 @@ public void stateFunction(double[][] x, double[][] f) { f[2][0] = m_acc-acc_bias; // Acceleration enters here f[3][0] = omega; // phi derivative is omega f[4][0] = 0; // Assume constant omega + f[5][0] = 0; // Assume constant bias + f[6][0] = 0; // Assume constant bias } @Override @@ -70,6 +75,7 @@ public void stateFunctionJacobian(double[][] x, double[][] j) { double phi = x[3][0]; double omega = x[4][0]; double acc_bias = x[5][0]; + double angle_bias = x[6][0]; // Derivative of state equation j[0][2] = cos(phi); @@ -87,7 +93,8 @@ public void processNoiseCovariance(double[][] cov) { cov[2][2] = 1e-4; // Allow change in velocity can - change by measurements cov[3][3] = 1e-9; // assume phi is not changing cov[4][4] = 1e-2; // Allow change in omega - cov[4][4] = 1e-1; // Allow change in bias + cov[5][5] = 1e-1; // Allow change in bias + cov[6][6] = 1e-6; // Allow change in bias } } \ No newline at end of file From 2ed89fbc9913be4cde0bbed51865f58ec5bd94f5 Mon Sep 17 00:00:00 2001 From: Paulo Khayat Date: Fri, 21 Feb 2020 19:46:58 +0200 Subject: [PATCH 02/22] Revert "Merge pull request #195 from Galaxia5987/revert-43-kalman" This reverts commit 20bc526235d333005d2ad3ac9d080dbb66fc1ac8. --- src/main/java/frc/robot/Constants.java | 14 +- .../KalmanRange/KalmanRangeEstimator.java | 66 +++ .../KalmanRange/Linear1dObservationModel.java | 46 ++ .../KalmanRange/Linear1dProcessModel.java | 55 +++ src/main/java/frc/robot/Robot.java | 44 +- src/main/java/frc/robot/RobotContainer.java | 23 +- .../subsystems/drivetrain/Drivetrain.java | 70 ++- .../drivetrain/EKF/KalmanFilter.java | 174 +++++++ .../subsystems/drivetrain/EKF/Matrix.java | 311 +++++++++++++ .../drivetrain/EKF/ObservationModel.java | 44 ++ .../drivetrain/EKF/ProcessModel.java | 68 +++ .../drivetrain/FullLocalization.java | 226 ++++++++++ .../OdometryInertialObservation.java | 91 ++++ .../OdometryInertialProcess.java | 93 ++++ .../drivetrain/auto/FollowPath.java | 8 +- .../turret/commands/PoseVisionTurret.java | 9 +- .../turret/commands/TurretSwitching.java | 4 +- .../subsystems/FullLocalizationTest.java | 423 ++++++++++++++++++ .../subsystems/KalmanRangeEstimatorTest.java | 117 +++++ 19 files changed, 1839 insertions(+), 47 deletions(-) create mode 100644 src/main/java/frc/robot/KalmanRange/KalmanRangeEstimator.java create mode 100644 src/main/java/frc/robot/KalmanRange/Linear1dObservationModel.java create mode 100644 src/main/java/frc/robot/KalmanRange/Linear1dProcessModel.java create mode 100644 src/main/java/frc/robot/subsystems/drivetrain/EKF/KalmanFilter.java create mode 100644 src/main/java/frc/robot/subsystems/drivetrain/EKF/Matrix.java create mode 100644 src/main/java/frc/robot/subsystems/drivetrain/EKF/ObservationModel.java create mode 100644 src/main/java/frc/robot/subsystems/drivetrain/EKF/ProcessModel.java create mode 100644 src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java create mode 100644 src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialObservation.java create mode 100644 src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java create mode 100644 src/test/java/frc/robot/subsystems/FullLocalizationTest.java create mode 100644 src/test/java/frc/robot/subsystems/KalmanRangeEstimatorTest.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 39b107a2..f938bf2e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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; /** @@ -14,6 +20,7 @@ */ public class Constants { public static final int TALON_TIMEOUT = 10; + public static final double EFFECTIVE_TURN_WIDTH = 0.73; // wheel center to wheel center [m] public static class Drivetrain { //Remember! High gear == High speed! @@ -35,6 +42,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.08; @@ -66,7 +79,6 @@ public static class FieldGeometry { public static final Pose2d RED_INNER_POWER_PORT_LOCATION = new Pose2d(15.98 + 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 PORT_HEIGHT = 2.4; } diff --git a/src/main/java/frc/robot/KalmanRange/KalmanRangeEstimator.java b/src/main/java/frc/robot/KalmanRange/KalmanRangeEstimator.java new file mode 100644 index 00000000..0f4a57d1 --- /dev/null +++ b/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]; + } +} + + diff --git a/src/main/java/frc/robot/KalmanRange/Linear1dObservationModel.java b/src/main/java/frc/robot/KalmanRange/Linear1dObservationModel.java new file mode 100644 index 00000000..6ca144a5 --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/frc/robot/KalmanRange/Linear1dProcessModel.java b/src/main/java/frc/robot/KalmanRange/Linear1dProcessModel.java new file mode 100644 index 00000000..e8fe146b --- /dev/null +++ b/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]; + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7f0f9c34..8caba15b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,6 +7,8 @@ package frc.robot; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import com.kauailabs.navx.frc.AHRS; import edu.wpi.cscore.UsbCamera; import edu.wpi.first.wpilibj.*; @@ -31,9 +33,12 @@ public class Robot extends TimedRobot { public static Compressor compressor = new Compressor(); public static PowerDistributionPanel pdp = new PowerDistributionPanel(); private Command m_autonomousCommand; + public static Timer robotTimer = new Timer(); + private RobotContainer m_robotContainer; + /** * @return Robot in debug mode */ @@ -86,8 +91,10 @@ public void robotPeriodic() { // 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(); + NetworkTableInstance.getDefault().flush(); } + /** * This function is called once each time the robot enters Disabled mode. */ @@ -105,30 +112,25 @@ public void disabledPeriodic() { @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } - } - - /** - * This function is called periodically during autonomous. - */ - @Override - public void autonomousPeriodic() { } - @Override - public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); } + } /** * This function is called periodically during operator control. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5ae6ad36..eb60cfae 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,9 +9,14 @@ 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.commandgroups.OuttakeBalls; import frc.robot.commandgroups.PickupBalls; import frc.robot.subsystems.climb.Climber; import frc.robot.subsystems.color_wheel.ColorWheel; @@ -21,10 +26,10 @@ 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.intake.Intake; -import frc.robot.commandgroups.OuttakeBalls; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.commands.ShootAtVelocity; import frc.robot.subsystems.shooter.commands.SpeedUp; @@ -33,8 +38,13 @@ 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; + /** * 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} @@ -42,19 +52,24 @@ * (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); private final VisionModule visionModule = new VisionModule(); private final CustomDashboard customDashboard = new CustomDashboard(); - private final Drivetrain drivetrain = new Drivetrain(); + public static final Drivetrain drivetrain = new Drivetrain(); private final ColorWheel colorWheel = new ColorWheel(); private final Shooter shooter = new Shooter(); 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. */ @@ -100,6 +115,7 @@ private void configureButtonBindings() { } } + /** * Initiates the value tuner. */ @@ -127,4 +143,5 @@ public Command getAutonomousCommand() { return null; } -} \ No newline at end of file +} + diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 0e3e468f..d3c2632f 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; +import frc.robot.UtilityFunctions; import frc.robot.subsystems.UnitModel; import frc.robot.utilities.CustomDashboard; import frc.robot.utilities.FalconConfiguration; @@ -31,6 +32,11 @@ import org.techfire225.webapp.FireLog; import static frc.robot.Constants.Drivetrain.*; +import static frc.robot.Constants.EFFECTIVE_TURN_WIDTH; +import static frc.robot.Ports.Drivetrain.LEFT_MASTER_INVERTED; +import static frc.robot.Ports.Drivetrain.LEFT_SLAVE_INVERTED; +import static frc.robot.Ports.Drivetrain.RIGHT_MASTER_INVERTED; +import static frc.robot.Ports.Drivetrain.RIGHT_SLAVE_INVERTED; import static frc.robot.Ports.Drivetrain.*; import static frc.robot.RobotContainer.navx; @@ -39,9 +45,11 @@ public class Drivetrain extends SubsystemBase { private final TalonFX leftSlave = new TalonFX(LEFT_SLAVE); private final TalonFX rightMaster = new TalonFX(RIGHT_MASTER); private final TalonFX rightSlave = new TalonFX(RIGHT_SLAVE); - private final DifferentialDriveOdometry odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading())); + private DifferentialDriveOdometry odometry; private UnitModel lowGearUnitModel = new UnitModel(LOW_TICKS_PER_METER); private UnitModel highGearUnitModel = new UnitModel(HIGH_TICKS_PER_METER); + public static final FullLocalization localization = new FullLocalization(new Rotation2d(0), EFFECTIVE_TURN_WIDTH); + /** * The gear shifter will be programmed according to the following terms * High gear - low torque High speed @@ -50,6 +58,7 @@ public class Drivetrain extends SubsystemBase { private DoubleSolenoid gearShifterA = null; private Solenoid gearShifterB = null; private Timer shiftCooldown = new Timer(); + private Timer localizationTimer = new Timer(); private boolean isShifting = false; @@ -82,11 +91,16 @@ public Drivetrain() { motorConfigurations.setEnableCurrentLimit(true); motorConfigurations.setEnableCurrentLimit(true); motorConfigurations.setSupplyCurrentLimit(40); - Utils.configAllFalcons(motorConfigurations, rightMaster, rightSlave, leftMaster, leftSlave); + UtilityFunctions.configAllFalcons(motorConfigurations, rightMaster, rightSlave, leftMaster, leftSlave); if (Robot.isRobotA) gearShifterA = new DoubleSolenoid(SHIFTER_FORWARD_PORT, SHIFTER_REVERSE_PORT); else gearShifterB = new Solenoid(SHIFTER_PORT); + + navx.reset(); + Pose2d INITIAL_POSE = new Pose2d(UtilityFunctions.getAlliancePort(false).getTranslation().getX() - 10, UtilityFunctions.getAlliancePort(false).getTranslation().getY(), new Rotation2d()); + localization.resetPosition(INITIAL_POSE, new Rotation2d(Math.toRadians(navx.getAngle())), Robot.robotTimer.get()); + odometry.resetPosition(INITIAL_POSE, Rotation2d.fromDegrees(getHeading())); } public void shiftGear(shiftModes mode) { @@ -128,6 +142,7 @@ public double getCooldown() { return shiftCooldown.get(); } + private void shiftHigh() { startCooldown(); if (Robot.isRobotA) @@ -185,6 +200,14 @@ public double getLeftVelocity() { return getCurrentUnitModel().toVelocity(leftMaster.getSelectedSensorVelocity()); } + public double getLeftPosition() { + return getCurrentUnitModel().toUnits(leftMaster.getSelectedSensorPosition()); + } + + public double getRightPosition() { + return getCurrentUnitModel().toUnits(rightMaster.getSelectedSensorPosition()); + } + /** * Indicates whether the shifter is on a high gear * @@ -192,7 +215,7 @@ public double getLeftVelocity() { */ public boolean isShiftedHigh() { if (Robot.isRobotA && gearShifterA != null) - return gearShifterA.get() == DoubleSolenoid.Value.kForward; + return gearShifterA.get() == DoubleSolenoid.Value.kForward || gearShifterA.get() == DoubleSolenoid.Value.kOff; else if (gearShifterB != null) return !gearShifterB.get(); else @@ -222,7 +245,7 @@ public double getCCWHeading() { } public Pose2d getPose() { - return odometry.getPoseMeters(); + return localization.getPoseMeters(); } public void setPose(Pose2d pose, Rotation2d rotation) { @@ -230,6 +253,7 @@ public void setPose(Pose2d pose, Rotation2d rotation) { rightMaster.setSelectedSensorPosition(0); navx.reset(); odometry.resetPosition(pose, rotation); + localization.resetPosition(pose, rotation, Robot.robotTimer.get()); } public void setVelocityAndFeedForward(double leftVelocity, double rightVelocity, double leftFF, double rightFF) { @@ -243,27 +267,39 @@ public void setPower(double leftPower, double rightPower) { rightMaster.set(ControlMode.PercentOutput, rightPower); } + public void resetEncoders() { + leftMaster.setSelectedSensorPosition(0); + rightMaster.setSelectedSensorPosition(0); + } + @Override public void periodic() { // This method will be called once per scheduler run - UnitModel unitModel = getCurrentUnitModel(); - Pose2d current = odometry.update( - Rotation2d.fromDegrees(getCCWHeading()), - unitModel.toUnits(leftMaster.getSelectedSensorPosition()), - unitModel.toUnits(rightMaster.getSelectedSensorPosition()) - ); if (getCooldown() > SHIFTER_COOLDOWN) resetCooldown(); - FalconDashboard.INSTANCE.setRobotX(Units.metersToFeet(current.getTranslation().getX())); - FalconDashboard.INSTANCE.setRobotY(Units.metersToFeet(current.getTranslation().getY())); - FalconDashboard.INSTANCE.setRobotHeading(Math.toRadians(-navx.getAngle())); - SmartDashboard.putBoolean("shiftedHigh", isShiftedHigh()); + SmartDashboard.putNumber("leftPosition", getLeftPosition()); + SmartDashboard.putNumber("rightPosition", getRightPosition()); + + Pose2d current = localization.update( new Rotation2d( Math.toRadians(navx.getAngle())), + getLeftPosition(), + getRightPosition(), + navx.getWorldLinearAccelY()*GRAVITY_ACCELERATION, + Robot.robotTimer.get() + ); + + odometry.update(new Rotation2d( Math.toRadians(navx.getAngle())), + getLeftPosition(), + getRightPosition()); - CustomDashboard.setShift(isShiftedHigh()); + SmartDashboard.putNumber(" simple x", odometry.getPoseMeters().getTranslation().getX()); + SmartDashboard.putNumber(" simple y", odometry.getPoseMeters().getTranslation().getY()); + SmartDashboard.putNumber(" simple angle", odometry.getPoseMeters().getRotation().getRadians()); + SmartDashboard.putNumber("navx accel", navx.getWorldLinearAccelY() * GRAVITY_ACCELERATION); - FireLog.log("driveRightVelocity", Math.abs(getRightVelocity())); - FireLog.log("driveLeftVelocity", Math.abs(getLeftVelocity())); + FalconDashboard.INSTANCE.setRobotX(current.getTranslation().getX()); + FalconDashboard.INSTANCE.setRobotY(current.getTranslation().getY()); + FalconDashboard.INSTANCE.setRobotHeading(Math.toRadians(navx.getAngle() * (GYRO_INVERTED ? -1 : 1))); } /** diff --git a/src/main/java/frc/robot/subsystems/drivetrain/EKF/KalmanFilter.java b/src/main/java/frc/robot/subsystems/drivetrain/EKF/KalmanFilter.java new file mode 100644 index 00000000..2e62f072 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/EKF/KalmanFilter.java @@ -0,0 +1,174 @@ +package frc.robot.subsystems.drivetrain.EKF; + +/* Refer to http://en.wikipedia.org/wiki/Kalman_filter for + mathematical details. The naming scheme is that variables get names + that make sense, and are commented with their analog in + the Wikipedia mathematical notation. + This Kalman filter implementation does not support controlled + input. + (Like knowing which way the steering wheel in a car is turned and + using that to inform the model.) + Vectors are handled as n-by-1 matrices. + TODO: comment on the dimension of the matrices */ + +public class KalmanFilter { + + public double time = 0; + public double maximalTimeStep = Double.MAX_VALUE; + + public ProcessModel model; + + public KalmanFilter(ProcessModel model) { + this.model = model; + + model.initialState(model.state_estimate.data); + model.initialStateCovariance(model.estimate_covariance.data); + } + + public void setMaximalTimeStep(double maximalTimeStep) { + this.maximalTimeStep = maximalTimeStep; + } + + /* + * Runs one timestep of prediction + estimation. + * + * Before each time step of running this, set f.observation to be the next + * time step's observation. + * + * Before the first step, define the model by setting: f.state_transition + * f.observation_model f.process_noise_covariance + * f.observation_noise_covariance + * + * It is also advisable to initialize with reasonable guesses for + * f.state_estimate f.estimate_covariance + */ + public void update(double t, ObservationModel obs) { + do { + double dt = t - time; + if (dt > maximalTimeStep) { + dt = maximalTimeStep; + } + predict(dt); + time += dt; + } while (time < t); + estimate(obs); + } + + /* Just the prediction phase of update. */ + public void predict(double dt) { + /* Predict the state estimate covariance */ + model.stateFunctionJacobian(model.state_estimate.data, model.state_jacobian.data); + Matrix.add_scaled_matrix(model.identity_scratch, dt, model.state_jacobian, model.state_transition); + Matrix.multiply_matrix(model.state_transition, model.estimate_covariance, model.big_square_scratch); + Matrix.multiply_by_transpose_matrix(model.big_square_scratch, model.state_transition, + model.estimate_covariance); + model.processNoiseCovariance(model.process_noise_covariance.data); + Matrix.add_scaled_matrix(model.estimate_covariance, dt, model.process_noise_covariance, + model.estimate_covariance); + + /* Predict the state */ + model.stateFunction(model.state_estimate.data, model.state_function.data); + Matrix.add_scaled_matrix(model.state_estimate, dt, model.state_function, model.state_estimate); + + model.normalizeState(model.state_estimate.data); + } + + // unfortunately there is no observable improvement + public void predict_rk2(double dt) { + // runge-kutta 2 (explicit midpoint method) + model.stateFunction(model.state_estimate.data, model.state_function.data); + Matrix.add_scaled_matrix(model.state_estimate, dt / 2, model.state_function, model.predicted_state_midpoint); + + /* Predict the state estimate covariance */ + model.stateFunctionJacobian(model.predicted_state_midpoint.data, model.state_jacobian.data); + Matrix.add_scaled_matrix(model.identity_scratch, dt, model.state_jacobian, model.state_transition); + Matrix.multiply_matrix(model.state_transition, model.estimate_covariance, model.big_square_scratch); + Matrix.multiply_by_transpose_matrix(model.big_square_scratch, model.state_transition, + model.estimate_covariance); + model.processNoiseCovariance(model.process_noise_covariance.data); + Matrix.add_scaled_matrix(model.estimate_covariance, dt, model.process_noise_covariance, + model.estimate_covariance); + + /* Predict the state */ + model.stateFunction(model.predicted_state_midpoint.data, model.state_function.data); + Matrix.add_scaled_matrix(model.state_estimate, dt, model.state_function, model.state_estimate); + } + + // this requires a lot of iterations (step size 0.001) + public void predict_continuous(double dt) { + /* Predict the state estimate covariance */ + model.stateFunctionJacobian(model.state_estimate.data, model.state_jacobian.data); + Matrix.multiply_matrix(model.state_jacobian, model.estimate_covariance, model.big_square_scratch); + Matrix.multiply_by_transpose_matrix(model.estimate_covariance, model.state_jacobian, model.big_square_scratch2); + Matrix.add_matrix(model.big_square_scratch, model.big_square_scratch2, model.big_square_scratch); + + model.processNoiseCovariance(model.process_noise_covariance.data); + Matrix.add_matrix(model.big_square_scratch, model.process_noise_covariance, model.big_square_scratch); + + Matrix.add_scaled_matrix(model.estimate_covariance, dt, model.big_square_scratch, model.estimate_covariance); + + /* Predict the state */ + model.stateFunction(model.state_estimate.data, model.state_function.data); + Matrix.add_scaled_matrix(model.state_estimate, dt, model.state_function, model.state_estimate); + } + + // this requires a lot of iterations (step size 0.001) + public void predict_continuous_rk2(double dt) { + // runge-kutta 2 (explicit midpoint method) + model.stateFunction(model.state_estimate.data, model.state_function.data); + Matrix.add_scaled_matrix(model.state_estimate, dt / 2, model.state_function, model.predicted_state_midpoint); + + /* Predict the state estimate covariance */ + model.stateFunctionJacobian(model.predicted_state_midpoint.data, model.state_jacobian.data); + Matrix.multiply_matrix(model.state_jacobian, model.estimate_covariance, model.big_square_scratch); + Matrix.multiply_by_transpose_matrix(model.estimate_covariance, model.state_jacobian, model.big_square_scratch2); + Matrix.add_matrix(model.big_square_scratch, model.big_square_scratch2, model.big_square_scratch); + + model.processNoiseCovariance(model.process_noise_covariance.data); + Matrix.add_matrix(model.big_square_scratch, model.process_noise_covariance, model.big_square_scratch); + + Matrix.add_scaled_matrix(model.estimate_covariance, dt, model.big_square_scratch, model.estimate_covariance); + + /* Predict the state */ + model.stateFunction(model.predicted_state_midpoint.data, model.state_function.data); + Matrix.add_scaled_matrix(model.state_estimate, dt, model.state_function, model.state_estimate); + } + + /* Just the estimation phase of update. */ + void estimate(ObservationModel obs) { + /* Calculate innovation */ + obs.observationMeasurement(obs.observation.data); + obs.observationModel(model.state_estimate.data, obs.innovation.data); + Matrix.subtract_matrix(obs.observation, obs.innovation, obs.innovation); + obs.normalizeInnovation(obs.innovation.data); + + /* Calculate innovation covariance */ + obs.observationModelJacobian(model.state_estimate.data, obs.observation_model.data); + obs.observationNoiseCovariance(obs.observation_noise_covariance.data); + Matrix.multiply_by_transpose_matrix(model.estimate_covariance, obs.observation_model, obs.vertical_scratch); + Matrix.multiply_matrix(obs.observation_model, obs.vertical_scratch, obs.innovation_covariance); + Matrix.add_matrix(obs.innovation_covariance, obs.observation_noise_covariance, obs.innovation_covariance); + + /* + * Invert the innovation covariance. Note: this destroys the innovation + * covariance. TODO: handle inversion failure intelligently. + */ + Matrix.destructive_invert_matrix(obs.innovation_covariance, obs.inverse_innovation_covariance); + + /* + * Calculate the optimal Kalman gain. Note we still have a useful + * partial product in vertical scratch from the innovation covariance. + */ + Matrix.multiply_matrix(obs.vertical_scratch, obs.inverse_innovation_covariance, obs.optimal_gain); + + /* Estimate the state */ + Matrix.multiply_matrix(obs.optimal_gain, obs.innovation, model.state_delta_scratch); + Matrix.add_matrix(model.state_estimate, model.state_delta_scratch, model.state_estimate); + + /* Estimate the state covariance */ + Matrix.multiply_matrix(obs.optimal_gain, obs.observation_model, model.big_square_scratch); + Matrix.subtract_from_identity_matrix(model.big_square_scratch); + Matrix.multiply_matrix(model.big_square_scratch, model.estimate_covariance, model.big_square_scratch2); + Matrix.copy_matrix(model.big_square_scratch2, model.estimate_covariance); + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/EKF/Matrix.java b/src/main/java/frc/robot/subsystems/drivetrain/EKF/Matrix.java new file mode 100644 index 00000000..cf48e533 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/EKF/Matrix.java @@ -0,0 +1,311 @@ +package frc.robot.subsystems.drivetrain.EKF; + +public class Matrix { + /* Dimensions */ + public int rows; + public int cols; + + /* Contents of the matrix */ + public double[][] data; + + /* + * Allocate memory for a new matrix. Zeros out the matrix. Assert-fails if + * we are out of memory. + */ + public Matrix(int rows, int cols) { + this.rows = rows; + this.cols = cols; + this.data = new double[rows][cols]; + } + + /* Set values of a matrix, row by row. */ + public void set_matrix(double... arg) { + assert (arg.length == rows * cols); + for (int i = 0; i < rows; i++) { + for (int j = 0; j < cols; j++) { + data[i][j] = arg[i * cols + j]; + } + } + } + + /* Turn m into an identity matrix. */ + public void set_identity_matrix() { + assert (rows == cols); + for (int i = 0; i < rows; ++i) { + for (int j = 0; j < cols; ++j) { + if (i == j) { + data[i][j] = 1.0; + } else { + data[i][j] = 0.0; + } + } + } + } + + /* Copy a matrix. */ + public static void copy_matrix(Matrix source, Matrix destination) { + assert (source.rows == destination.rows); + assert (source.cols == destination.cols); + for (int i = 0; i < source.rows; ++i) { + for (int j = 0; j < source.cols; ++j) { + destination.data[i][j] = source.data[i][j]; + } + } + } + + /* Pretty-print a matrix. */ + public void print_matrix() { + for (int i = 0; i < rows; ++i) { + for (int j = 0; j < cols; ++j) { + if (j > 0) { + System.out.print(" "); + } + System.out.format("%6.2f", data[i][j]); + } + System.out.print("\n"); + } + } + + /* Add matrices a and b and put the result in c. */ + public static void add_matrix(Matrix a, Matrix b, Matrix c) { + assert (a.rows == b.rows); + assert (a.rows == c.rows); + assert (a.cols == b.cols); + assert (a.cols == c.cols); + for (int i = 0; i < a.rows; ++i) { + for (int j = 0; j < a.cols; ++j) { + c.data[i][j] = a.data[i][j] + b.data[i][j]; + } + } + } + + /* Subtract matrices a and b and put the result in c. */ + public static void subtract_matrix(Matrix a, Matrix b, Matrix c) { + assert (a.rows == b.rows); + assert (a.rows == c.rows); + assert (a.cols == b.cols); + assert (a.cols == c.cols); + for (int i = 0; i < a.rows; ++i) { + for (int j = 0; j < a.cols; ++j) { + c.data[i][j] = a.data[i][j] - b.data[i][j]; + } + } + } + + /* Subtract from the identity matrix in place. */ + public static void subtract_from_identity_matrix(Matrix a) { + assert (a.rows == a.cols); + for (int i = 0; i < a.rows; ++i) { + for (int j = 0; j < a.cols; ++j) { + if (i == j) { + a.data[i][j] = 1.0 - a.data[i][j]; + } else { + a.data[i][j] = 0.0 - a.data[i][j]; + } + } + } + } + + /* Multiply matrices a and b and put the result in c. */ + public static void multiply_matrix(Matrix a, Matrix b, Matrix c) { + assert (a.cols == b.rows); + assert (a.rows == c.rows); + assert (b.cols == c.cols); + for (int i = 0; i < c.rows; ++i) { + for (int j = 0; j < c.cols; ++j) { + /* + * Calculate element c.data[i][j] via a dot product of one row + * of a with one column of b + */ + c.data[i][j] = 0.0; + for (int k = 0; k < a.cols; ++k) { + c.data[i][j] += a.data[i][k] * b.data[k][j]; + } + } + } + } + + /* Multiply matrix a by b-transpose and put the result in c. */ + /* + * This is multiplying a by b-tranpose so it is like multiply_matrix but + * references to b reverse rows and cols. + */ + public static void multiply_by_transpose_matrix(Matrix a, Matrix b, Matrix c) { + assert (a.cols == b.cols); + assert (a.rows == c.rows); + assert (b.rows == c.cols); + for (int i = 0; i < c.rows; ++i) { + for (int j = 0; j < c.cols; ++j) { + /* + * Calculate element c.data[i][j] via a dot product of one row + * of a with one row of b + */ + c.data[i][j] = 0.0; + for (int k = 0; k < a.cols; ++k) { + c.data[i][j] += a.data[i][k] * b.data[j][k]; + } + } + } + } + + /* Transpose input and put the result in output. */ + public static void transpose_matrix(Matrix input, Matrix output) { + assert (input.rows == output.cols); + assert (input.cols == output.rows); + for (int i = 0; i < input.rows; ++i) { + for (int j = 0; j < input.cols; ++j) { + output.data[j][i] = input.data[i][j]; + } + } + } + + /* Whether two matrices are approximately equal. */ + public static boolean equal_matrix(Matrix a, Matrix b, double tolerance) { + assert (a.rows == b.rows); + assert (a.cols == b.cols); + for (int i = 0; i < a.rows; ++i) { + for (int j = 0; j < a.cols; ++j) { + if (Math.abs(a.data[i][j] - b.data[i][j]) > tolerance) { + return false; + } + } + } + return true; + } + + /* Multiply a matrix by a scalar. */ + public void scale_matrix(double scalar) { + assert (scalar != 0.0); + for (int i = 0; i < rows; ++i) { + for (int j = 0; j < cols; ++j) { + data[i][j] *= scalar; + } + } + } + + /* + * Swap rows r1 and r2 of a matrix. This is one of the three + * "elementary row operations". + */ + public void swap_rows(int r1, int r2) { + assert (r1 != r2); + double[] tmp = data[r1]; + data[r1] = data[r2]; + data[r2] = tmp; + } + + /* + * Multiply row r of a matrix by a scalar. This is one of the three + * "elementary row operations". + */ + public void scale_row(int r, double scalar) { + assert (scalar != 0.0); + for (int i = 0; i < cols; ++i) { + data[r][i] *= scalar; + } + } + + /* + * Add a multiple of row r2 to row r1. Also known as a "shear" operation. + * This is one of the three "elementary row operations". + */ + /* Add scalar * row r2 to row r1. */ + public void shear_row(int r1, int r2, double scalar) { + assert (r1 != r2); + for (int i = 0; i < cols; ++i) { + data[r1][i] += scalar * data[r2][i]; + } + } + + /* + * Invert a square matrix. Returns whether the matrix is invertible. input + * is mutated as well by this routine. + */ + + /* + * Uses Gauss-Jordan elimination. + * + * The elimination procedure works by applying elementary row operations to + * our input matrix until the input matrix is reduced to the identity + * matrix. Simultaneously, we apply the same elementary row operations to a + * separate identity matrix to produce the inverse matrix. If this makes no + * sense, read wikipedia on Gauss-Jordan elimination. + * + * This is not the fastest way to invert matrices, so this is quite possibly + * the bottleneck. + */ + public static boolean destructive_invert_matrix(Matrix input, Matrix output) { + assert (input.rows == input.cols); + assert (input.rows == output.rows); + assert (input.rows == output.cols); + + output.set_identity_matrix(); + + /* + * Convert input to the identity matrix via elementary row operations. + * The ith pass through this loop turns the element at i,i to a 1 and + * turns all other elements in column i to a 0. + */ + for (int i = 0; i < input.rows; ++i) { + if (input.data[i][i] == 0.0) { + /* We must swap rows to get a nonzero diagonal element. */ + int r; + for (r = i + 1; r < input.rows; ++r) { + if (input.data[r][i] != 0.0) { + break; + } + } + if (r == input.rows) { + /* + * Every remaining element in this column is zero, so this + * matrix cannot be inverted. + */ + return false; + } + input.swap_rows(i, r); + output.swap_rows(i, r); + } + + /* + * Scale this row to ensure a 1 along the diagonal. We might need to + * worry about overflow from a huge scalar here. + */ + double scalar = 1.0 / input.data[i][i]; + input.scale_row(i, scalar); + output.scale_row(i, scalar); + + /* Zero out the other elements in this column. */ + for (int j = 0; j < input.rows; ++j) { + if (i == j) { + continue; + } + double shear_needed = -input.data[j][i]; + input.shear_row(j, i, shear_needed); + output.shear_row(j, i, shear_needed); + } + } + + return true; + } + + public void zero_matrix() { + for (int i = 0; i < rows; ++i) { + for (int j = 0; j < cols; ++j) { + data[i][j] = 0.0; + } + } + } + + public static void add_scaled_matrix(Matrix a, double s, Matrix b, Matrix c) { + assert (a.rows == b.rows); + assert (a.rows == c.rows); + assert (a.cols == b.cols); + assert (a.cols == c.cols); + for (int i = 0; i < a.rows; ++i) { + for (int j = 0; j < a.cols; ++j) { + c.data[i][j] = a.data[i][j] + s * b.data[i][j]; + } + } + } + +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/EKF/ObservationModel.java b/src/main/java/frc/robot/subsystems/drivetrain/EKF/ObservationModel.java new file mode 100644 index 00000000..dcf32eec --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/EKF/ObservationModel.java @@ -0,0 +1,44 @@ +package frc.robot.subsystems.drivetrain.EKF; + +public abstract class ObservationModel { + + /* This group of matrices must be specified by the user. */ + /* H_k */ + public Matrix observation_model = new Matrix(observationDimension(), stateDimension()); + /* R_k */ + public Matrix observation_noise_covariance = new Matrix(observationDimension(), observationDimension()); + + /* The observation is modified by the user every time step. */ + /* z_k */ + public Matrix observation = new Matrix(observationDimension(), 1); + + /* This group of matrices are updated every time step by the filter. */ + /* y-tilde_k */ + public Matrix innovation = new Matrix(observationDimension(), 1); + /* S_k */ + public Matrix innovation_covariance = new Matrix(observationDimension(), observationDimension()); + /* S_k^-1 */ + public Matrix inverse_innovation_covariance = new Matrix(observationDimension(), observationDimension()); + /* K_k */ + public Matrix optimal_gain = new Matrix(stateDimension(), observationDimension()); + + /* This group is used for meaningless intermediate calculations */ + public Matrix vertical_scratch = new Matrix(stateDimension(), observationDimension()); + + public abstract int observationDimension(); + + public abstract int stateDimension(); + + public abstract void observationMeasurement(double[][] y); + + public abstract void observationModel(double[][] x, double[][] h); + + public abstract void observationModelJacobian(double[][] x, double[][] j); + + public abstract void observationNoiseCovariance(double[][] cov); + + public void normalizeInnovation(double[][] i) { + // TODO Auto-generated method stub + + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/EKF/ProcessModel.java b/src/main/java/frc/robot/subsystems/drivetrain/EKF/ProcessModel.java new file mode 100644 index 00000000..135bebd9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/EKF/ProcessModel.java @@ -0,0 +1,68 @@ +package frc.robot.subsystems.drivetrain.EKF; + +public abstract class ProcessModel { + + /* This group of matrices must be specified by the user. */ + public Matrix state_function = new Matrix(stateDimension(), 1); + public Matrix state_jacobian = new Matrix(stateDimension(), stateDimension()); + public Matrix process_noise_covariance = new Matrix(stateDimension(), stateDimension()); + + /* This group of matrices are updated every time step by the filter. */ + public Matrix state_estimate = new Matrix(stateDimension(), 1); + public Matrix estimate_covariance = new Matrix(stateDimension(), stateDimension()); + + /* This group is used for meaningless intermediate calculations */ + public Matrix big_square_scratch = new Matrix(stateDimension(), stateDimension()); + public Matrix big_square_scratch2 = new Matrix(stateDimension(), stateDimension()); + public Matrix identity_scratch = new Matrix(stateDimension(), stateDimension()); + public Matrix state_transition = new Matrix(stateDimension(), stateDimension()); + public Matrix predicted_state_midpoint = new Matrix(stateDimension(), 1); + public Matrix state_delta_scratch = new Matrix(stateDimension(), 1); + + public ProcessModel() { + identity_scratch.set_identity_matrix(); + } + + public double[][] getState() { + return state_estimate.data; + } + + public double getState(int state) { + return state_estimate.data[state][0]; + } + + public void setState(int state, double v) { + state_estimate.data[state][0] = v; + } + + public double[][] getCovariance() { + return estimate_covariance.data; + } + + public double getCovariance(int state) { + return estimate_covariance.data[state][0]; + } + + public void setCovarianceClearRowCol(int state, double v) { + for (int i = 0; i < estimate_covariance.data.length; i++) { + estimate_covariance.data[state][i] = 0; + estimate_covariance.data[i][state] = 0; + } + estimate_covariance.data[state][state] = v; + } + + public abstract int stateDimension(); + + public abstract void initialState(double[][] x); + + public abstract void initialStateCovariance(double[][] cov); + + public abstract void stateFunction(double[][] x, double[][] f); + + public abstract void stateFunctionJacobian(double[][] x, double[][] j); + + public abstract void processNoiseCovariance(double[][] cov); + + public void normalizeState(double[][] x) { + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java b/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java new file mode 100644 index 00000000..46e13a6f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java @@ -0,0 +1,226 @@ +package frc.robot.subsystems.drivetrain; + + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Robot; +import frc.robot.subsystems.drivetrain.EKF.KalmanFilter; +import frc.robot.subsystems.drivetrain.KalmanLocalization.OdometryInertialObservation; +import frc.robot.subsystems.drivetrain.KalmanLocalization.OdometryInertialProcess; +import org.ghrobotics.lib.debug.FalconDashboard; + +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +import static frc.robot.RobotContainer.drivetrain; +import static frc.robot.RobotContainer.navx; +import static java.lang.Math.abs; +import static java.lang.Math.min; + +/** + * Class for localization using differntial drive odometry and inertial sensors. Odometry allows you to track the + * robot's position on the field over the course of a match using readings from + * 2 encoders, a gyroscope and an accelerometer. + * + *

Teams can use odometry during the autonomous period for complex tasks like + * path following. Furthermore, odometry can be used for latency compensation + * when using computer-vision systems. + * + *

It is important that you reset your encoders to zero before using this class. + * Any subsequent pose resets also require the encoders to be reset to zero. + */ +public class FullLocalization { + + private Pose2d m_poseInitialMeters; + public KalmanFilter filter; + private OdometryInertialProcess process; + private OdometryInertialObservation observation; + double m_width; // Robot width - distance between wheel centers + final double MAX_ANGLE_DELTA_ENCODER_GYRO = 0.05; // allowed deviation in one cycle of between gyro and encoders. A larger value means wheel is slipping + + + private Pose2d m_poseMeters; + private Rotation2d m_gyroOffset; + private Rotation2d m_previousAngle; + + private double m_prevLeftDistance; + private double m_prevRightDistance; + private double m_prev_time; + + private NetworkTable localizationTable = NetworkTableInstance.getDefault().getTable("localization"); + private NetworkTableEntry x = localizationTable.getEntry("x"); + private NetworkTableEntry y = localizationTable.getEntry("y"); + private NetworkTableEntry velocity = localizationTable.getEntry("velocity"); + private NetworkTableEntry theta = localizationTable.getEntry("theta"); + private NetworkTableEntry angularVelocity = localizationTable.getEntry("angular-velocity"); + private NetworkTableEntry accelerationBias = localizationTable.getEntry("acceleration-bias"); + private NetworkTableEntry encoderLeft = localizationTable.getEntry("left-encoder"); + private NetworkTableEntry encoderRight = localizationTable.getEntry("right-encoder"); + + /** + * Constructs a DifferentialDriveOdometry object. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param initialPoseMeters The starting position of the robot on the field. + */ + public FullLocalization(Rotation2d gyroAngle, + Pose2d initialPoseMeters, double widthMeters) { + m_poseMeters = initialPoseMeters; + m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); + m_previousAngle = gyroAngle; + + observation = new OdometryInertialObservation(widthMeters / 2, widthMeters / 2); + process = new OdometryInertialProcess(); + filter = new KalmanFilter(process); + m_prev_time = 0; + m_width = widthMeters; + + process.setState(0, m_poseMeters.getTranslation().getX()); + process.setState(1, m_poseMeters.getTranslation().getY()); + process.setState(2, 0); + process.setState(3, m_poseMeters.getRotation().getRadians()); + process.setState(4, 0); + + } + + /** + * Constructs a DifferentialDriveOdometry object with the default pose at the origin. + * + * @param gyroAngle The angle reported by the gyroscope. + */ + public FullLocalization(Rotation2d gyroAngle, double widthMeters) { + this(gyroAngle, new Pose2d(), widthMeters); + } + + /** + * Resets the robot's position on the field. + * + *

You NEED to reset your encoders (to zero) when calling this method. + * + *

The gyroscope angle does not need to be reset here on the user's robot code. + * The library automatically takes care of offsetting the gyro angle. + * + * @param poseMeters The position on the field that your robot is at. + * @param gyroAngle The angle reported by the gyroscope. + */ + public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle, double time) { + m_poseMeters = poseMeters; + m_previousAngle = gyroAngle; + m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); + + m_prevLeftDistance = 0.0; + m_prevRightDistance = 0.0; + + m_prev_time = time; + + process.setState(0, poseMeters.getTranslation().getX()); + process.setState(1, poseMeters.getTranslation().getY()); + process.setState(2, 0); + process.setState(3, poseMeters.getRotation().getRadians()); + process.setState(4, 0); + + } + + /** + * Returns the position of the robot on the field. + * + * @return The pose of the robot (x and y are in meters). + */ + public Pose2d getPoseMeters() { + return m_poseMeters; + } + + + /** + * Updates the robot position on the field using distance measurements from encoders and IMU. This + * method is more numerically accurate than using velocities to integrate the pose and + * is also advantageous for teams that are using lower CPR encoders. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param leftDistanceMeters The distance traveled by the left encoder. + * @param rightDistanceMeters The distance traveled by the right encoder. + * @param acc The acceleration along X axis of the robot [m/s^2] + * @param time The current time [s] + * @return The new pose of the robot. + */ + public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters, + double rightDistanceMeters, double acc, double time) { + double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance; + double deltaRightDistance = rightDistanceMeters - m_prevRightDistance; + + m_prevLeftDistance = leftDistanceMeters; + m_prevRightDistance = rightDistanceMeters; + + var angle = new Rotation2d( gyroAngle.getRadians() + m_gyroOffset.getRadians()); + + + double dt = Math.max(0.02,time - m_prev_time); + // Observation object holds the new measurements + observation.SetMeasurement(angle.getRadians(), deltaLeftDistance, deltaRightDistance, dt); + // Acceleration enters the process and not the observation + process.setAcc(0); + + // Check if encoders are valid or slipping: + observation.setEncoderValid(EncoderValid(gyroAngle, deltaLeftDistance, deltaRightDistance)); + + m_previousAngle = gyroAngle; + + + // The main estimate step: + filter.update(time, observation); + + // Pack the results in Pose2d class + Rotation2d phi = new Rotation2d(filter.model.state_estimate.data[3][0]); + m_poseMeters = new Pose2d(filter.model.state_estimate.data[0][0], + filter.model.state_estimate.data[1][0], + phi); + + + x.setDouble(filter.model.state_estimate.data[0][0]); + y.setDouble(filter.model.state_estimate.data[1][0]); + velocity.setDouble(filter.model.state_estimate.data[2][0]); + theta.setDouble(filter.model.state_estimate.data[3][0]); + angularVelocity.setDouble(filter.model.state_estimate.data[4][0]); + accelerationBias.setDouble(filter.model.state_estimate.data[5][0]); + + m_prev_time = time; + return m_poseMeters; + + + } + + public Pose2d getPose() { + return getPoseMeters(); + } + + public void setPose(Pose2d pose, Rotation2d rotation) { + drivetrain.resetEncoders(); + navx.reset(); + resetPosition(pose, rotation, Robot.robotTimer.get()); + } + + + // Detect encoder slipping condition by comparing gyro and encoder readings. + public boolean EncoderValid(Rotation2d gyroAngle, double deltaLeftDistance, + double deltaRightDistance) { + double delta_angle = gyroAngle.getRadians() - m_previousAngle.getRadians(); + SmartDashboard.putNumber("previous angle", m_previousAngle.getRadians()); + SmartDashboard.putNumber("delta angle: " , delta_angle ); + double delta_angle_from_encoder = (deltaLeftDistance - deltaRightDistance) / m_width; + SmartDashboard.putNumber("delta angle from encoder: " , delta_angle_from_encoder); + if (abs(delta_angle - delta_angle_from_encoder) > MAX_ANGLE_DELTA_ENCODER_GYRO) { + return false; + } else { + return true; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialObservation.java b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialObservation.java new file mode 100644 index 00000000..6a206f92 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialObservation.java @@ -0,0 +1,91 @@ +package frc.robot.subsystems.drivetrain.KalmanLocalization; + +import frc.robot.subsystems.drivetrain.EKF.ObservationModel; + +// Class that implements the observation model for localization Kalman filter +public class OdometryInertialObservation extends ObservationModel { + + // Measurements: + private double m_yaw; + private double m_LeftTrackSpeed; + private double m_RightTrackSpeed; + + private double m_Rr; // distance from robot IMU to right wheel + private double m_Rl; // distacne from robot IMU to left wheel + + private boolean m_encoderValid = true; // Encoder valid flag + + public void setEncoderValid(boolean valid){ + m_encoderValid = valid; + } + + // Constructor must receive the geometry of the robot + public OdometryInertialObservation(double Rr, double Rl) + { + m_Rl = Rl; + m_Rr = Rr; + } + + public void SetMeasurement(double yaw, double LeftTrack, double RightTrack, double dt){ + m_yaw = yaw; + m_LeftTrackSpeed = LeftTrack/dt; + m_RightTrackSpeed = RightTrack/dt; + } + + + @Override + public int observationDimension() { + return 3; + } + + @Override + public int stateDimension() { + return 6; + } + + @Override + public void observationMeasurement(double[][] y) { + y[0][0] = m_yaw; + y[1][0] = m_LeftTrackSpeed; + y[2][0] = m_RightTrackSpeed; + } + + @Override + public void observationModel(double[][] x, double[][] h) { + double X = x[0][0]; + double Y = x[1][0]; + double v = x[2][0]; + double phi = x[3][0]; + double omega = x[4][0]; + + h[0][0] = phi; // First measurement is the gyro angle + h[1][0] = v + m_Rl*omega; // Second measurement is the left encoder + h[2][0] = v - m_Rr*omega; // Third measurement is the right encoder + } + + @Override + public void observationModelJacobian(double[][] x, double[][] j) { + j[0][3] = 1; + j[1][2] = 1; + j[2][2] = 1; + j[1][4] = m_Rl; + j[2][4] = -m_Rr; // TODO: check if the direction of encoders is correct in Kalman tuning + + } + + @Override + public void observationNoiseCovariance(double[][] cov) { + cov[0][0] = 1e-8; // sigma = 1e-4 rad + + if (m_encoderValid) { + cov[1][1] = 1e-6; // sigma = 1e-3 m/s + cov[2][2] = 1e-6; // sigma = 1e-3 m/s + } else{ // Throw away encoders if it slips + cov[1][1] = 1e6; // garbage measurement + cov[2][2] = 1e6; // garbage measurement + System.out.println("encoders invalid"); + } + + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java new file mode 100644 index 00000000..e4455320 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java @@ -0,0 +1,93 @@ +package frc.robot.subsystems.drivetrain.KalmanLocalization; +import frc.robot.subsystems.drivetrain.EKF.ProcessModel; + +import static java.lang.Math.cos; +import static java.lang.Math.sin; + +// Class that describes the 2D dynamics of robot. Has 5 states as following +// State [x y v phi omega acc_bias] +// [ 0 1 2 3 4 5] +// x [m] , y [m], v [m/s], phi [rad], omega [rad/s] acc_bias[m/s^2] +public class OdometryInertialProcess extends ProcessModel { + + private double m_acc = 0; + + public void setAcc(double acc) + { + m_acc = acc; + } + + @Override + public int stateDimension() { + return 6; + } + + @Override + public void initialState(double[][] x) { + x[0][0] = 0; // initial position + x[1][0] = 0; // initial position + x[2][0] = 0; + x[3][0] = 0; + x[4][0] = 0; + x[5][0] = 0; + + + } + + @Override + public void initialStateCovariance(double[][] cov) { + // Sets initial variance for state variables. + cov[0][0] = 0.01; // 10 cm accuracy for X + cov[1][1] = 0.01; // 10 cm accuracy for Y + cov[2][2] = 0.0001; // 0.01 m/s accuracy for X + cov[3][3] = 8e-3; // 5 deg sqrd in rad for phi + cov[4][4] = 1e-5; // assume not moving : 0.2 deg/s for omega + cov[5][5] = 1e0; // assume 1 m/s^2 + } + + @Override + public void stateFunction(double[][] x, double[][] f) { + double X = x[0][0]; + double Y = x[1][0]; + double v = x[2][0]; + double phi = x[3][0]; + double omega = x[4][0]; + double acc_bias = x[5][0]; + + // The main system dynamics: + f[0][0] = v * cos(phi); // 2 D motion + f[1][0] = v * sin(phi); // 2 D motion + f[2][0] = m_acc-acc_bias; // Acceleration enters here + f[3][0] = omega; // phi derivative is omega + f[4][0] = 0; // Assume constant omega + } + + @Override + public void stateFunctionJacobian(double[][] x, double[][] j) { + double X = x[0][0]; + double Y = x[1][0]; + double v = x[2][0]; + double phi = x[3][0]; + double omega = x[4][0]; + double acc_bias = x[5][0]; + + // Derivative of state equation + j[0][2] = cos(phi); + j[1][2] = sin(phi); + j[0][3] = -v * sin(phi); + j[1][3] = v * cos(phi); + j[3][4] = 1; + j[2][5] = -1; + } + + @Override + public void processNoiseCovariance(double[][] cov) { + cov[0][0] = 0; // Assume the position is not changing by itself - use a very small covariance + cov[1][1] = 0; // Assume the position is not changing by itself - use a very small covariance + cov[2][2] = 1e-4; // Allow change in velocity can - change by measurements + cov[3][3] = 1e-9; // assume phi is not changing + cov[4][4] = 1e-2; // Allow change in omega + cov[4][4] = 1e1; // Allow change in bias + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drivetrain/auto/FollowPath.java b/src/main/java/frc/robot/subsystems/drivetrain/auto/FollowPath.java index 31e88479..5de350bf 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/auto/FollowPath.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/auto/FollowPath.java @@ -18,10 +18,14 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Constants; import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.drivetrain.FullLocalization; import frc.robot.utilities.Utils; import org.ghrobotics.lib.debug.FalconDashboard; import org.techfire225.webapp.FireLog; +import static frc.robot.subsystems.drivetrain.Drivetrain.localization; + + /** * This command handles trajectory-following. * A modified fork of {@link edu.wpi.first.wpilibj2.command.RamseteCommand} @@ -50,7 +54,7 @@ public void initialize() { FalconDashboard.INSTANCE.setFollowingPath(true); prevTime = 0; var initialState = trajectory.sample(0); - drivetrain.setPose(trajectory.getInitialPose(), trajectory.getInitialPose().getRotation()); //TODO: Ommit in the real world + localization.setPose(trajectory.getInitialPose(), trajectory.getInitialPose().getRotation()); //TODO: Ommit in the real world prevSpeeds = kinematics.toWheelSpeeds( new ChassisSpeeds(initialState.velocityMetersPerSecond, 0, @@ -69,7 +73,7 @@ public void execute() { Trajectory.State state = trajectory.sample(curTime); var targetWheelSpeeds = kinematics.toWheelSpeeds( - follower.calculate(drivetrain.getPose(), state) + follower.calculate(localization.getPose(), state) ); diff --git a/src/main/java/frc/robot/subsystems/turret/commands/PoseVisionTurret.java b/src/main/java/frc/robot/subsystems/turret/commands/PoseVisionTurret.java index 75a48e73..d4230b33 100644 --- a/src/main/java/frc/robot/subsystems/turret/commands/PoseVisionTurret.java +++ b/src/main/java/frc/robot/subsystems/turret/commands/PoseVisionTurret.java @@ -1,16 +1,22 @@ package frc.robot.subsystems.turret.commands; import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Robot; +import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.turret.Turret; import frc.robot.utilities.Utils; import frc.robot.utilities.VisionModule; +import static frc.robot.RobotContainer.navx; public class PoseVisionTurret extends CommandBase { private final Turret turret; + private final Drivetrain drivetrain; - public PoseVisionTurret(Turret turret) { + public PoseVisionTurret(Turret turret, Drivetrain drivetrain) { this.turret = turret; + this.drivetrain = drivetrain; } @Override @@ -23,6 +29,7 @@ public void execute() { Pose2d robotPose = VisionModule.getRobotPose(); if(robotPose == null || turret.inDeadZone()) return; turret.setAngle(Utils.calculateTurretAngle(robotPose, true)); + drivetrain.setPose(robotPose, new Rotation2d(Math.toRadians(navx.getAngle()))); } @Override diff --git a/src/main/java/frc/robot/subsystems/turret/commands/TurretSwitching.java b/src/main/java/frc/robot/subsystems/turret/commands/TurretSwitching.java index 88f52cfe..d3234759 100644 --- a/src/main/java/frc/robot/subsystems/turret/commands/TurretSwitching.java +++ b/src/main/java/frc/robot/subsystems/turret/commands/TurretSwitching.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.drivetrain.Drivetrain; +import frc.robot.subsystems.drivetrain.FullLocalization; import frc.robot.subsystems.turret.Turret; import frc.robot.utilities.VisionModule; @@ -10,8 +11,7 @@ public class TurretSwitching extends CommandBase { private final VisionTurret visionTurret; private final Turret turret; - public TurretSwitching(Turret turret, Drivetrain drivetrain) { - addRequirements(turret); + public TurretSwitching(Turret turret, FullLocalization localization, Drivetrain drivetrain) { this.turret = turret; this.localizationTurret = new LocalizationTurret(turret, drivetrain); this.visionTurret = new VisionTurret(turret); diff --git a/src/test/java/frc/robot/subsystems/FullLocalizationTest.java b/src/test/java/frc/robot/subsystems/FullLocalizationTest.java new file mode 100644 index 00000000..1b07243e --- /dev/null +++ b/src/test/java/frc/robot/subsystems/FullLocalizationTest.java @@ -0,0 +1,423 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; +import frc.robot.subsystems.drivetrain.FullLocalization; +import org.junit.Before; +import org.junit.Test; + +import static java.lang.Math.*; +import static org.junit.Assert.*; + +public class FullLocalizationTest { + + FullLocalization localization; + DifferentialDriveOdometry wpi_localization; + double m_width = 1; + @Before + public void setUp() throws Exception { + Rotation2d angle = new Rotation2d(0); + localization = new FullLocalization(angle,m_width); + wpi_localization = new DifferentialDriveOdometry(angle); + } + + @Test + public void updateNominal() { + Rotation2d angle = new Rotation2d(0); + + double left ; + double right; + + + double dt = 2e-2; // 20 msec timestep + double time = dt; + double[] acc = {0.1,0.2,0.3,0.4,0.6,0.8,1,1,1,1,1,1,1,1,1,0.8,0.8,0.8,0.7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 , 0 ,0 ,0 }; // m/s^2 + double[] omega = {0 ,0 ,0 ,0 , 0,0 ,0,0,0,0,0,0,0,0,0,0 ,0 , 0 , 0 ,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.02,0.02,0.02}; // rad/s + + double[] v = new double[acc.length]; + double[] phi = new double[acc.length]; + double[] x = new double[acc.length]; + double[] y = new double[acc.length]; + double[] left_track = new double[acc.length]; + double[] right_track = new double[acc.length]; + double[] right_track_pos_x = new double[acc.length]; + double[] right_track_pos_y = new double[acc.length]; + double[] left_track_pos_x = new double[acc.length]; + double[] left_track_pos_y = new double[acc.length]; + + v[0] = 0; + phi[0] = 0; + x[0] = 0; + y[0] = 0; + right_track_pos_x[0] = x[0] + m_width/2*sin(-phi[0]); + right_track_pos_y[0] = y[0] - m_width/2*cos(-phi[0]); + left_track_pos_x[0] = x[0] - m_width/2*sin(-phi[0]); + left_track_pos_y[0] = y[0] + m_width/2*cos(-phi[0]); + right_track[0]=0; + left_track[0]=0; + + + for (int i=1; i < acc.length; i ++ ) + { + omega[i] = omega[i]+0.1; // Create constant angular rate + v[i]=v[i-1]+dt*acc[i-1]; + phi[i]=phi[i-1]+dt*omega[i-1]; + x[i] = x[i-1] + dt*(v[i] + v[i-1])/2*cos((phi[i]+phi[i-1])/2); + y[i] = y[i-1] + dt*(v[i] + v[i-1])/2*sin((phi[i]+phi[i-1])/2); + + + right_track_pos_x[i] = x[i] + m_width/2*sin(-phi[i]); + right_track_pos_y[i] = y[i] - m_width/2*cos(-phi[i]); + left_track_pos_x[i] = x[i] - m_width/2*sin(-phi[i]); + left_track_pos_y[i] = y[i] + m_width/2*cos(-phi[i]); + + left_track[i] =left_track[i-1] + sqrt( pow(left_track_pos_x[i]-left_track_pos_x[i-1],2) + pow(left_track_pos_y[i]-left_track_pos_y[i-1],2) ); + right_track[i] = right_track[i-1] + sqrt( pow(right_track_pos_x[i]-right_track_pos_x[i-1],2) + pow(right_track_pos_y[i]-right_track_pos_y[i-1],2) ); + } + + + + Pose2d ref_pose; + Pose2d ekf_pose; + + for (int i =1; i 8) { // ignore first cycles, before KF is stabilized + assertEquals(ref_pose.getTranslation().getX(), ekf_pose.getTranslation().getX(), 1e-2); + assertEquals(ref_pose.getTranslation().getY(), ekf_pose.getTranslation().getY(), 1e-2); + assertEquals(ref_pose.getRotation().getDegrees(), ekf_pose.getRotation().getDegrees(), 1); + assertEquals(v[i], localization.filter.model.state_estimate.data[2][0], 0.07); + assertEquals(omega[i], localization.filter.model.state_estimate.data[4][0], 0.07); + assertEquals(phi[i],ekf_pose.getRotation().getRadians(), 0.05); + } + time += dt; + } + } + + @Test + public void initial_pose() { + Rotation2d angle ; + double gyro_offset = 0.3; + + double left ; + double right; + + + double dt = 2e-2; // 20 msec timestep + double time = dt; + double[] acc = {0.1,0.2,0.3,0.4,0.6,0.8,1,1,1,1,1,1,1,1,1,0.8,0.8,0.8,0.7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 , 0 ,0 ,0 }; // m/s^2 + double[] omega = {0 ,0 ,0 ,0 , 0,0 ,0,0,0,0,0,0,0,0,0,0 ,0 , 0 , 0 ,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.02,0.02,0.02}; // rad/s + + double[] v = new double[acc.length]; + double[] phi = new double[acc.length]; + double[] x = new double[acc.length]; + double[] y = new double[acc.length]; + double[] left_track = new double[acc.length]; + double[] right_track = new double[acc.length]; + double[] right_track_pos_x = new double[acc.length]; + double[] right_track_pos_y = new double[acc.length]; + double[] left_track_pos_x = new double[acc.length]; + double[] left_track_pos_y = new double[acc.length]; + + v[0] = 0; + phi[0] = toRadians(720); + x[0] = 10; + y[0] = 5; + right_track_pos_x[0] = x[0] + m_width/2*sin(-phi[0]); + right_track_pos_y[0] = y[0] - m_width/2*cos(-phi[0]); + left_track_pos_x[0] = x[0] - m_width/2*sin(-phi[0]); + left_track_pos_y[0] = y[0] + m_width/2*cos(-phi[0]); + right_track[0]=0; + left_track[0]=0; + + + for (int i=1; i < acc.length; i ++ ) + { + omega[i] = omega[i]+0.1; // Create constant angular rate + v[i]=v[i-1]+dt*acc[i-1]; + phi[i]=phi[i-1]+dt*omega[i-1]; + x[i] = x[i-1] + dt*(v[i] + v[i-1])/2*cos((phi[i]+phi[i-1])/2); + y[i] = y[i-1] + dt*(v[i] + v[i-1])/2*sin((phi[i]+phi[i-1])/2); + + + right_track_pos_x[i] = x[i] + m_width/2*sin(-phi[i]); + right_track_pos_y[i] = y[i] - m_width/2*cos(-phi[i]); + left_track_pos_x[i] = x[i] - m_width/2*sin(-phi[i]); + left_track_pos_y[i] = y[i] + m_width/2*cos(-phi[i]); + + left_track[i] =left_track[i-1] + sqrt( pow(left_track_pos_x[i]-left_track_pos_x[i-1],2) + pow(left_track_pos_y[i]-left_track_pos_y[i-1],2) ); + right_track[i] = right_track[i-1] + sqrt( pow(right_track_pos_x[i]-right_track_pos_x[i-1],2) + pow(right_track_pos_y[i]-right_track_pos_y[i-1],2) ); + } + + + + Pose2d ref_pose; + Pose2d ekf_pose; + + + // localization = new FullLocalization(new Rotation2d(phi[0]+gyro_offset),new Pose2d(x[0],y[0], new Rotation2d(phi[0])) ,m_width); + localization = new FullLocalization(new Rotation2d(),new Pose2d() ,m_width); + wpi_localization = new DifferentialDriveOdometry( new Rotation2d(phi[0]+gyro_offset), new Pose2d(x[0],y[0],new Rotation2d(phi[0]))); + + localization.resetPosition(new Pose2d(x[0],y[0], new Rotation2d(phi[0])) , new Rotation2d(phi[0]+gyro_offset),time); + + for (int i =1; i 8) { // ignore first cycles, before KF is stabilized + assertEquals(ref_pose.getTranslation().getX(), ekf_pose.getTranslation().getX(), 1e-2); + assertEquals(ref_pose.getTranslation().getY(), ekf_pose.getTranslation().getY(), 1e-2); +// assertEquals(ref_pose.getRotation().getDegrees(), ekf_pose.getRotation().getDegrees(), 1); + assertEquals(v[i], localization.filter.model.state_estimate.data[2][0], 0.07); + assertEquals(omega[i], localization.filter.model.state_estimate.data[4][0], 0.07); + assertEquals(phi[i],ekf_pose.getRotation().getRadians(), 0.05); + } + time += dt; + } + } + + @Test + public void updateAccBias() { + Rotation2d angle = new Rotation2d(0); + + double left ; + double right; + + + double dt = 2e-2; // 20 msec timestep + double time = dt; + double[] acc = {0.1,0.2,0.3,0.4,0.6,0.8,1,1,1,1,1,1,1,1,1,0.8,0.8,0.8,0.7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 , 0 ,0 ,0 }; // m/s^2 + double[] omega = {0 ,0 ,0 ,0 , 0,0 ,0,0,0,0,0,0,0,0,0,0 ,0 , 0 , 0 ,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.02,0.02,0.02}; // rad/s + + double[] v = new double[acc.length]; + double[] phi = new double[acc.length]; + double[] x = new double[acc.length]; + double[] y = new double[acc.length]; + double[] left_track = new double[acc.length]; + double[] right_track = new double[acc.length]; + double[] right_track_pos_x = new double[acc.length]; + double[] right_track_pos_y = new double[acc.length]; + double[] left_track_pos_x = new double[acc.length]; + double[] left_track_pos_y = new double[acc.length]; + + v[0] = 0; + phi[0] = 0; + x[0] = 0; + y[0] = 0; + right_track_pos_x[0] = x[0] + m_width/2*sin(-phi[0]); + right_track_pos_y[0] = y[0] - m_width/2*cos(-phi[0]); + left_track_pos_x[0] = x[0] - m_width/2*sin(-phi[0]); + left_track_pos_y[0] = y[0] + m_width/2*cos(-phi[0]); + right_track[0]=0; + left_track[0]=0; + + + for (int i=1; i < acc.length; i ++ ) + { + omega[i] = omega[i]+0.1; // Create constant angular rate + v[i]=v[i-1]+dt*acc[i-1]; + phi[i]=phi[i-1]+dt*omega[i-1]; + x[i] = x[i-1] + dt*(v[i] + v[i-1])/2*cos((phi[i]+phi[i-1])/2); + y[i] = y[i-1] + dt*(v[i] + v[i-1])/2*sin((phi[i]+phi[i-1])/2); + + + right_track_pos_x[i] = x[i] + m_width/2*sin(-phi[i]); + right_track_pos_y[i] = y[i] - m_width/2*cos(-phi[i]); + left_track_pos_x[i] = x[i] - m_width/2*sin(-phi[i]); + left_track_pos_y[i] = y[i] + m_width/2*cos(-phi[i]); + + left_track[i] =left_track[i-1] + sqrt( pow(left_track_pos_x[i]-left_track_pos_x[i-1],2) + pow(left_track_pos_y[i]-left_track_pos_y[i-1],2) ); + right_track[i] = right_track[i-1] + sqrt( pow(right_track_pos_x[i]-right_track_pos_x[i-1],2) + pow(right_track_pos_y[i]-right_track_pos_y[i-1],2) ); + } + + + + Pose2d ref_pose; + Pose2d ekf_pose; + + for (int i =1; i 8) { // ignore first cycles, before KF is stabilized + assertEquals(ref_pose.getTranslation().getX(), ekf_pose.getTranslation().getX(), 1e-2); + assertEquals(ref_pose.getTranslation().getY(), ekf_pose.getTranslation().getY(), 1e-2); + assertEquals(ref_pose.getRotation().getDegrees(), ekf_pose.getRotation().getDegrees(), 1); + assertEquals(v[i], localization.filter.model.state_estimate.data[2][0], 0.07); + assertEquals(omega[i], localization.filter.model.state_estimate.data[4][0], 0.07); + assertEquals(phi[i],ekf_pose.getRotation().getRadians(), 0.05); + } + time += dt; + } + + + } + + + @Test + public void encoderValid() + { + Rotation2d angle = new Rotation2d(20); + Pose2d pose = new Pose2d(0,0,angle); + + localization.resetPosition(pose,angle,0); + assertEquals(true,localization.EncoderValid(angle,0,0)); + assertEquals(false,localization.EncoderValid(angle,1,0)); + assertEquals(false,localization.EncoderValid(angle,0.1,0)); + } + + @Test + public void updateNoEncoder() { + Rotation2d angle = new Rotation2d(0); + + double left ; + double right; + + + double dt = 2e-2; + double time = dt; + double[] acc = {0.1,0.2,0.3,0.4,0.6,0.8,1,1,1,1,1,1,1,1,1,0.8,0.8,0.8,0.7,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 , 0 ,0 ,0 }; // m/s^2 + double[] omega = {0 ,0 ,0 ,0 , 0,0 ,0,0,0,0,0,0,0,0,0,0 ,0 , 0 , 0 ,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.02,0.02,0.02}; // rad/s + + double[] v = new double[acc.length]; + double[] phi = new double[acc.length]; + double[] x = new double[acc.length]; + double[] y = new double[acc.length]; + double[] left_track = new double[acc.length]; + double[] right_track = new double[acc.length]; + double[] right_track_pos_x = new double[acc.length]; + double[] right_track_pos_y = new double[acc.length]; + double[] left_track_pos_x = new double[acc.length]; + double[] left_track_pos_y = new double[acc.length]; + + v[0] = 0; + phi[0] = 0; + x[0] = 0; + y[0] = 0; + right_track_pos_x[0] = x[0] + m_width/2*sin(-phi[0]); + right_track_pos_y[0] = y[0] - m_width/2*cos(-phi[0]); + left_track_pos_x[0] = x[0] - m_width/2*sin(-phi[0]); + left_track_pos_y[0] = y[0] + m_width/2*cos(-phi[0]); + right_track[0]=0; + left_track[0]=0; + + + + for (int i=1; i < acc.length; i ++ ) + { + omega[i] = omega[i]+0.1; // Create constant angular rate + v[i]=v[i-1]+dt*acc[i-1]; + phi[i]=phi[i-1]+dt*omega[i-1]; + x[i] = x[i-1] + dt*(v[i] + v[i-1])/2*cos((phi[i]+phi[i-1])/2); + y[i] = y[i-1] + dt*(v[i] + v[i-1])/2*sin((phi[i]+phi[i-1])/2); + + + right_track_pos_x[i] = x[i] + m_width/2*sin(-phi[i]); + right_track_pos_y[i] = y[i] - m_width/2*cos(-phi[i]); + left_track_pos_x[i] = x[i] - m_width/2*sin(-phi[i]); + left_track_pos_y[i] = y[i] + m_width/2*cos(-phi[i]); + + left_track[i] =left_track[i-1] + sqrt( pow(left_track_pos_x[i]-left_track_pos_x[i-1],2) + pow(left_track_pos_y[i]-left_track_pos_y[i-1],2) ); + right_track[i] = right_track[i-1] + sqrt( pow(right_track_pos_x[i]-right_track_pos_x[i-1],2) + pow(right_track_pos_y[i]-right_track_pos_y[i-1],2) ); + } + + + + Pose2d ref_pose; + Pose2d ekf_pose; + + for (int i =1; i= 15) // Insert wrong encoder value + { + left_track[i] += 1; + } + + angle = Rotation2d.fromDegrees( toDegrees( phi[i])); + left = left_track[i]; + right = right_track[i]; + + ref_pose = wpi_localization.update(angle, left, right); + ekf_pose = localization.update(angle, left, right, acc[i] + 2*(random()-0.5),time); + + + if ( i> 8) { // ignore first cycles, before KF is stabilized + assertEquals(x[i], ekf_pose.getTranslation().getX(), 1e-2); + assertEquals(y[i], ekf_pose.getTranslation().getY(), 1e-2); + assertEquals(v[i], localization.filter.model.state_estimate.data[2][0], 0.07); + assertEquals(omega[i], localization.filter.model.state_estimate.data[4][0], 0.07); + assertEquals(phi[i],ekf_pose.getRotation().getRadians(), 0.05); + } + + if (i>15) + { + // WPI localization should be wrong + assertNotEquals(ref_pose.getTranslation().getX(), ekf_pose.getTranslation().getX(), 1e-2); + + } + time += dt; + } + } + + @Test + public void TestData() { + Rotation2d angle = new Rotation2d(0); + + double left ; + double right; + + + double dt = 2e-2; + double time = dt; + + + + double[] left_track = {0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000151,0.000151,0.000505,0.001338,0.002979,0.003358,0.002929,0.003030,0.003005,0.003005,0.003005,0.003080,0.003484,0.004418,0.005580,0.007221,0.007650,0.008079,0.010882,0.014240,0.018153,0.022420,0.027445,0.033201,0.039084,0.051834,0.058752,0.065670,0.065670,0.072689,0.078320,0.083293,0.088621,0.094024,0.099503,0.105436,0.111799,0.117732,0.124423,0.132628,0.141920,0.152170,0.161840,0.171839,0.182948,0.195748,0.209534,0.224759,0.241069,0.259121,0.279522,0.302977,0.328983,0.357109,0.386574,0.417553,0.447876,0.479259,0.514001,0.549575,0.585731,0.658622,0.693060,0.728988,0.765901,0.803041,0.803041,0.840282,0.879694,0.917516,0.954833,0.993588,1.033001,1.072691,1.113037,1.149950,1.185550,1.221730,1.258390,1.294369,1.330171,1.366629,1.400764,1.436263,1.507816,1.582551,1.619640,1.655316,1.691925,1.730075,1.767745,1.804683,1.842126,1.878004,1.916028,1.953773,1.990913,2.029467,2.106827,2.144144,2.183026,2.183026,2.222565,2.261548,2.301894,2.342544,2.422530,2.461386,2.500546,2.500546,2.540362,2.579118,2.616536,2.654459,2.693063,2.731743,2.770751,2.811174,2.850081,2.888963,2.927315,2.965540,3.042345,3.042345,3.078122,3.114075,3.151240,3.188860,3.226454,3.302375,3.338303,3.409654,3.445582,3.481965,3.518120,3.552458,3.624011,3.660621,3.697407,3.735456,3.772798,3.808221,3.845007,3.879698,3.913404,3.947868,3.982433,4.016593,4.050729,4.086707,4.086707,4.123696,4.161442,4.199844,4.239787,4.278239,4.316061,4.389811,4.426597,4.461995,4.500019,4.536881,4.572077,4.609141,4.646685,4.684027,4.720308,4.753888,4.753888,4.788958,4.823270,4.857355,4.892930,4.929464,4.966806,5.001875,5.038914,5.077114,5.114608,5.151192,5.224235,5.260567,5.296798,5.332019,5.368099,5.405239,5.405239,5.442000,5.477347,5.514336,5.551955,5.589550,5.626286,5.663754,5.699682,5.735509,5.771715,5.806784,5.842940,5.879373,5.915831,5.950673,5.986879,6.023994,6.061740,6.099662,6.137913,6.175558,6.248172,6.284100,6.320785,6.356865,6.392742,6.426651,6.461543,6.497699,6.533425,6.568898,6.605003,6.642194,6.677844,6.712408,6.744448,6.773534,6.799085,6.821707,6.841603,6.860539,6.879576,6.899068,6.917423,6.935905,6.957189,6.978978,6.999580,7.020132,7.039422,7.058661,7.077445,7.094993,7.111581,7.127714,7.143924,7.159704,7.175156,7.221309,7.221309,7.251885,7.267160,7.282233,7.297382,7.312581,7.312581,7.328134,7.343535,7.359820,7.379110,7.399485,7.420668,7.441372,7.455687,7.469952,7.503078,7.520550,7.539713,7.557790,7.572611,7.587785,7.602227,7.616669,7.616669,7.631565,7.646916,7.662797,7.678577,7.694484,7.710769,7.726927,7.743364,7.777954,7.795299,7.795299,7.813781,7.831126,7.849255,7.868064,7.886723,7.906441,7.927448,7.971455,7.993876,7.993876,8.016170,8.038842,8.062399,8.085526,8.109916,8.135745,8.160614,8.185913,8.212196,8.239413,8.266353,8.293444,8.320106,8.347172,8.375223,8.402945,8.430491,8.457304,8.483941,8.510502,8.537240,8.588292,8.614449,8.640202,8.665929,8.692718,8.745941,8.771997,8.797901,8.823099,8.849963,8.876776,8.904069,8.931363,8.958328,8.985192,9.011652,9.037632,9.037632,9.062426,9.087951,9.112720,9.137135,9.162534,9.189020,9.242116,9.242116,9.268526,9.294506,9.321597,9.348941,9.376562,9.404007,9.431856,9.459932,9.487301,9.514568,9.540195,9.566883,9.592282,9.617379,9.642602,9.670526,9.699713,9.728799,9.757304,9.786061,9.815955,9.845268,9.874253,9.934646,9.934646,9.968984,10.001024,10.069345,10.103657,10.139232,10.175665,10.211163,10.242572,10.273703,10.305137,10.337101,10.337101,10.369696,10.402544,10.436200,10.469451,10.501012,10.533405,10.596449,10.628615,10.661059,10.693680,10.725821,10.757911,10.757911,10.789926,10.821688,10.853980,10.886954,10.920332,10.953558,10.984563,11.016325,11.045992,11.075406,11.105426,11.137011,11.169531,11.201167,11.232853,11.265827,11.299508,11.333441,11.366895,11.400778,11.434560,11.466423,11.499271,11.531412,11.563704,11.596880,11.630334,11.663409,11.695474,11.728473,11.761649,11.795355,11.829011,11.862061,11.895944,11.928337,11.961286,11.994285,12.061470,12.061470,12.095429,12.129160,12.162513,12.195285,12.262016,12.295318,12.329125,12.365584,12.402471,12.471020,12.506039,12.541790,12.578324,12.615085,12.650483,12.686512,12.722592,12.757661,12.792403,12.827826,12.862921,12.896374,12.931015,12.963711,12.997846,13.032537,13.068592,13.104722,13.104722,13.139917,13.175517,13.211471,13.247777,13.283276,13.318447,13.385026,13.418177,13.450166,13.482458,13.515862,13.550047,13.583526,13.616728,13.685907,13.720497,13.754355,13.754355,13.789021,13.822676,13.855044,13.888372,13.920513,13.952199,13.984845,14.019107,14.052737,14.086241,14.120882,14.154840,14.188925,14.258130,14.291988,14.324912,14.358921,14.392602,14.426611,14.461277,14.496775,14.532123,14.566056,14.600974,14.636852,14.673462,14.709440,14.746858,14.783266,14.783266,14.817426,14.887363,14.922307,14.959952,14.997193,15.031808,15.065817,15.100558,15.135678,15.172288,15.208948,15.245154,15.280603,15.316455,15.352055,15.388008,15.424643,15.461808,15.497130,15.528413,15.560680,15.592745,15.625239,15.658743,15.691591,15.724262,15.756681,15.789781,15.822704,15.855779,15.890369,15.926348,15.961695,15.996462,16.031658,16.067358,16.103564,16.139947,16.176481,16.213242,16.248867,16.285401,16.320597,16.356323,16.393715,16.430754,16.466455,16.501802,16.537806,16.574063,16.610117,16.646272,16.683033,16.719668,16.756985,16.793393,16.829674,16.867092,16.903550,16.939377,16.974927,17.010678,17.046707,17.082534,17.118613,17.155501,17.192489,17.228544,17.264876,17.301182,17.301182,17.338474,17.375715,17.411491,17.445753,17.481555,17.553714,17.590450,17.627161,17.662962,17.697956,17.734137,17.769636,17.806195,17.843209,17.879743,17.914130,17.947281,17.978437,18.006488,18.032039,18.054813,18.072436,18.090413,18.110561,18.130204,18.148812,18.166763,18.183073,18.198273,18.212260,18.225187,18.237306,18.247910,18.256974,18.265029,18.273133,18.282349,18.291413,18.300325,18.310197,18.318807,18.327139,18.334865,18.341631,18.347615,18.353447,18.364102,18.368975,18.373974,18.379099,18.384250,18.388997,18.393567,18.397707,18.401166,18.403994,18.405888,18.407049,18.407756,18.407908,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.407832,18.404928,18.400636,18.397505,18.395662,18.394602,18.392784,18.390890,18.389199,18.386573,18.383720,18.380589,18.376676,18.372409,18.366122,18.359785,18.351932,18.343121,18.333880,18.324311,18.314717,18.304491,18.293357,18.282020,18.270558,18.258792,18.247153,18.236119,18.225667,18.215719,18.206074,18.196833,18.187391,18.177443,18.167066,18.155906,18.143408,18.130507,18.117100,18.103845,18.090236,18.076753,18.063246,18.049965,18.036559,18.023354,18.010174,17.997298,17.983664,17.969828,17.955992,17.942055,17.927689,17.913272,17.898982,17.884413,17.870098,17.855479,17.840457,17.824954,17.809553,17.792738,17.776554,17.761405,17.746786,17.733102,17.720225,17.706869,17.693614,17.680257,17.664856,17.649960,17.634281,17.618324,17.602291,17.586360,17.570125,17.554042,17.538136,17.522078,17.506601,17.491402,17.476379,17.461003,17.445374,17.428256,17.411769,17.394828,17.394828,17.378063,17.360465,17.342842,17.324436,17.306687,17.289543,17.272627,17.257453,17.242279,17.227433,17.210769,17.195620,17.180497,17.165120,17.151714,17.139317,17.127021,17.116644,17.106974,17.099147,17.091674,17.085362,17.078494,17.068117,17.057160,17.047414,17.038577,17.030523,17.022418,17.014415,17.006562,16.997195,16.987349,16.978184,16.970129,16.963312,16.957278,16.950461,16.944099,16.938569,16.929051,16.922991,16.916679,16.909256,16.900899,16.892719,16.884917,16.877292,16.869793,16.861411,16.852398,16.843763,16.835582,16.828159,16.822024,16.816924,16.812607,16.809021,16.806143,16.803744,16.800437,16.796397,16.792055,16.787384,16.782183,16.776401,16.771907,16.770063,16.770063,16.768145,16.765645,16.762918,16.759434,16.755470,16.751329,16.740675,16.732823,16.723077,16.712296,16.700051,16.686694,16.672682,16.658240,16.642434,16.625897,16.610470,16.593857,16.577446,16.560302,16.541922,16.521370,16.495844,16.468046,16.441889,16.413762,16.384071,16.353419,0}; + double[] right_track = {0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.002954,0.006994,0.010251,0.012245,0.013987,0.015755,0.017724,0.020400,0.023784,0.028429,0.028429,0.035044,0.041710,0.049941,0.068119,0.078496,0.090287,0.102785,0.118565,0.132679,0.147424,0.163204,0.176484,0.188679,0.201530,0.214836,0.228420,0.242356,0.257177,0.272250,0.288030,0.303684,0.319565,0.335547,0.352640,0.370263,0.388669,0.388669,0.406898,0.426062,0.445604,0.465701,0.488197,0.511577,0.534048,0.556822,0.579570,0.601940,0.625294,0.649785,0.675084,0.698362,0.722070,0.746081,0.769284,0.792841,0.816498,0.840812,0.864672,0.888960,0.913653,0.938346,0.964225,0.990079,1.016286,1.042418,1.064384,1.085466,1.103897,1.120965,1.136568,1.150732,1.164038,1.176359,1.191180,1.209889,1.226755,1.242358,1.273867,1.289521,1.289521,1.304342,1.318733,1.332090,1.344941,1.356959,1.371552,1.388696,1.408617,1.430406,1.452700,1.473858,1.492617,1.531600,1.580430,1.609011,1.636051,1.663193,1.690890,1.718335,1.746638,1.777794,1.810945,1.844424,1.877802,1.912468,1.948623,1.989853,2.028205,2.062416,2.100010,2.140382,2.177876,2.215167,2.253569,2.292704,2.325324,2.355597,2.382966,2.407936,2.432604,2.486988,2.511479,2.535389,2.557910,2.577023,2.598736,2.622621,2.645950,2.665618,2.665618,2.683671,2.700612,2.716771,2.731617,2.745428,2.759642,2.772973,2.798979,2.810113,2.831498,2.842456,2.853161,2.867830,2.885327,2.921710,2.939888,2.958521,2.978114,2.995131,3.012123,3.029645,3.043860,3.055651,3.067013,3.078526,3.090165,3.101804,3.113621,3.125260,3.136899,3.147806,3.157805,3.167752,3.176867,3.185527,3.194793,3.210624,3.229156,3.245163,3.260110,3.274426,3.287782,3.300078,3.315328,3.332193,3.348302,3.362844,3.376352,3.388976,3.400616,3.412406,3.425056,3.440104,3.455328,3.471159,3.487191,3.502946,3.518247,3.534153,3.548721,3.562380,3.577453,3.591718,3.605681,3.620400,3.635271,3.650673,3.665544,3.682258,3.698392,3.714020,3.729270,3.743636,3.757018,3.770071,3.784690,3.800192,3.814710,3.823774,3.834125,3.846371,3.858591,3.870306,3.881844,3.892247,3.901790,3.911107,3.920196,3.929488,3.938930,3.948146,3.957210,3.967233,3.980463,3.996496,4.013160,4.032575,4.054718,4.084056,4.107411,4.130134,4.158715,4.191613,4.220447,4.248926,4.279780,4.317349,4.363326,4.396173,4.428870,4.465050,4.507745,4.552914,4.600153,4.641762,4.689405,4.734649,4.780298,4.825795,4.869373,4.907548,4.941557,4.971855,5.054694,5.054694,5.108523,5.132812,5.154197,5.174623,5.192574,5.210096,5.226810,5.240672,5.252614,5.263142,5.272913,5.283896,5.294854,5.304474,5.312805,5.320203,5.327348,5.333711,5.339619,5.345502,5.351183,5.356232,5.361509,5.367594,5.374436,5.381657,5.389332,5.397134,5.405163,5.412990,5.420640,5.428063,5.434855,5.440864,5.446090,5.451367,5.457401,5.465758,5.476363,5.487295,5.497521,5.507317,5.517366,5.527212,5.527212,5.536428,5.544583,5.551602,5.557813,5.563570,5.569503,5.575487,5.581067,5.586343,5.593994,5.602881,5.609395,5.616692,5.624493,5.632648,5.641207,5.650322,5.659462,5.667844,5.675343,5.682362,5.699430,5.707534,5.715235,5.724046,5.731949,5.740407,5.749067,5.756995,5.764645,5.772296,5.780022,5.788303,5.796685,5.805194,5.814940,5.822413,5.828624,5.834860,5.841273,5.848217,5.856346,5.865183,5.875661,5.889851,5.925905,5.925905,5.944765,5.964080,5.982839,5.998367,6.015309,6.035078,6.056210,6.079944,6.103727,6.127259,6.151219,6.177326,6.202346,6.227393,6.251302,6.278318,6.309727,6.341211,6.369363,6.398448,6.429150,6.453464,6.474117,6.492245,6.506561,6.533248,6.533248,6.552714,6.572660,6.589299,6.605028,6.619243,6.631640,6.640527,6.648909,6.659160,6.678349,6.691730,6.705718,6.718367,6.729577,6.736571,6.743236,6.750836,6.760809,6.771161,6.783507,6.799009,6.815496,6.833423,6.851576,6.869704,6.887832,6.905758,6.923912,6.942292,6.960092,6.975140,6.991551,7.007306,7.023137,7.035887,7.047400,7.058863,7.070527,7.082344,7.094564,7.110243,7.127512,7.146196,7.166268,7.186694,7.208433,7.230323,7.252642,7.274658,7.296826,7.317580,7.337602,7.358305,7.379640,7.401833,7.425692,7.449451,7.471366,7.493610,7.518883,7.546454,7.576499,7.576499,7.604020,7.631187,7.659338,7.687591,7.746520,7.776035,7.806686,7.838474,7.871296,7.934467,7.967643,8.000794,8.034020,8.067752,8.100776,8.135492,8.164174,8.190811,8.217069,8.242014,8.266404,8.287662,8.308315,8.329145,8.350126,8.372244,8.397618,8.421806,8.421806,8.446094,8.468161,8.489294,8.511209,8.532468,8.552338,8.588872,8.605814,8.621468,8.637273,8.654038,8.671055,8.687441,8.704029,8.735892,8.751899,8.766796,8.766796,8.781187,8.801082,8.823225,8.847362,8.871954,8.894904,8.914169,8.953934,8.977541,9.004153,9.029654,9.055987,9.085250,9.114866,9.141376,9.168872,9.195635,9.223534,9.253074,9.283599,9.314174,9.344649,9.376158,9.407592,9.439531,9.472960,9.506009,9.539261,9.573144,9.573144,9.601169,9.647525,9.665805,9.681888,9.699688,9.719028,9.735464,9.750613,9.768287,9.786263,9.805477,9.823125,9.838451,9.852767,9.866678,9.880338,9.892330,9.902733,9.912680,9.921745,9.929748,9.937449,9.944998,9.952118,9.960930,9.969918,9.979840,9.989914,10.002033,10.014809,10.014809,10.026474,10.037381,10.047960,10.058513,10.068310,10.089165,10.099239,10.110146,10.120573,10.130647,10.140696,10.150795,10.161702,10.173317,10.185612,10.198009,10.209472,10.219925,10.232018,10.245375,10.261079,10.276834,10.291730,10.306147,10.320639,10.334955,10.350634,10.365076,10.378786,10.392016,10.400726,10.409487,10.419612,10.434205,10.443900,10.453899,10.465286,10.476597,10.487125,10.498032,10.508056,10.517524,10.527573,10.537773,10.549160,10.564031,10.581604,10.599959,10.621268,10.640002,10.656540,10.673936,10.694816,10.717968,10.740288,10.762001,10.782275,10.803282,10.824743,10.847516,10.872916,10.899906,10.927906,10.955654,10.984412,11.013876,11.043189,11.072957,11.103330,11.134108,11.164633,11.196319,11.229192,11.260929,11.290999,11.321524,11.353009,11.388002,11.422188,11.457208,11.494221,11.531159,11.569587,11.607585,11.646013,11.691762,11.776293,11.811565,11.849159,11.887890,11.926368,11.966058,12.005369,12.043923,12.081946,12.119768,12.160064,12.199805,12.237449,12.274413,12.313295,12.353717,12.392978,12.431936,12.470414,12.505483,12.541967,12.576632,12.609404,12.643287,12.678609,12.710902,12.740291,12.769780,12.797099,12.824897,12.850145,12.873626,12.894380,12.911574,12.926117,12.938236,12.948663,12.957778,12.965983,12.975174,12.986081,12.996256,13.006809,13.016530,13.025847,13.034355,13.041046,13.046196,13.050590,13.054528,13.057028,13.057432,13.055917,13.055008,13.053897,13.053316,13.053039,13.052938,13.052534,13.052483,13.052407,13.052256,13.051928,13.051372,13.050539,13.049706,13.050034,13.049984,13.050009,13.050009,13.049984,13.049958,13.049630,13.046499,13.042712,13.038597,13.034330,13.030063,13.025417,13.020090,13.014283,13.008678,13.004562,13.001179,12.997518,12.993403,12.988959,12.983758,12.977547,12.970629,12.963054,12.955101,12.947628,12.940760,12.933868,12.926470,12.919173,12.911877,12.903974,12.896147,12.888295,12.880165,12.872010,12.863905,12.855927,12.847696,12.839364,12.830527,12.821716,12.813232,12.804547,12.795432,12.786267,12.777027,12.767508,12.758116,12.748976,12.739836,12.731075,12.722188,12.712997,12.703454,12.693481,12.683255,12.672903,12.662400,12.651089,12.639525,12.627583,12.617863,12.608571,12.596528,12.583172,12.572163,12.561534,12.550526,12.538205,12.526389,12.513310,12.499449,12.485184,12.470616,12.455820,12.441202,12.424765,12.407849,12.389342,12.370355,12.350839,12.331650,12.331650,12.311856,12.291834,12.271661,12.251033,12.231238,12.210762,12.188140,12.163548,12.137114,12.113481,12.091288,12.069423,12.046700,12.023017,11.997618,11.971865,11.945430,11.918566,11.891172,11.863273,11.834566,11.805152,11.775687,11.747384,11.720267,11.690929,11.660354,11.630308,11.601526,11.570243,11.539213,11.506871,11.474932,11.443700,11.411029,11.345611,11.315818,11.287389,11.258025,11.227652,11.193996,11.160366,11.128427,11.095377,11.062908,11.031676,11.000621,10.969010,10.937046,10.904779,10.872234,10.839185,10.806968,10.776014,10.744706,10.711025,10.678001,10.647804,10.618037,10.586754,10.555169,10.523179,10.491114,10.458014,10.424106,10.390753,10.357628,10.325967,10.325967,10.295442,10.264765,10.231917,10.200332,10.172988,10.147033,10.119538,10.094038,10.069395,10.046344,10.024403,10.004079,9.986607,9.957572,9.944140,9.930228,9.915458,9.902076,9.889856,9.878570,9.867133,9.856200,9.846505,9.838350,9.832114,9.825070,9.817116,9.808835,0}; + double[] acc = {0.009807,0.019613,0.019613,0.000000,0.009807,0.019613,0.000000,0.009807,-0.019613,0.009807,0.009807,0.019613,0.000000,0.000000,0.000000,0.000000,0.009807,0.000000,0.000000,0.000000,0.019613,0.000000,0.019613,0.019613,-0.009807,-0.019613,0.000000,0.000000,-0.009807,0.000000,0.000000,0.009807,0.000000,0.000000,0.009807,0.000000,0.009807,-0.009807,0.000000,0.000000,0.019613,0.019613,-0.009807,0.000000,0.000000,0.009807,0.000000,0.000000,0.019613,0.019613,0.019613,0.000000,0.000000,0.000000,0.000000,0.000000,-0.009807,0.000000,0.000000,0.009807,-0.009807,0.009807,-0.009807,0.000000,-0.009807,0.000000,0.000000,0.009807,0.009807,0.000000,0.000000,-0.009807,0.019613,0.019613,0.000000,0.000000,0.009807,-0.009807,0.000000,0.009807,-0.019613,0.019613,0.000000,0.000000,0.000000,0.000000,0.000000,0.029420,0.000000,0.000000,0.000000,0.000000,0.000000,0.009807,0.000000,0.000000,0.009807,0.029420,0.029420,0.019613,-0.009807,0.009807,0.000000,0.000000,0.000000,-0.009807,0.009807,0.009807,0.000000,-0.009807,0.000000,0.000000,0.000000,0.000000,0.000000,-0.009807,0.000000,0.000000,0.000000,0.019613,-0.098066,-0.127486,0.049033,0.245166,0.686466,0.686466,0.480526,0.931632,0.931632,0.813952,1.863263,1.706357,1.127765,1.088538,0.804145,0.225553,0.666852,0.441299,4.393379,1.372931,1.108151,0.519752,0.637432,2.069203,1.333704,1.421964,0.862985,0.990472,2.157463,1.520031,0.333426,-1.804424,-0.813952,-0.764919,-1.235638,-0.637432,-0.686466,-2.912575,0.519752,1.922103,2.020170,-1.068925,-0.284393,3.216581,2.029977,1.157185,0.362846,-2.471276,-4.981778,-3.246001,-1.108151,-0.049033,-2.922382,-2.363403,-1.608291,-1.882877,-1.157185,-1.117958,-1.274864,-2.137850,3.255808,-0.304006,-1.608291,1.706357,-2.265336,-0.647239,-6.217416,-5.521144,8.041453,4.226666,-0.990472,2.873349,-4.844485,-1.951523,-1.304284,-0.892405,-2.226110,-2.834122,-1.461191,0.196133,2.098623,2.814509,0.902212,-0.804145,-3.324454,1.441578,3.040062,5.766310,-1.529837,-2.706635,-2.079010,3.128321,2.294756,1.088538,2.530116,7.364794,6.599875,2.951802,-1.127765,-1.245445,2.716442,-1.569064,-3.432327,2.314369,1.461191,1.461191,-4.089373,1.608291,4.079566,6.021283,6.050703,-0.176520,-0.402073,0.941438,-0.872792,1.255251,-1.078731,2.304563,-3.510781,-6.550842,-12.150439,-5.540757,-0.225553,0.235360,-0.196133,-3.402908,-4.746419,-3.491167,-1.696550,-3.049868,3.275421,-0.657046,0.715885,0.666852,-2.892962,3.824593,-0.872792,-1.098345,-0.088260,-4.099180,2.628182,-0.088260,-4.834678,-1.402351,1.196411,8.316039,-3.069481,-3.510781,0.617819,1.549451,-0.058840,-11.414941,-3.883433,2.147656,-0.058840,-3.187161,-2.510503,3.432327,6.560649,4.226666,1.765197,-0.500139,-4.305119,-3.981500,-0.411879,0.107873,-7.727640,-0.970858,-3.559814,1.059118,3.785367,-1.637711,-8.531786,-4.265893,-4.756225,-3.167548,-4.207053,-5.560370,-5.991863,1.657324,-1.323898,1.206218,-2.167270,3.844207,3.285228,-4.344346,-7.129435,-3.147935,-1.470998,-0.019613,-1.333704,-6.089930,-5.011198,3.334261,0.029420,-2.510503,1.539644,-1.922103,-2.912575,-5.226944,-3.098902,-0.578592,-1.598484,-3.089095,0.254973,-1.333704,1.863263,-0.813952,-2.392823,0.078453,0.078453,-2.167270,-1.510224,-2.216303,-0.539366,-0.402073,1.676937,-4.177633,-1.382738,0.176520,-2.294756,0.990472,1.225831,1.745584,-5.285784,-1.392544,-0.980665,2.333983,1.284671,1.539644,0.470719,3.736334,-1.343511,1.255251,-5.148491,1.520031,-3.579427,-0.500139,-4.775838,-6.864655,7.296148,3.344068,-5.766310,-3.638267,-8.217973,-3.804980,-4.314926,-5.756504,0.637432,5.207331,2.108430,0.509946,2.186883,2.392823,1.186605,6.217416,7.011754,8.875018,7.256921,2.559536,4.520866,3.393101,4.010920,8.933858,5.089651,4.138406,3.412714,0.343233,-3.059675,-2.118236,-0.117680,0.676659,0.039227,1.667131,-0.588399,0.941438,-1.117958,-0.460913,-0.284393,0.196133,-0.500139,-0.706079,0.304006,0.666852,1.274864,2.687022,-0.088260,-1.363124,-2.618375,-2.088816,-2.353596,-3.903047,-2.961608,0.843372,0.853179,0.519752,-3.177355,-7.286341,3.687300,13.660664,16.867438,16.220200,10.983448,10.963835,12.856518,5.089651,-5.442691,-2.392823,0.039227,3.716720,-4.834678,-5.903603,-2.049590,0.068647,-0.058840,-3.157741,-0.205940,2.785089,1.343511,3.677494,1.029698,-1.902490,-1.049312,-4.040340,-5.315204,-2.069203,-0.529559,0.480526,3.608847,-1.157185,0.882599,0.588399,1.971137,0.107873,0.117680,0.686466,-0.951245,3.295034,4.873905,4.452219,2.324176,-1.343511,-5.295591,-0.853179,0.706079,3.510781,0.156906,-4.795452,0.205940,-0.657046,-1.225831,2.726249,-2.471276,-1.520031,0.764919,3.000835,5.579984,0.490333,1.696550,0.333426,-0.696272,-0.872792,0.627626,-2.932188,1.470998,-3.069481,-2.363403,-2.775282,2.530116,-0.451106,-0.725692,6.344903,-2.383016,0.588399,-0.127486,-0.088260,2.834122,2.451662,6.648908,8.433719,-6.423355,0.627626,4.903325,7.796287,2.049590,-4.275699,5.442691,-2.579149,1.843650,3.363681,-1.667131,-4.412992,2.696829,6.256643,0.794339,-0.725692,5.834957,1.902490,-1.657324,-0.117680,6.325289,-1.333704,4.010920,1.363124,-3.275421,-1.176798,-2.137850,0.480526,1.451384,2.284949,-2.657602,0.735499,6.168383,-1.716164,5.403464,0.215746,0.715885,-4.540479,0.382459,-1.961330,1.961330,2.520309,0.460913,-7.482474,-4.393379,1.166991,2.412436,2.579149,3.854014,-0.490333,-1.539644,-3.589234,-1.216025,-1.922103,7.433441,6.354709,1.157185,-4.971972,-1.245445,-0.382459,1.343511,-0.323619,1.873070,4.432606,2.559536,-4.491446,-5.177911,-0.529559,1.608291,-3.030255,-2.049590,-4.020726,2.147656,3.657880,1.157185,0.666852,-2.128043,-1.186605,-1.814230,-5.560370,-1.971137,-2.432049,0.441299,-0.843372,-5.423077,-1.078731,-3.706914,-2.137850,-2.118236,0.568786,-2.559536,-3.010642,-6.423355,-3.814787,-1.667131,2.981222,2.500696,8.561206,6.060509,3.187161,-1.402351,-3.520587,-0.549172,-1.578871,-2.510503,2.029977,-3.451941,1.088538,1.186605,-0.068647,6.442969,8.571012,3.942273,-5.040618,6.060509,-3.236195,0.470719,1.559257,0.558979,0.509946,-3.461747,5.540757,3.804980,1.892683,-3.608847,8.463139,4.265893,6.952915,4.648352,-2.834122,-2.530116,1.470998,-0.009807,1.176798,-0.392266,-3.520587,1.569064,-6.845042,-0.264780,-0.804145,-5.177911,-1.814230,4.824872,-0.764919,0.529559,-1.980943,-3.432327,4.658159,-0.343233,0.627626,-11.238421,-3.187161,-4.962165,0.480526,5.177911,-1.029698,-6.080123,-1.127765,2.510503,10.434276,10.434276,-5.540757,-2.039783,2.255530,-8.492559,-2.951802,3.491167,-6.511615,2.598762,1.549451,4.354153,-6.658716,-2.539922,-2.804702,-0.921825,-1.490611,-0.009807,-2.785089,-5.285784,-5.864377,-3.010642,2.079010,0.441299,0.696272,-3.216581,-5.344624,-5.982057,-0.872792,3.069481,-0.843372,1.470998,-3.530394,-0.009807,-1.627904,1.784810,-0.460913,3.255808,-1.725970,0.941438,-0.823759,-3.216581,1.274864,2.794895,-0.098066,-3.373488,0.862985,0.774725,0.961052,1.117958,-0.921825,-2.716442,-0.784532,-1.794617,0.941438,0.813952,-2.628182,-2.412436,-3.451941,-2.088816,0.196133,-3.628461,-1.912297,3.912853,0.127486,-1.853457,2.157463,0.539366,-4.442413,3.226388,3.226388,1.451384,4.697385,8.796565,2.412436,-2.157463,-0.411879,-1.765197,-0.156906,-0.049033,0.137293,1.216025,-2.010363,0.441299,0.509946,0.853179,-6.148769,-2.706635,5.893797,-0.117680,2.284949,-1.265058,4.118793,1.843650,-0.156906,-0.951245,1.559257,-4.363959,-0.902212,-1.245445,5.383851,-0.941438,3.530394,4.403186,6.707749,-1.696550,-2.804702,2.098623,5.011198,7.247115,2.549729,-1.000278,4.118793,3.628461,-0.980665,1.520031,3.000835,1.068925,3.648074,-4.403186,1.873070,2.696829,6.305676,1.686744,-2.432049,0.431493,5.491724,3.138128,-1.971137,-1.814230,-0.823759,0.725692,1.441578,-0.519752,0.343233,1.049312,0.608012,0.902212,0.500139,-0.990472,-1.166991,0.009807,0.578592,-0.617819,-0.872792,-0.872792,-0.460913,-0.225553,-0.284393,-0.588399,-0.235360,-0.019613,0.000000,0.000000,-0.009807,-0.147100,0.000000,0.049033,-0.323619,-0.745305,-0.627626,-0.166713,-0.460913,-0.304006,-0.313813,-0.784532,-1.049312,-1.343511,-0.735499,-1.353318,-1.725970,-2.245723,-2.167270,0.264780,-1.461191,-0.343233,-0.215746,-0.647239,-0.509946,-1.157185,1.755390,2.588956,0.666852,1.470998,1.019892,1.206218,0.019613,0.245166,-1.441578,0.696272,-0.392266,-0.676659,-0.931632,-0.147100,-1.098345,-0.068647,0.431493,0.372653,-0.009807,-0.274586,-1.019892,-0.019613,-0.617819,-0.568786,0.117680,0.039227,-1.117958,0.176520,0.274586,-0.058840,-0.274586,-0.833565,1.068925,0.686466,-0.353039,-0.598206,-0.598206,0.431493,1.500417,2.883155,0.735499,1.784810,0.657046,0.029420,-0.294199,-0.706079,0.029420,-1.470998,-0.313813,0.784532,0.951245,0.892405,0.715885,-0.323619,-0.990472,-1.892683,1.598484,0.833565,-1.049312,-1.686744,-0.755112,0.961052,0.647239,-0.588399,-0.313813,1.559257,0.706079,-1.529837,1.147378,0.921825,-1.029698,-0.127486,0.774725,4.462026,1.696550,-1.500417,-3.196968,-2.843928,-1.451384,1.186605,1.019892,2.961608,1.627904,-1.059118,2.539922,7.423634,6.619489,1.382738,-1.019892,-0.666852,-4.942552,-0.088260,0.980665,-1.588677,-0.127486,2.000557,2.304563,3.520587,5.403464,6.541036,1.794617,-5.825150,-4.226666,-4.442413,-4.462026,-2.186883,-3.206774,2.235916,-2.265336,-2.510503,-0.804145,3.520587,2.912575,7.227501,0.657046,0.735499,4.344346,-0.353039,-2.745862,-2.745862,1.402351,-0.431493,4.491446,1.882877,2.196690,-0.470719,2.324176,2.245723,0.970858,1.049312,-0.657046,0.470719,-1.039505,1.765197,-1.137571,-2.902768,-4.422799,-2.853735,0.990472,-3.461747,-2.520309,-2.432049,0.009807,-1.157185,-0.755112,-0.549172,-0.735499,-1.578871,-3.422521,-3.746140,-1.951523,0.009807,2.363403,1.206218,-0.362846,-7.904160,-3.196968,1.245445,-3.285228,3.442134,3.814787,0}; + double[] phi = {-0.000524,-0.000524,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000698,-0.000873,-0.001222,-0.002094,-0.002967,-0.005760,-0.009425,-0.009425,-0.014835,-0.018326,-0.023038,-0.035605,-0.043459,-0.052534,-0.073478,-0.084823,-0.096517,-0.119730,-0.130551,-0.141372,-0.151844,-0.172613,-0.183609,-0.207345,-0.219737,-0.232827,-0.260403,-0.274017,-0.287281,-0.312763,-0.324806,-0.336499,-0.360061,-0.371930,-0.385020,-0.414167,-0.430922,-0.449073,-0.488518,-0.509636,-0.531453,-0.575086,-0.596379,-0.617323,-0.655895,-0.672824,-0.688707,-0.718727,-0.733736,-0.749095,-0.779988,-0.794997,-0.809135,-0.822050,-0.844739,-0.854513,-0.863589,-0.871617,-0.884882,-0.889943,-0.893434,-0.895703,-0.894307,-0.891689,-0.885580,-0.881740,-0.868825,-0.859749,-0.849801,-0.827635,-0.815767,-0.803201,-0.776846,-0.761487,-0.743336,-0.703891,-0.682773,-0.660433,-0.613658,-0.587478,-0.560425,-0.505622,-0.480140,-0.455531,-0.407360,-0.383100,-0.359189,-0.313461,-0.291295,-0.268781,-0.223402,-0.202458,-0.146957,-0.115366,-0.086219,-0.072780,-0.060388,-0.039095,-0.029845,-0.021118,-0.005061,0.002094,0.008029,0.017802,0.021817,0.026704,0.037525,0.043110,0.047822,0.051836,0.059516,0.062657,0.068766,0.071559,0.073653,0.078191,0.080634,0.082554,0.087965,0.091804,0.097040,0.113272,0.123918,0.136659,0.161443,0.172962,0.185179,0.215199,0.231954,0.249582,0.268432,0.308400,0.329169,0.350288,0.395841,0.420450,0.445583,0.523424,0.549255,0.600393,0.625701,0.651008,0.701448,0.726232,0.798663,0.823097,0.871792,0.896750,0.921359,0.945270,0.989602,1.009149,1.027650,1.063604,1.081057,1.099557,1.139700,1.160644,1.182461,1.226792,1.249132,1.271996,1.318422,1.341286,1.363975,1.410575,1.435533,1.459793,1.505870,1.527861,1.549852,1.590868,1.611637,1.632232,1.672374,1.692446,1.711993,1.750914,1.771335,1.792453,1.835388,1.857903,1.880418,1.924924,1.947613,1.970651,2.016204,2.038021,2.060012,2.104518,2.126858,2.149198,2.191610,2.213078,2.235069,2.279400,2.301217,2.322684,2.345024,2.389007,2.410474,2.431767,2.475226,2.497217,2.518685,2.560224,2.581342,2.603158,2.668783,2.668783,2.691821,2.741214,2.767568,2.795319,2.852391,2.881015,2.909987,2.966187,2.993938,3.020990,3.074921,3.101974,3.129201,-3.099705,-3.073001,-3.023434,-2.999697,-2.975612,-2.927092,-2.903704,-2.883109,-2.850472,-2.837207,-2.825688,-2.816089,-2.804220,-2.801603,-2.801603,-2.803871,-2.808933,-2.816438,-2.837905,-2.851344,-2.866529,-2.901261,-2.921507,-2.942974,-2.989051,-3.014358,-3.070034,-3.124837,3.132517,3.083648,3.016627,2.995858,2.958682,2.944545,2.933724,2.920809,2.917492,2.914874,2.910860,2.909464,2.909289,2.911907,2.913653,2.915398,2.920110,2.923950,2.928837,2.940007,2.946116,2.952573,2.966885,2.975437,2.984862,3.004933,3.015056,3.024656,3.043505,3.052581,3.061482,3.079110,3.087836,3.096738,3.115762,3.125885,3.137229,-3.120474,-3.107035,-3.094818,-3.077190,-3.068463,-3.059388,-3.041062,-3.031637,-3.021514,-2.998301,-2.985735,-2.972994,-2.959380,-2.929361,-2.913304,-2.896549,-2.860944,-2.841396,-2.819929,-2.777517,-2.757446,-2.737025,-2.696534,-2.676986,-2.658137,-2.619565,-2.599668,-2.579597,-2.541898,-2.523397,-2.504547,-2.464580,-2.443810,-2.402271,-2.363874,-2.346072,-2.311165,-2.293712,-2.276084,-2.240479,-2.221979,-2.203478,-2.165779,-2.146057,-2.126509,-2.087065,-2.067517,-2.048144,-2.008525,-1.988105,-1.967510,-1.927891,-1.908169,-1.888446,-1.849700,-1.830501,-1.811303,-1.772382,-1.752834,-1.733985,-1.717404,-1.687559,-1.673945,-1.661903,-1.641831,-1.633454,-1.625076,-1.613033,-1.609543,-1.606925,-1.602910,-1.601863,-1.600641,-1.597500,-1.596453,-1.595056,-1.590169,-1.588075,-1.586155,-1.581617,-1.579348,-1.576905,-1.569051,-1.564339,-1.551423,-1.543744,-1.532748,-1.518786,-1.484228,-1.464331,-1.444260,-1.406212,-1.386315,-1.345300,-1.323309,-1.300445,-1.277407,-1.230283,-1.207244,-1.184555,-1.139525,-1.117011,-1.094496,-1.049641,-1.027301,-1.004786,-0.959757,-0.937591,-0.916123,-0.874934,-0.855037,-0.835315,-0.797092,-0.779290,-0.762534,-0.730944,-0.716109,-0.701971,-0.674744,-0.660258,-0.645074,-0.614007,-0.597601,-0.581020,-0.547161,-0.529533,-0.512254,-0.479616,-0.463909,-0.448026,-0.432318,-0.400204,-0.383798,-0.367741,-0.337547,-0.322188,-0.306654,-0.275762,-0.260403,-0.230733,-0.216421,-0.202807,-0.174184,-0.159698,-0.145386,-0.119904,-0.108210,-0.097564,-0.087965,-0.070686,-0.062832,-0.048695,-0.042237,-0.026529,-0.021991,-0.017453,-0.008203,-0.004712,-0.001571,0.005236,0.008901,0.014661,0.017104,0.019548,0.024609,0.027227,0.029671,0.035779,0.039095,0.043284,0.053233,0.058469,0.063705,0.073827,0.080111,0.087790,0.106640,0.116937,0.128107,0.152891,0.166504,0.180642,0.208218,0.222529,0.266337,0.281871,0.297579,0.329344,0.345575,0.379086,0.396190,0.414516,0.453611,0.473159,0.492183,0.528660,0.547510,0.566883,0.604757,0.623257,0.641932,0.676315,0.691849,0.706684,0.734085,0.746303,0.757647,0.778242,0.787667,0.797092,0.815592,0.824843,0.833220,0.848055,0.853990,0.860098,0.864985,0.874235,0.878250,0.885755,0.889245,0.892212,0.898495,0.901812,0.905128,0.912109,0.916298,0.921010,0.926072,0.937242,0.944572,0.953648,0.974243,0.983144,0.993092,1.017701,1.032711,1.049641,1.088736,1.110378,1.158026,1.183333,1.208466,1.233599,1.280548,1.304110,1.328370,1.376890,1.400976,1.425410,1.470440,1.470440,1.512851,1.535541,1.558928,1.605354,1.628567,1.651954,1.699951,1.724211,1.748994,1.799609,1.825789,1.852318,1.904678,1.931207,1.958259,2.013063,2.039766,2.066121,2.117084,2.141868,2.191785,2.217266,2.242748,2.292665,2.315528,2.337345,2.381502,2.404017,2.427055,2.472957,2.495646,2.517812,2.561445,2.583785,2.606300,2.653424,2.677510,2.701944,2.751861,2.776470,2.801253,2.852566,2.878921,2.933549,2.961126,2.987654,3.041760,3.069860,3.098134,-3.129026,-3.102846,-3.077539,-3.028146,-3.002664,-2.976659,-2.926394,-2.903181,-2.881015,-2.837731,-2.815565,-2.793574,-2.751511,-2.733535,-2.718350,-2.695137,-2.686585,-2.679778,-2.670005,-2.670005,-2.668434,-2.673321,-2.679953,-2.688854,-2.713638,-2.728822,-2.765125,-2.785022,-2.805617,-2.845585,-2.864783,-2.885204,-2.927266,-2.948908,-2.970725,-3.018722,-3.044727,-3.072652,-3.131819,3.120300,3.088535,3.023608,2.958333,2.889218,2.855010,2.820976,2.753606,2.719747,2.684840,2.613980,2.578375,2.542421,2.468419,2.430545,2.353227,2.313085,2.272942,2.191785,2.151467,2.110627,2.028596,1.987581,1.947438,1.872913,1.839053,1.806241,1.742537,1.715484,1.689653,1.636421,1.609717,1.583363,1.532399,1.508139,1.483704,1.437104,1.415287,1.395391,1.361008,1.346172,1.332210,1.307775,1.297303,1.280548,1.273392,1.266411,1.254019,1.249132,1.245118,1.240580,1.239882,1.240056,1.242849,1.244245,1.244245,1.246689,1.247387,1.248085,1.249307,1.249830,1.250005,1.250179,1.250354,1.250528,1.250877,1.251052,1.251052,1.251576,1.251576,1.250528,1.249481,1.248434,1.245642,1.244245,1.242849,1.239009,1.236566,1.233773,1.226967,1.222778,1.217716,1.205673,1.198867,1.191711,1.178272,1.171465,1.164658,1.150696,1.143540,1.136384,1.122945,1.117883,1.113695,1.107062,1.104095,1.099383,1.097114,1.094321,1.087340,1.083326,1.078788,1.068665,1.063429,1.058019,1.047023,1.041263,1.035678,1.024334,1.018923,1.013687,1.003564,0.998677,0.993790,0.983842,0.978781,0.973719,0.963596,0.958535,0.953299,0.943001,0.937591,0.932180,0.920836,0.915600,0.910015,0.904953,0.894307,0.889420,0.885580,0.882962,0.878773,0.877028,0.875108,0.868825,0.864287,0.858877,0.846834,0.841074,0.835664,0.825541,0.821526,0.818559,0.814545,0.812800,0.811055,0.808960,0.808437,0.808262,0.809658,0.811229,0.814720,0.816814,0.819258,0.825192,0.828159,0.830777,0.836187,0.838980,0.842645,0.853641,0.860447,0.867254,0.881391,0.886802,0.891514,0.903208,0.910713,0.919090,0.938289,0.948412,0.958535,0.979828,0.991871,1.017876,1.031664,1.045976,1.078963,1.097812,1.118058,1.162040,1.184380,1.207593,1.258382,1.284911,1.311440,1.362928,1.388758,1.415113,1.466077,1.490162,1.514422,1.564164,1.589471,1.614255,1.664870,1.691573,1.718975,1.772731,1.798038,1.822298,1.846035,1.869597,1.893682,1.942901,1.968208,1.993690,2.068913,2.094395,2.094395,2.148326,2.176949,2.206096,2.263867,2.294235,2.357765,2.390927,2.425310,2.497915,2.535265,2.571394,2.639636,2.670528,2.700548,2.758144,2.785022,2.809980,2.856580,2.878571,2.900912,2.944021,2.962696,2.979975,3.012962,3.028670,3.043156,3.066020,3.073874,3.079633,3.086266,3.087138,3.086789,3.081553,3.077714,3.073350,3.062704,3.055722,3.046821,3.023259,3.008424,2.991669,0}; + + + Pose2d ref_pose; + Pose2d ekf_pose; + + for (int i =1; i 30 ) // invalidate the measurements + { + valid = false; + } + + filter.FilterRange(R + r.nextGaussian()*0.05 + V*time, time , valid); + } + + // The position must be correct despite no range data for 10 cycles + assertEquals(R+V*40*dt, filter.GetRange() , 0.2); + + assertEquals(V, filter.filter.model.state_estimate.data[1][0] , 0.15); + + } +} \ No newline at end of file From a0d83e00e339bdb1006b9a04a01af191cf9b9561 Mon Sep 17 00:00:00 2001 From: Adam Date: Sat, 22 Feb 2020 16:06:46 +0200 Subject: [PATCH 03/22] Fix error --- .../java/frc/robot/subsystems/drivetrain/Drivetrain.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index d3c2632f..843ef76e 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -33,6 +33,7 @@ import static frc.robot.Constants.Drivetrain.*; import static frc.robot.Constants.EFFECTIVE_TURN_WIDTH; +import static frc.robot.Constants.FieldGeometry.FIELD_WIDTH; import static frc.robot.Ports.Drivetrain.LEFT_MASTER_INVERTED; import static frc.robot.Ports.Drivetrain.LEFT_SLAVE_INVERTED; import static frc.robot.Ports.Drivetrain.RIGHT_MASTER_INVERTED; @@ -45,7 +46,7 @@ public class Drivetrain extends SubsystemBase { private final TalonFX leftSlave = new TalonFX(LEFT_SLAVE); private final TalonFX rightMaster = new TalonFX(RIGHT_MASTER); private final TalonFX rightSlave = new TalonFX(RIGHT_SLAVE); - private DifferentialDriveOdometry odometry; + private DifferentialDriveOdometry odometry = new DifferentialDriveOdometry(new Rotation2d(0)); private UnitModel lowGearUnitModel = new UnitModel(LOW_TICKS_PER_METER); private UnitModel highGearUnitModel = new UnitModel(HIGH_TICKS_PER_METER); public static final FullLocalization localization = new FullLocalization(new Rotation2d(0), EFFECTIVE_TURN_WIDTH); @@ -98,7 +99,8 @@ public Drivetrain() { gearShifterB = new Solenoid(SHIFTER_PORT); navx.reset(); - Pose2d INITIAL_POSE = new Pose2d(UtilityFunctions.getAlliancePort(false).getTranslation().getX() - 10, UtilityFunctions.getAlliancePort(false).getTranslation().getY(), new Rotation2d()); + // Pose2d INITIAL_POSE = new Pose2d(UtilityFunctions.getAlliancePort(false).getTranslation().getX() - 10, UtilityFunctions.getAlliancePort(false).getTranslation().getY(), new Rotation2d()); + Pose2d INITIAL_POSE = new Pose2d(15.98 - 3.2, 8.23 - 2.42, new Rotation2d()); localization.resetPosition(INITIAL_POSE, new Rotation2d(Math.toRadians(navx.getAngle())), Robot.robotTimer.get()); odometry.resetPosition(INITIAL_POSE, Rotation2d.fromDegrees(getHeading())); } From 07fb533889cfef1cb280fabaf10f820d8cff6551 Mon Sep 17 00:00:00 2001 From: Adam Date: Sat, 22 Feb 2020 16:08:53 +0200 Subject: [PATCH 04/22] Create flipCoordSystem method The coordinate system of the falcon dashboard is flipped to our definition --- .../frc/robot/subsystems/drivetrain/Drivetrain.java | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 843ef76e..b8bf0b74 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -312,4 +312,14 @@ public enum shiftModes { HIGH, LOW } + + /** + * Flips coordinate definition from Falcon (0,0) bottom left to Galaxia (0,0) top left and the reverse + * @param pose + * @return + */ + public Pose2d flipCoordSystem(Pose2d pose ) + { + return new Pose2d(pose.getTranslation().getX(),FIELD_WIDTH - pose.getTranslation().getY(), new Rotation2d(- pose.getRotation().getRadians()) ); + } } From 42ce200e4088848ded1849789f91b46754f47291 Mon Sep 17 00:00:00 2001 From: Adam Date: Sat, 22 Feb 2020 16:09:35 +0200 Subject: [PATCH 05/22] Utilize flipCoordSystem in setPose and getPose methods --- .../frc/robot/subsystems/drivetrain/Drivetrain.java | 13 ++++++++----- .../turret/commands/PoseVisionTurret.java | 2 +- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index b8bf0b74..5f0b6be9 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -247,10 +247,12 @@ public double getCCWHeading() { } public Pose2d getPose() { - return localization.getPoseMeters(); + return flipCoordSystem( localization.getPoseMeters()); } - public void setPose(Pose2d pose, Rotation2d rotation) { + public void setPose(Pose2d pose) { + pose = flipCoordSystem(pose); + Rotation2d rotation = new Rotation2d(Math.toRadians(navx.getAngle())); leftMaster.setSelectedSensorPosition(0); rightMaster.setSelectedSensorPosition(0); navx.reset(); @@ -299,9 +301,10 @@ public void periodic() { // This method will be called once per scheduler run SmartDashboard.putNumber(" simple angle", odometry.getPoseMeters().getRotation().getRadians()); SmartDashboard.putNumber("navx accel", navx.getWorldLinearAccelY() * GRAVITY_ACCELERATION); - FalconDashboard.INSTANCE.setRobotX(current.getTranslation().getX()); - FalconDashboard.INSTANCE.setRobotY(current.getTranslation().getY()); - FalconDashboard.INSTANCE.setRobotHeading(Math.toRadians(navx.getAngle() * (GYRO_INVERTED ? -1 : 1))); + Pose2d falcon_pose = getPose(); + FalconDashboard.INSTANCE.setRobotX(Units.metersToFeet(falcon_pose.getTranslation().getX())); + FalconDashboard.INSTANCE.setRobotY(Units.metersToFeet(falcon_pose.getTranslation().getY())); + FalconDashboard.INSTANCE.setRobotHeading(Math.toRadians(falcon_pose.getRotation().getRadians())); } /** diff --git a/src/main/java/frc/robot/subsystems/turret/commands/PoseVisionTurret.java b/src/main/java/frc/robot/subsystems/turret/commands/PoseVisionTurret.java index d4230b33..14e9b3fd 100644 --- a/src/main/java/frc/robot/subsystems/turret/commands/PoseVisionTurret.java +++ b/src/main/java/frc/robot/subsystems/turret/commands/PoseVisionTurret.java @@ -29,7 +29,7 @@ public void execute() { Pose2d robotPose = VisionModule.getRobotPose(); if(robotPose == null || turret.inDeadZone()) return; turret.setAngle(Utils.calculateTurretAngle(robotPose, true)); - drivetrain.setPose(robotPose, new Rotation2d(Math.toRadians(navx.getAngle()))); + drivetrain.setPose(robotPose); } @Override From b2bf60b65554921cfb4fcdffe380932e3091ccb7 Mon Sep 17 00:00:00 2001 From: Adam Date: Sat, 22 Feb 2020 16:10:07 +0200 Subject: [PATCH 06/22] Create FIELD_WIDTH constant --- src/main/java/frc/robot/Constants.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f938bf2e..1f3a7c96 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -75,6 +75,7 @@ public static class Vision { } public static class FieldGeometry { + public static final double FIELD_WIDTH = 8.2296 ; 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 RED_INNER_POWER_PORT_LOCATION = new Pose2d(15.98 + 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). From d8eebf11c94a3d7919caf6b84633b0e1dc0df069 Mon Sep 17 00:00:00 2001 From: Adam Date: Mon, 2 Mar 2020 11:19:03 +0200 Subject: [PATCH 07/22] Fix conflicts --- src/main/java/frc/robot/Robot.java | 3 ++- src/main/java/frc/robot/RobotContainer.java | 4 +++- .../frc/robot/subsystems/turret/commands/TurretSwitching.java | 2 +- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 755a0185..a2bca4dd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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; @@ -211,7 +212,7 @@ public void teleopInit() { } m_robotContainer.drivetrain.setBrake(false); } - } + /** * This function is called periodically during operator control. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4a9d7953..4a60dbfc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ 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; @@ -51,6 +52,7 @@ 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 @@ -66,7 +68,7 @@ public class RobotContainer { private final CustomDashboard customDashboard = new CustomDashboard(); public static final Drivetrain drivetrain = new Drivetrain(); private final ColorWheel colorWheel = new ColorWheel(); - private final Shooter shooter = new Shooter(); + public final Shooter shooter = new Shooter(); private final Intake intake = new Intake(); private final Conveyor conveyor = new Conveyor(intake); public static final Climber climber = new Climber(); diff --git a/src/main/java/frc/robot/subsystems/turret/commands/TurretSwitching.java b/src/main/java/frc/robot/subsystems/turret/commands/TurretSwitching.java index 587ba5b8..57c0c50c 100644 --- a/src/main/java/frc/robot/subsystems/turret/commands/TurretSwitching.java +++ b/src/main/java/frc/robot/subsystems/turret/commands/TurretSwitching.java @@ -14,7 +14,7 @@ public class TurretSwitching extends CommandBase { private final Timer timer = new Timer(); private static final double VISION_TIMEOUT = 0.3; - public TurretSwitching(Turret turret, FullLocalization localization, Drivetrain drivetrain) { + public TurretSwitching(Turret turret, Drivetrain drivetrain) { this.turret = turret; this.localizationTurret = new LocalizationTurret(turret, drivetrain); this.visionTurret = new VisionTurret(turret); From 6042f156daa85aa88c12f69bffb5d17ddbab0644 Mon Sep 17 00:00:00 2001 From: Adam Date: Mon, 2 Mar 2020 11:20:10 +0200 Subject: [PATCH 08/22] Fix error --- src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 241586e6..88dcd61b 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -311,7 +311,7 @@ public void periodic() { // This method will be called once per scheduler run Pose2d falcon_pose = getPose(); FalconDashboard.INSTANCE.setRobotX(Units.metersToFeet(falcon_pose.getTranslation().getX())); FalconDashboard.INSTANCE.setRobotY(Units.metersToFeet(falcon_pose.getTranslation().getY())); - FalconDashboard.INSTANCE.setRobotHeading(Math.toRadians(falcon_pose.getRotation().getRadians())); + FalconDashboard.INSTANCE.setRobotHeading(falcon_pose.getRotation().getRadians()); } /** From 472709defa8d5e761139db85263dc28633acc467 Mon Sep 17 00:00:00 2001 From: Adam Date: Mon, 2 Mar 2020 11:21:52 +0200 Subject: [PATCH 09/22] Change covariance --- .../KalmanLocalization/OdometryInertialProcess.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java index e551cd49..420fa756 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java @@ -92,8 +92,8 @@ public void processNoiseCovariance(double[][] cov) { cov[1][1] = 0; // Assume the position is not changing by itself - use a very small covariance cov[2][2] = 1e-4; // Allow change in velocity can - change by measurements cov[3][3] = 1e-9; // assume phi is not changing - cov[4][4] = 1e1; // Allow change in omega - cov[5][5] = 1e-1; // Allow change in bias + cov[4][4] = 1e-2; // Allow change in omega + cov[5][5] = 1e1; // Allow change in bias cov[6][6] = 1e-6; // Allow change in bias } From e93dd50039d838796e1a34a7770a5a0e0ccfbc13 Mon Sep 17 00:00:00 2001 From: Adam Date: Mon, 2 Mar 2020 11:37:01 +0200 Subject: [PATCH 10/22] Create VisionValid method --- .../drivetrain/FullLocalization.java | 28 +++++++++++++++---- 1 file changed, 23 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java b/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java index e947e1e0..a9372a82 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; +import frc.robot.UtilityFunctions; import frc.robot.subsystems.drivetrain.EKF.KalmanFilter; import frc.robot.subsystems.drivetrain.KalmanLocalization.OdometryInertialObservation; import frc.robot.subsystems.drivetrain.KalmanLocalization.OdometryInertialProcess; @@ -21,13 +22,11 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -import static frc.robot.Constants.Drivetrain.GYRO_INVERTED; -import static frc.robot.Constants.FieldGeometry.OUTER_POWER_PORT_LOCATION; +import static frc.robot.Constants.FieldGeometry.RED_OUTER_POWER_PORT_LOCATION; import static frc.robot.RobotContainer.drivetrain; import static frc.robot.RobotContainer.turret; import static frc.robot.RobotContainer.navx; import static java.lang.Math.abs; -import static java.lang.Math.min; /** * Class for localization using differntial drive odometry and inertial sensors. Odometry allows you to track the @@ -173,7 +172,7 @@ public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters, double target_angle = turret.getAngle() + visionAngle.getDouble(0); double target_range = visionDistance.getDouble(1); - final Pose2d target_POS = OUTER_POWER_PORT_LOCATION; + final Pose2d target_POS = UtilityFunctions.getPortLocation(false); double dt = Math.max(0.02,time - m_prev_time); // Observation object holds the new measurements @@ -185,7 +184,7 @@ public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters, // Check if encoders are valid or slipping: observation.setEncoderValid(EncoderValid(gyroAngle, deltaLeftDistance, deltaRightDistance)); - observation.setTargetValid(visionValid.getBoolean(false)); + observation.setTargetValid(VisionValid()); m_previousAngle = gyroAngle; @@ -224,6 +223,25 @@ public void setPose(Pose2d pose, Rotation2d rotation) { resetPosition(pose, rotation, Robot.robotTimer.get()); } + public boolean VisionValid() + { + + if (!visionValid.getBoolean(false)) + { + return false; + } + + if (visionDistance.getDouble(0) < 0.1 ) + { + return false; + } + + if ( Math.abs(visionAngle.getDouble(180)) > 10 ) // Ignore when large tracking errors + { + return false; + } + return true; + } // Detect encoder slipping condition by comparing gyro and encoder readings. public boolean EncoderValid(Rotation2d gyroAngle, double deltaLeftDistance, From ad7f80c6d36d5b2560e2451733bdb6b3c8293dfe Mon Sep 17 00:00:00 2001 From: Adam Date: Mon, 2 Mar 2020 11:37:20 +0200 Subject: [PATCH 11/22] Change initial pose --- src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 88dcd61b..c98fe321 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -99,7 +99,7 @@ public Drivetrain() { navx.reset(); // Pose2d INITIAL_POSE = new Pose2d(UtilityFunctions.getAlliancePort(false).getTranslation().getX() - 10, UtilityFunctions.getAlliancePort(false).getTranslation().getY(), new Rotation2d()); - Pose2d INITIAL_POSE = new Pose2d(15.98 - 3.2, 8.23 - 2.42, new Rotation2d()); + Pose2d INITIAL_POSE = new Pose2d(15.98 - 3.2, 8.23 - 2.42, new Rotation2d(Math.PI)); localization.resetPosition(INITIAL_POSE, new Rotation2d(Math.toRadians(navx.getAngle())), Robot.robotTimer.get()); odometry.resetPosition(INITIAL_POSE, Rotation2d.fromDegrees(getHeading())); } From b91f567c51f395757ffd046e8208f13fde578609 Mon Sep 17 00:00:00 2001 From: Adam Date: Mon, 2 Mar 2020 15:12:31 +0200 Subject: [PATCH 12/22] Fix outer port position constant --- src/main/java/frc/robot/Constants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 48c44ac8..1531aad5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -81,8 +81,8 @@ public static class Vision { public static class FieldGeometry { public static final double FIELD_WIDTH = 8.2296 ; - 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 RED_INNER_POWER_PORT_LOCATION = new Pose2d(15.98 + 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 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 @@ -272,7 +272,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); } From 450d2061e9ababf78831ab2ae8a0e7361c8bbaaf Mon Sep 17 00:00:00 2001 From: Adam Date: Mon, 2 Mar 2020 15:12:55 +0200 Subject: [PATCH 13/22] Move flip coordinates to Utils --- .../frc/robot/subsystems/drivetrain/Drivetrain.java | 11 ++--------- src/main/java/frc/robot/utilities/Utils.java | 11 ++++++++++- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index c98fe321..54133fd4 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -40,6 +40,7 @@ import static frc.robot.Ports.Drivetrain.RIGHT_SLAVE_INVERTED; import static frc.robot.Ports.Drivetrain.*; import static frc.robot.RobotContainer.navx; +import static frc.robot.utilities.Utils.flipCoordSystem; public class Drivetrain extends SubsystemBase { private final TalonFX leftMaster = new TalonFX(LEFT_MASTER); @@ -323,13 +324,5 @@ public enum shiftModes { LOW } - /** - * Flips coordinate definition from Falcon (0,0) bottom left to Galaxia (0,0) top left and the reverse - * @param pose - * @return - */ - public Pose2d flipCoordSystem(Pose2d pose ) - { - return new Pose2d(pose.getTranslation().getX(),FIELD_WIDTH - pose.getTranslation().getY(), new Rotation2d(- pose.getRotation().getRadians()) ); - } + } diff --git a/src/main/java/frc/robot/utilities/Utils.java b/src/main/java/frc/robot/utilities/Utils.java index 2ad3e3d1..9cfc13f3 100644 --- a/src/main/java/frc/robot/utilities/Utils.java +++ b/src/main/java/frc/robot/utilities/Utils.java @@ -16,6 +16,7 @@ import java.util.Arrays; import java.util.Optional; +import static frc.robot.Constants.FieldGeometry.FIELD_WIDTH; import static frc.robot.Constants.FieldGeometry.RED_OUTER_POWER_PORT_LOCATION; public class Utils { @@ -97,7 +98,15 @@ public static double calculateTurretAngle(Pose2d currentPosition, boolean innerP if (angle < 0) angle += 360; return angle; } - + /** + * Flips coordinate definition from Falcon (0,0) bottom left to Galaxia (0,0) top left and the reverse + * @param pose + * @return + */ + public static Pose2d flipCoordSystem(Pose2d pose ) + { + return new Pose2d(pose.getTranslation().getX(),FIELD_WIDTH - pose.getTranslation().getY(), new Rotation2d(- pose.getRotation().getRadians()) ); + } /** * Replaces fields between constants classes. * From c0f2b62ed30135727bf8a390dfb09d1dde079d71 Mon Sep 17 00:00:00 2001 From: Adam Date: Mon, 2 Mar 2020 15:13:26 +0200 Subject: [PATCH 14/22] Create GetExpectedRange method --- .../KalmanLocalization/OdometryInertialObservation.java | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialObservation.java b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialObservation.java index e3aa4c63..09068faa 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialObservation.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialObservation.java @@ -21,6 +21,8 @@ public class OdometryInertialObservation extends ObservationModel { private Pose2d m_TargetPose; private boolean m_targetValid; + private double m_ExpectedRangeSqr; + public void setEncoderValid(boolean valid){ m_encoderValid = valid; } @@ -94,8 +96,13 @@ public void observationModel(double[][] x, double[][] h) { h[3][0] = dx*dx + dy*dy; h[4][0] = Math.IEEEremainder( Math.atan2(dy,dx) - phi,2*Math.PI); + m_ExpectedRangeSqr = h[3][0]; } + public double GetExpectedRange() + { + return Math.sqrt(m_ExpectedRangeSqr); + } @Override public void observationModelJacobian(double[][] x, double[][] j) { double X = x[0][0]; @@ -143,7 +150,7 @@ public void observationNoiseCovariance(double[][] cov) { { // TODO: tune vision and turret errors cov[3][3] = 1 ; // 10 cm range error for 5 m range (5+0.1)^2 = 25 + 2*0.5 + 0.01; => std = 1 => cov = 1 - cov[4][4] = 1e-4 ; // about 0.5 degrees + cov[4][4] = 1e6 ; // about 0.5 degrees 1e-4 } else { From facc4b5083e364106b1cf645f306430e01e2b5f1 Mon Sep 17 00:00:00 2001 From: Adam Date: Mon, 2 Mar 2020 15:13:49 +0200 Subject: [PATCH 15/22] Add new condition --- .../drivetrain/FullLocalization.java | 26 ++++++++++++++----- 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java b/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java index a9372a82..7740ca00 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java @@ -13,6 +13,7 @@ import frc.robot.subsystems.drivetrain.EKF.KalmanFilter; import frc.robot.subsystems.drivetrain.KalmanLocalization.OdometryInertialObservation; import frc.robot.subsystems.drivetrain.KalmanLocalization.OdometryInertialProcess; +import frc.robot.utilities.VisionModule; import org.ghrobotics.lib.debug.FalconDashboard; /*----------------------------------------------------------------------------*/ @@ -26,6 +27,7 @@ import static frc.robot.RobotContainer.drivetrain; import static frc.robot.RobotContainer.turret; import static frc.robot.RobotContainer.navx; +import static frc.robot.utilities.Utils.flipCoordSystem; import static java.lang.Math.abs; /** @@ -68,9 +70,8 @@ public class FullLocalization { private NetworkTableEntry encoderLeft = localizationTable.getEntry("left-encoder"); private NetworkTableEntry encoderRight = localizationTable.getEntry("right-encoder"); - private NetworkTable visionTable = NetworkTableInstance.getDefault().getTable("chameleon-vision").getSubTable("turret"); + private NetworkTable visionTable = NetworkTableInstance.getDefault().getTable("chameleon-vision").getSubTable("ps3"); private NetworkTableEntry visionAngle = visionTable.getEntry("targetYaw"); - private NetworkTableEntry visionDistance = visionTable.getEntry("distance"); private NetworkTableEntry visionValid = visionTable.getEntry("isValid"); @@ -170,9 +171,13 @@ public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters, var angle = new Rotation2d( gyroAngle.getRadians() + m_gyroOffset.getRadians()); double target_angle = turret.getAngle() + visionAngle.getDouble(0); - double target_range = visionDistance.getDouble(1); + double target_range; + if (VisionModule.getRobotDistance() == null) + target_range = 0; + else + target_range = VisionModule.getRobotDistance(); - final Pose2d target_POS = UtilityFunctions.getPortLocation(false); + final Pose2d target_POS = flipCoordSystem(UtilityFunctions.getPortLocation(false)); double dt = Math.max(0.02,time - m_prev_time); // Observation object holds the new measurements @@ -184,7 +189,12 @@ public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters, // Check if encoders are valid or slipping: observation.setEncoderValid(EncoderValid(gyroAngle, deltaLeftDistance, deltaRightDistance)); - observation.setTargetValid(VisionValid()); + boolean isVisionValid = VisionValid(); + observation.setTargetValid(isVisionValid); + SmartDashboard.putBoolean("valid", visionValid.getBoolean(false)); + SmartDashboard.putBoolean("vision-valid-local", isVisionValid); + SmartDashboard.putNumber("vision-angle-local", visionAngle.getDouble(0)); + SmartDashboard.putNumber("vision-range-local", target_range); m_previousAngle = gyroAngle; @@ -231,7 +241,7 @@ public boolean VisionValid() return false; } - if (visionDistance.getDouble(0) < 0.1 ) + if (VisionModule.getRobotDistance() < 0.1) { return false; } @@ -240,6 +250,10 @@ public boolean VisionValid() { return false; } + + if (Math.abs(VisionModule.getRobotDistance() - observation.GetExpectedRange()) > 1 ) { + return false; + } return true; } From bb20feead5fe401179ad894e04b625de4f7d3b2c Mon Sep 17 00:00:00 2001 From: Adam Date: Mon, 2 Mar 2020 15:14:01 +0200 Subject: [PATCH 16/22] Add reset for testing --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4a60dbfc..b0224c42 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -110,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) From 116d72cfb224194e6c960eb1952ba8f6e6a6b33a Mon Sep 17 00:00:00 2001 From: Adam Date: Mon, 2 Mar 2020 16:18:40 +0200 Subject: [PATCH 17/22] Add angle bias and covariances to dashboard --- .../frc/robot/subsystems/drivetrain/FullLocalization.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java b/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java index 7740ca00..d51b9a2a 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java @@ -67,6 +67,9 @@ public class FullLocalization { private NetworkTableEntry theta = localizationTable.getEntry("theta"); private NetworkTableEntry angularVelocity = localizationTable.getEntry("angular-velocity"); private NetworkTableEntry accelerationBias = localizationTable.getEntry("acceleration-bias"); + private NetworkTableEntry angleBias = localizationTable.getEntry("angle-bias"); + private NetworkTableEntry xCov = localizationTable.getEntry("x-std"); + private NetworkTableEntry yCov = localizationTable.getEntry("y-std"); private NetworkTableEntry encoderLeft = localizationTable.getEntry("left-encoder"); private NetworkTableEntry encoderRight = localizationTable.getEntry("right-encoder"); @@ -195,6 +198,7 @@ public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters, SmartDashboard.putBoolean("vision-valid-local", isVisionValid); SmartDashboard.putNumber("vision-angle-local", visionAngle.getDouble(0)); SmartDashboard.putNumber("vision-range-local", target_range); + SmartDashboard.putNumber("vision-angle-local", target_angle); m_previousAngle = gyroAngle; @@ -216,6 +220,10 @@ public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters, theta.setDouble(filter.model.state_estimate.data[3][0]); angularVelocity.setDouble(filter.model.state_estimate.data[4][0]); accelerationBias.setDouble(filter.model.state_estimate.data[5][0]); + angleBias.setDouble(filter.model.state_estimate.data[6][0]); + xCov.setDouble(Math.sqrt(filter.model.estimate_covariance.data[0][0])); + yCov.setDouble(Math.sqrt(filter.model.estimate_covariance.data[1][1])); + m_prev_time = time; return m_poseMeters; From 983805bb5444f647b77dc1b3102e97833cd7dba3 Mon Sep 17 00:00:00 2001 From: Adam Date: Mon, 2 Mar 2020 17:42:26 +0200 Subject: [PATCH 18/22] Add angle valid boolean --- .../subsystems/drivetrain/FullLocalization.java | 16 ++++++++++++---- .../OdometryInertialObservation.java | 11 +++++++++-- 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java b/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java index d51b9a2a..ab110bd4 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java @@ -166,14 +166,21 @@ public Pose2d getPoseMeters() { */ public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters, double acc, double time) { + boolean angleValid; double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance; double deltaRightDistance = rightDistanceMeters - m_prevRightDistance; m_prevLeftDistance = leftDistanceMeters; m_prevRightDistance = rightDistanceMeters; - var angle = new Rotation2d( gyroAngle.getRadians() + m_gyroOffset.getRadians()); + var angle = new Rotation2d(gyroAngle.getRadians() + m_gyroOffset.getRadians()); double target_angle = turret.getAngle() + visionAngle.getDouble(0); + target_angle = Math.IEEEremainder(target_angle, 360); + if (abs(target_angle) > 170) { + angleValid = false; + } else { + angleValid = true; + } double target_range; if (VisionModule.getRobotDistance() == null) target_range = 0; @@ -193,7 +200,7 @@ public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters, observation.setEncoderValid(EncoderValid(gyroAngle, deltaLeftDistance, deltaRightDistance)); boolean isVisionValid = VisionValid(); - observation.setTargetValid(isVisionValid); + observation.setTargetValid(angleValid, isVisionValid); SmartDashboard.putBoolean("valid", visionValid.getBoolean(false)); SmartDashboard.putBoolean("vision-valid-local", isVisionValid); SmartDashboard.putNumber("vision-angle-local", visionAngle.getDouble(0)); @@ -248,13 +255,13 @@ public boolean VisionValid() { return false; } - + if (VisionModule.getRobotDistance() == null) return false; if (VisionModule.getRobotDistance() < 0.1) { return false; } - if ( Math.abs(visionAngle.getDouble(180)) > 10 ) // Ignore when large tracking errors + if ( Math.abs(visionAngle.getDouble(180)) > 2 ) // Ignore when large tracking errors { return false; } @@ -262,6 +269,7 @@ public boolean VisionValid() if (Math.abs(VisionModule.getRobotDistance() - observation.GetExpectedRange()) > 1 ) { return false; } + return true; } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialObservation.java b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialObservation.java index 09068faa..c11ed11e 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialObservation.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialObservation.java @@ -20,6 +20,7 @@ public class OdometryInertialObservation extends ObservationModel { private double m_TargetAngle; private Pose2d m_TargetPose; private boolean m_targetValid; + private boolean m_angleValid; private double m_ExpectedRangeSqr; @@ -27,7 +28,10 @@ public void setEncoderValid(boolean valid){ m_encoderValid = valid; } - public void setTargetValid(boolean valid) { m_targetValid = valid;} + public void setTargetValid(boolean angle_valid, boolean valid) { + m_targetValid = valid; + m_angleValid = angle_valid; + } // Constructor must receive the geometry of the robot public OdometryInertialObservation(double Rr, double Rl) @@ -150,7 +154,10 @@ public void observationNoiseCovariance(double[][] cov) { { // TODO: tune vision and turret errors cov[3][3] = 1 ; // 10 cm range error for 5 m range (5+0.1)^2 = 25 + 2*0.5 + 0.01; => std = 1 => cov = 1 - cov[4][4] = 1e6 ; // about 0.5 degrees 1e-4 + if (m_angleValid) + cov[4][4] = 1e-4 ; // about 0.5 degrees 1e-4 + else + cov[4][4] = 1e6; } else { From 4aafb7d67532e4062c69cfa875fb66ec07824804 Mon Sep 17 00:00:00 2001 From: Adam Date: Mon, 2 Mar 2020 17:43:29 +0200 Subject: [PATCH 19/22] Change turret switching condition --- src/main/java/frc/robot/Constants.java | 2 ++ .../subsystems/turret/commands/TurretSwitching.java | 12 +++++++++--- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1531aad5..30bee21b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -132,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. diff --git a/src/main/java/frc/robot/subsystems/turret/commands/TurretSwitching.java b/src/main/java/frc/robot/subsystems/turret/commands/TurretSwitching.java index 57c0c50c..276dcd70 100644 --- a/src/main/java/frc/robot/subsystems/turret/commands/TurretSwitching.java +++ b/src/main/java/frc/robot/subsystems/turret/commands/TurretSwitching.java @@ -5,17 +5,22 @@ import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.drivetrain.FullLocalization; import frc.robot.subsystems.turret.Turret; +import frc.robot.utilities.Utils; import frc.robot.utilities.VisionModule; +import static frc.robot.Constants.Turret.TURRET_SWITCHING_TOLERANCE; + public class TurretSwitching extends CommandBase { private final LocalizationTurret localizationTurret; private final VisionTurret visionTurret; private final Turret turret; + private final Drivetrain drivetrain; private final Timer timer = new Timer(); private static final double VISION_TIMEOUT = 0.3; public TurretSwitching(Turret turret, Drivetrain drivetrain) { this.turret = turret; + this.drivetrain = drivetrain; this.localizationTurret = new LocalizationTurret(turret, drivetrain); this.visionTurret = new VisionTurret(turret); } @@ -29,12 +34,13 @@ public void initialize() { @Override public void execute() { - if (VisionModule.targetSeen()) { + if (turret.inDeadZone() || Math.abs(Utils.calculateTurretAngle(drivetrain.getPose(), false) - VisionModule.getVisionAngle()) < TURRET_SWITCHING_TOLERANCE) { + localizationTurret.execute(); + } + else if (VisionModule.targetSeen()) { visionTurret.execute(); timer.stop(); timer.reset(); - } else if (turret.inDeadZone()) { - localizationTurret.execute(); } else { if(timer.get() == 0) From 2dd59dca0bdccd3342941ca165f48d9be6e6e046 Mon Sep 17 00:00:00 2001 From: Adam Date: Mon, 2 Mar 2020 17:43:45 +0200 Subject: [PATCH 20/22] Add x, y covariance --- .../KalmanLocalization/OdometryInertialProcess.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java index 420fa756..0a1a52c8 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java @@ -88,8 +88,8 @@ public void stateFunctionJacobian(double[][] x, double[][] j) { @Override public void processNoiseCovariance(double[][] cov) { - cov[0][0] = 0; // Assume the position is not changing by itself - use a very small covariance - cov[1][1] = 0; // Assume the position is not changing by itself - use a very small covariance + cov[0][0] = 1e-5; // Assume the position is not changing by itself - use a very small covariance + cov[1][1] = 1e-5; // Assume the position is not changing by itself - use a very small covariance cov[2][2] = 1e-4; // Allow change in velocity can - change by measurements cov[3][3] = 1e-9; // assume phi is not changing cov[4][4] = 1e-2; // Allow change in omega From 0ad3a4d043da49cc0fe0c924702ebc63da0b315c Mon Sep 17 00:00:00 2001 From: Adam Date: Fri, 6 Mar 2020 16:57:48 +0200 Subject: [PATCH 21/22] Increase x,y covariance --- .../KalmanLocalization/OdometryInertialProcess.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java index 0a1a52c8..a4c461df 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/KalmanLocalization/OdometryInertialProcess.java @@ -39,7 +39,7 @@ public void initialState(double[][] x) { public void initialStateCovariance(double[][] cov) { // Sets initial variance for state variables. cov[0][0] = 0.01; // 10 cm accuracy for X - cov[1][1] = 0.01; // 10 cm accuracy for Y + cov[1][1] = 0.1; // 10 cm accuracy for Y cov[2][2] = 0.0001; // 0.01 m/s accuracy for X cov[3][3] = 8e-3; // 5 deg sqrd in rad for phi cov[4][4] = 1e-5; // assume not moving : 0.2 deg/s for omega @@ -88,8 +88,8 @@ public void stateFunctionJacobian(double[][] x, double[][] j) { @Override public void processNoiseCovariance(double[][] cov) { - cov[0][0] = 1e-5; // Assume the position is not changing by itself - use a very small covariance - cov[1][1] = 1e-5; // Assume the position is not changing by itself - use a very small covariance + cov[0][0] = 5e-4; // Assume the position is not changing by itself - use a very small covariance + cov[1][1] = 5e-4; // Assume the position is not changing by itself - use a very small covariance cov[2][2] = 1e-4; // Allow change in velocity can - change by measurements cov[3][3] = 1e-9; // assume phi is not changing cov[4][4] = 1e-2; // Allow change in omega From 356b91b8e59ce11077d1ccb61aaeb38beda08224 Mon Sep 17 00:00:00 2001 From: Adam Date: Fri, 6 Mar 2020 16:58:15 +0200 Subject: [PATCH 22/22] Add isMoving condition --- .../frc/robot/subsystems/drivetrain/FullLocalization.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java b/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java index ab110bd4..d307b1ce 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/FullLocalization.java @@ -176,7 +176,7 @@ public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters, var angle = new Rotation2d(gyroAngle.getRadians() + m_gyroOffset.getRadians()); double target_angle = turret.getAngle() + visionAngle.getDouble(0); target_angle = Math.IEEEremainder(target_angle, 360); - if (abs(target_angle) > 170) { + if (abs(target_angle) > 90) { angleValid = false; } else { angleValid = true; @@ -270,6 +270,9 @@ public boolean VisionValid() return false; } + if (navx.isMoving()) + return false; + return true; }