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

switch to pigeon2 + vector2d docs

Browse Source
This commit is contained in:
aarav18
2022-03-14 18:29:11 -06:00
parent 946e7aee47
commit 8a25b7c1d5
9 changed files with 44 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Extender.java
-2
View File
@@ -12,7 +12,6 @@ import com.revrobotics.CANSparkMax.SoftLimitDirection;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ExtenderConstants;
import frc4388.utility.DesmosServer;
public class Extender extends SubsystemBase {
@@ -51,7 +50,6 @@ public class Extender extends SubsystemBase {
public void periodic() {
// This method will be called once per scheduler run
SmartDashboard.putNumber("Extender Position", m_extenderMotor.getEncoder().getPosition());
DesmosServer.putDouble("ExtenderPosition", m_extenderMotor.getEncoder().getPosition());
}
/**
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+7 -15
View File
@@ -4,11 +4,9 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
@@ -46,14 +44,8 @@ public class SwerveDrive extends SubsystemBase {
m_backLeftLocation, m_backRightLocation);
public SwerveModule[] modules;
public WPI_PigeonIMU m_gyro;
protected FusionStatus fstatus = new FusionStatus();
public WPI_Pigeon2 m_gyro;
/*
* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings.
* The numbers used
* below are robot specific, and should be tuned.
*/
public SwerveDrivePoseEstimator m_poseEstimator;
public SwerveDriveOdometry m_odometry;
@@ -65,7 +57,7 @@ public class SwerveDrive extends SubsystemBase {
private final Field2d m_field = new Field2d();
public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack,
WPI_PigeonIMU gyro) {
WPI_Pigeon2 gyro) {
m_leftFront = leftFront;
m_leftBack = leftBack;
@@ -74,14 +66,14 @@ public class SwerveDrive extends SubsystemBase {
m_gyro = gyro;
modules = new SwerveModule[] { m_leftFront, m_rightFront, m_leftBack, m_rightBack};
m_poseEstimator = new SwerveDrivePoseEstimator(
m_gyro.getRotation2d(),
new Pose2d(),
m_kinematics,
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),
VecBuilder.fill(Units.degreesToRadians(1)),
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)));
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)), // TODO: tune
VecBuilder.fill(Units.degreesToRadians(1)), // TODO: tune
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1))); // TODO: tune
m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());