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

driving field relative works and nathans turning works

Browse Source
This commit is contained in:
aarav18
2022-03-17 15:09:46 -06:00
parent ba8de492b3
commit b4bf4296de
3 changed files with 9 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1 -1
View File
@@ -34,7 +34,7 @@ import frc4388.utility.LEDPatterns;
*/ */
public final class Constants { public final class Constants {
public static final class SwerveDriveConstants { public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = 2.0; public static final double ROTATION_SPEED = 4.0;
public static final double WIDTH = 23.5; public static final double WIDTH = 23.5;
public static final double HEIGHT = 23.5; public static final double HEIGHT = 23.5;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
src/main/java/frc4388/robot/RobotContainer.java
+1 -1
View File
@@ -155,7 +155,7 @@ public class RobotContainer {
getDriverController().getLeftY(), getDriverController().getLeftY(),
//getDriverController().getRightX(), //getDriverController().getRightX(),
getDriverController().getRightX(), getDriverController().getRightX(),
// getDriverController().getRightY(), getDriverController().getRightY(),
true), true),
m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
// Intake with Triggers // Intake with Triggers
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+7 -5
View File
@@ -113,16 +113,16 @@ public class SwerveDrive extends SubsystemBase {
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) { public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) {
ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0; ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
Translation2d speed = new Translation2d(leftX, leftY); Translation2d speed = new Translation2d(-leftX, leftY);
speed = speed.times(speed.getNorm() * speedAdjust); speed = speed.times(speed.getNorm() * speedAdjust);
if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND) if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND)
rotTarget = new Rotation2d(rightY, rightX); rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1));
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians(); double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
double xSpeedMetersPerSecond = speed.getX(); double xSpeedMetersPerSecond = -speed.getX();
double ySpeedMetersPerSecond = speed.getY(); double ySpeedMetersPerSecond = speed.getY();
chassisSpeeds = fieldRelative chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(m_gyro.getRotation2d().getDegrees())) rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d(m_gyro.getRotation2d().getRadians() + (Math.PI/2)))
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * 8); : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED * 8);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates( SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
chassisSpeeds); chassisSpeeds);
@@ -154,6 +154,8 @@ public class SwerveDrive extends SubsystemBase {
updateOdometry(); updateOdometry();
updateSmartDash(); updateSmartDash();
SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees());
SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle());
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360); SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360);
@@ -165,7 +167,7 @@ public class SwerveDrive extends SubsystemBase {
// odometry // odometry
SmartDashboard.putNumber("Odometry: X", getOdometry().getX()); SmartDashboard.putNumber("Odometry: X", getOdometry().getX());
SmartDashboard.putNumber("Odometry: Y", getOdometry().getY()); SmartDashboard.putNumber("Odometry: Y", getOdometry().getY());
SmartDashboard.putNumber("Odometry: θ", getOdometry().getRotation().getDegrees()); SmartDashboard.putNumber("Odometry: Theta", getOdometry().getRotation().getDegrees());
// chassis speeds // chassis speeds
// TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds // TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds