Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

getScalarForLine function

Browse Source
This commit is contained in:
aarav18
2022-01-20 19:38:58 -07:00
parent bfa044e340
commit 8d2911b4f7
2 changed files with 39 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+7 -2
View File
@@ -4,6 +4,9 @@
package frc4388.robot;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import frc4388.utility.Gains;
import frc4388.utility.LEDPatterns;
@@ -28,8 +31,8 @@ public final class Constants {
public static final double HEIGHT = 22;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2;
public static final double MAX_SPEED_FEET_PER_SEC = 20;
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
public static final double MAX_SPEED_FEET_PER_SEC = 20; // redundant constant?
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // redundant constant?
//IDs
public static final int LEFT_FRONT_STEER_CAN_ID = 2;
@@ -83,6 +86,8 @@ public final class Constants {
// misc
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
// TODO: put in real numbers for the hub
public static final Pose2d HUB_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0));
}
public static final class LEDConstants {
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+32 -1
View File
@@ -12,6 +12,7 @@ import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.StateSpaceUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
@@ -148,6 +149,36 @@ public class SwerveDrive extends SubsystemBase {
super.periodic();
}
/**
* Get a "noisy" fake global pose reading.
*
* @param estimatedRobotPose The robot pose.
*/
// TEST FAKE CODE FROM EXAMPLE
public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
var rand =
StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
return new Pose2d(
estimatedRobotPose.getX() + rand.get(0, 0),
estimatedRobotPose.getY() + rand.get(1, 0),
estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0))));
}
/**
* Returns a scalar from your distance to the hub to your target distance.
*
* @param target_dist The target distance.
* @return A scalar that multiplies your distance from the hub to get your target distance.
*/
public double getScalarForLine(double target_dist) {
Pose2d p1 = m_poseEstimator.getEstimatedPosition();
Pose2d p2 = SwerveDriveConstants.HUB_POSE;
double dist = Math.sqrt(Math.pow(p1.getX() - p2.getX(), 2) + Math.pow(p1.getY() - p2.getY(), 2));
return target_dist/dist;
}
/** Updates the field relative position of the robot. */
public void updateOdometry() {
m_poseEstimator.update( m_gyro.getRotation2d(),
@@ -159,7 +190,7 @@ public class SwerveDrive extends SubsystemBase {
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
// a real robot, this must be calculated based either on latency or timestamps.
m_poseEstimator.addVisionMeasurement(
ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
SwerveDrive.getEstimatedGlobalPose(
m_poseEstimator.getEstimatedPosition()),
Timer.getFPGATimestamp() - 0.1);
}