diff --git a/build.gradle b/build.gradle index 3617d19..c16e3c3 100644 --- a/build.gradle +++ b/build.gradle @@ -28,7 +28,7 @@ deploy { frcJava(getArtifactTypeClass('FRCJavaArtifact')) { // Enable visualvm - jvmArgs << "-Dcom.sun.management.jmxremote=true -Dcom.sun.management.jmxremote.port=1099 -Dcom.sun.management.jmxremote.local.only=false -Dcom.sun.management.jmxremote.ssl=false -Dcom.sun.management.jmxremote.authenticate=false -Djava.rmi.server.hostname=10.43.88.2" + // jvmArgs << "-Dcom.sun.management.jmxremote=true -Dcom.sun.management.jmxremote.port=1099 -Dcom.sun.management.jmxremote.local.only=false -Dcom.sun.management.jmxremote.ssl=false -Dcom.sun.management.jmxremote.authenticate=false -Djava.rmi.server.hostname=10.43.88.2" } // Static files artifact diff --git a/simgui.json b/simgui.json index a47a9ab..71b8c82 100644 --- a/simgui.json +++ b/simgui.json @@ -7,5 +7,10 @@ "/LiveWindow/SwerveModule": "Subsystem", "/LiveWindow/Ungrouped/Scheduler": "Scheduler" } + }, + "NetworkTables": { + "SmartDashboard": { + "open": true + } } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 03eda20..521b2c2 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -5,6 +5,8 @@ package frc4388.robot; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.RobotTime; @@ -49,6 +51,9 @@ 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(); + 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()); } /** @@ -63,6 +68,9 @@ 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,8 +122,8 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - m_robotContainer.getDriverController().updateInput(); - m_robotContainer.getOperatorController().updateInput(); + // m_robotContainer.getDriverController().updateInput(); + // m_robotContainer.getOperatorController().updateInput(); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 938890a..07be219 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -5,6 +5,7 @@ package frc4388.robot; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -116,6 +117,9 @@ public class RobotContainer { return m_driverXbox; } + public Pose2d getOdometry() { + return m_robotSwerveDrive.getOdometry(); + } /** * Add your docs here. */ diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 4b41fa7..6d331de 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -4,6 +4,7 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_PigeonIMU; @@ -61,32 +62,41 @@ public class RobotMap { rightBackSteerMotor.configFactoryDefault(); rightBackWheelMotor.configFactoryDefault(); - leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + + leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake); + leftFrontWheelMotor.setNeutralMode(NeutralMode.Coast); + rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake); + rightFrontWheelMotor.setNeutralMode(NeutralMode.Coast); + leftBackSteerMotor.setNeutralMode(NeutralMode.Brake); + leftBackWheelMotor.setNeutralMode(NeutralMode.Coast); + rightBackSteerMotor.setNeutralMode(NeutralMode.Brake); + rightBackWheelMotor.setNeutralMode(NeutralMode.Coast); // config cancoder as remote encoder for swerve steer motors //leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 1ca26e1..341671f 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -32,7 +32,7 @@ public class LED extends SubsystemBase { @Override public void periodic(){ - SmartDashboard.putNumber("LED", m_currentPattern.getValue()); + //SmartDashboard.putNumber("LED", m_currentPattern.getValue()); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 15bb384..c4fe655 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -8,24 +8,20 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.WPI_PigeonIMU; -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; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; public class SwerveDrive extends SubsystemBase { - SwerveDriveKinematics m_kinematics; private WPI_TalonFX m_leftFrontSteerMotor; private WPI_TalonFX m_leftFrontWheelMotor; private WPI_TalonFX m_rightFrontSteerMotor; @@ -51,14 +47,14 @@ public class SwerveDrive extends SubsystemBase { // setSwerveGains(); - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); + private SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); public SwerveModule[] modules; public WPI_PigeonIMU m_gyro; /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used below are robot specific, and should be tuned. */ - private final SwerveDrivePoseEstimator m_poseEstimator; + private SwerveDrivePoseEstimator m_poseEstimator; public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public boolean ignoreAngles; @@ -119,7 +115,7 @@ public class SwerveDrive extends SubsystemBase { double xSpeedMetersPerSecond = -speeds[0] * speedAdjust; double ySpeedMetersPerSecond = speeds[1] * speedAdjust; SwerveModuleState[] states = - kinematics.toSwerveModuleStates( + m_kinematics.toSwerveModuleStates( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED)); @@ -133,28 +129,18 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { - System.err.println(m_gyro.getFusedHeading() +" aaa"); + //System.err.println(m_gyro.getFusedHeading() +" aaa"); + updateOdometry(); // m_gyro.setFusedHeadingToCompass(); // m_gyro.setYawToCompass(); - // m_gyro.getRotation2d(); + // System.out.println(m_gyro.getRotation2d()); + // System.out.println(modules[0].getState()); + // System.out.println(modules[1].getState()); + // System.out.println(modules[2].getState()); + // System.out.println(modules[3].getState()); 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)))); - } - /** * Gets the distance between two given poses. * @param p1 The first pose. @@ -171,11 +157,22 @@ public class SwerveDrive extends SubsystemBase { * @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) { + public Pose2d poseGivenDist(double target_dist) { Pose2d p1 = m_poseEstimator.getEstimatedPosition(); Pose2d p2 = SwerveDriveConstants.HUB_POSE; - return target_dist/distBtwPoses(p1, p2); + double scalar = target_dist/distBtwPoses(p1, p2); + Pose2d new_pose = new Pose2d(p1.getX() * scalar, p1.getY() * scalar, p1.getRotation()); + + return new_pose; + } + + /** + * Gets the current pose of the robot. + * @return Robot's current pose. + */ + public Pose2d getOdometry() { + return m_poseEstimator.getEstimatedPosition(); } /** Updates the field relative position of the robot. */ @@ -188,10 +185,9 @@ 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( - SwerveDrive.getEstimatedGlobalPose( - m_poseEstimator.getEstimatedPosition()), - Timer.getFPGATimestamp() - 0.1); + // m_poseEstimator.addVisionMeasurement( + // m_poseEstimator.getEstimatedPosition(), + // Timer.getFPGATimestamp() - 0.1); } public void highSpeed(boolean shift){ diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 3b2b192..5a6a64d 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -27,7 +27,7 @@ public class SwerveModule extends SubsystemBase { public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; private static double kEncoderTicksPerRotation = 4096; - + private SwerveModuleState state; /** Creates a new SwerveModule. */ @@ -61,7 +61,7 @@ public class SwerveModule extends SubsystemBase { } - public Rotation2d getAngle() { + private Rotation2d getAngle() { // Note: This assumes the CANCoders are setup with the default feedback coefficient // and the sesnor value reports degrees. return Rotation2d.fromDegrees(canCoder.getAbsolutePosition()); @@ -74,8 +74,8 @@ public class SwerveModule extends SubsystemBase { public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) { Rotation2d currentRotation = getAngle(); - SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); - SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); + //SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); + state = SwerveModuleState.optimize(desiredState, currentRotation); // Find the difference between our current rotational position + our new rotational position Rotation2d rotationDelta = state.angle.minus(currentRotation); @@ -100,7 +100,8 @@ public class SwerveModule extends SubsystemBase { * @return The current state of the module. */ public SwerveModuleState getState() { - return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK * SwerveDriveConstants.METERS_PER_INCH * 10, new Rotation2d(canCoder.getPosition())); + // return state; + return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle()); } } diff --git a/src/main/java/frc4388/utility/controller/XboxController.java b/src/main/java/frc4388/utility/controller/XboxController.java index 4e1c541..51c0d4e 100644 --- a/src/main/java/frc4388/utility/controller/XboxController.java +++ b/src/main/java/frc4388/utility/controller/XboxController.java @@ -64,7 +64,7 @@ public class XboxController implements IHandController ret[1] = y / mag; } double [] to_smart_dashboard = {square_mag, x, y, ret[0], ret[1]}; - SmartDashboard.putNumberArray("Input", to_smart_dashboard); + //SmartDashboard.putNumberArray("Input", to_smart_dashboard); return ret; }