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

java docs and yaw print

Browse Source
This commit is contained in:
aarav18
2022-02-17 19:52:05 -07:00
parent 749085a0b6
commit 32ae624588
5 changed files with 38 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Main.java
+1 -2
View File
@@ -28,5 +28,4 @@ public final class Main {
}
}
// hi ryan
// hi ryan -aarav
src/main/java/frc4388/robot/Robot.java
+3 -5
View File
@@ -66,6 +66,8 @@ public class Robot extends TimedRobot {
// 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();
// print odometry data to smart dashboard for debugging (if causing timeout errors, you can comment it)
SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX());
SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY());
SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
@@ -84,9 +86,6 @@ public class Robot extends TimedRobot {
@Override
public void disabledPeriodic() {
// SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX());
// SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY());
// SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
}
/**
@@ -114,6 +113,7 @@ public class Robot extends TimedRobot {
@Override
public void teleopInit() {
LOGGER.fine("teleopInit()");
m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw());
// 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
@@ -130,8 +130,6 @@ public class Robot extends TimedRobot {
*/
@Override
public void teleopPeriodic() {
// m_robotContainer.getDriverController().updateInput();
// m_robotContainer.getOperatorController().updateInput();
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+16 -3
View File
@@ -44,7 +44,7 @@ public class RobotContainer {
private final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
private final TalonFX m_testMotor = new TalonFX(23);
@@ -115,17 +115,19 @@ public class RobotContainer {
* Generate autonomous
* @param maxVel max velocity for the path (null to override default value of 5.0)
* @param maxAccel max acceleration for the path (null to override default value of 5.0)
* @param inputs strings (paths) or commands you want to run (in order)
* @return array of commands
* @param inputs strings (path names) or commands you want to run (in order)
* @return array of commands, which can then be processed in a command group
*/
public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) {
// default vel and acc
maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.MAX_VEL);
maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.MAX_ACC);
ArrayList<Command> commands = new ArrayList<Command>();
commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()));
// pids controlling the path
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
@@ -134,7 +136,9 @@ public class RobotContainer {
// parse input
for (int i=0; i<inputs.length; i++) {
// if string, process as pathplanner trajectory
if (inputs[i] instanceof String) {
PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
PathPlannerState initState = (PathPlannerState) traj.sample(0);
@@ -150,6 +154,7 @@ public class RobotContainer {
m_robotSwerveDrive));
}
// if command, just add it to the array
if (inputs[i] instanceof Command) {
commands.add((Command) inputs[i]);
}
@@ -185,10 +190,18 @@ public class RobotContainer {
return m_driverXbox;
}
/**
* Get odometry.
* @return Odometry
*/
public Pose2d getOdometry() {
return m_robotSwerveDrive.getOdometry();
}
/**
* Set odometry to given pose.
* @param pose Pose to set odometry to.
*/
public void resetOdometry(Pose2d pose) {
m_robotSwerveDrive.resetOdometry(pose);
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+8 -14
View File
@@ -4,6 +4,8 @@
package frc4388.robot.subsystems;
import java.util.ArrayList;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
@@ -155,18 +157,14 @@ public class SwerveDrive extends SubsystemBase {
@Override
public void periodic() {
// System.err.println(m_gyro.getFusedHeading() +" aaa");
updateOdometry();
// SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
// SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
// SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());
// SmartDashboard.putNumber("Pigeon Rotation 2D", m_gyro.getRotation2d().getDegrees());
// SmartDashboard.putStringArray("Fusion Status", new String[] {"Is Fusing: "+fstatus.bIsFusing, "Is Valid: "+fstatus.bIsValid, "Heading: "+fstatus.heading});
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
super.periodic();
}
@@ -207,10 +205,9 @@ public class SwerveDrive extends SubsystemBase {
}
/**
* Resets the odometry of the robot to (x=0, y=0, theta=0).
* Resets the odometry of the robot to the given pose.
*/
public void resetOdometry(Pose2d pose) {
// m_odometry.resetPosition(pose, m_gyro.getRotation2d());
m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
}
@@ -222,12 +219,6 @@ public class SwerveDrive extends SubsystemBase {
modules[1].getState(),
modules[2].getState(),
modules[3].getState());
// m_odometry.update( m_gyro.getRotation2d(),
// modules[0].getState(),
// modules[1].getState(),
// modules[2].getState(),
// modules[3].getState());
// 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.
@@ -241,6 +232,9 @@ public class SwerveDrive extends SubsystemBase {
rotTarget = new Rotation2d(0);
}
/**
* Stop all four swerve modules.
*/
public void stopModules() {
modules[0].stop();
modules[1].stop();
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+10 -4
View File
@@ -62,7 +62,7 @@ public class SwerveModule extends SubsystemBase {
private Rotation2d getAngle() {
// Note: This assumes the CANCoders are setup with the default feedback coefficient
// and the sesnor value reports degrees.
// and the sensor value reports degrees.
return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
}
@@ -90,20 +90,26 @@ public class SwerveModule extends SubsystemBase {
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
// driveMotor.set(TalonFXControlMode.Velocity, /*angleMotor.get() + */(Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
// Add this line to correct for the slight drive movement the angle motor makes when turning in place.
// driveMotor.set(TalonFXControlMode.Velocity, angleMotor.get() + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC);
}
/**
* Returns the current state of the module.
* Get current module state.
*
* @return The current state of the module.
* @return The current state of the module in m/s.
*/
public SwerveModuleState getState() {
// return state;
return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
}
/**
* Stop the drive and steer motors of current module.
*/
public void stop() {
driveMotor.set(0);
angleMotor.set(0);