Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

FINAL OFFSETS ARE IN CODE

Browse Source
This commit is contained in:
aarav18
2023-01-31 19:24:12 -07:00
parent 2376d2636f
commit e6b2d2f486
5 changed files with 42 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
+3 -3
View File
@@ -169,9 +169,9 @@ public class Robot extends TimedRobot {
}
private void drive(boolean fieldRelative) {
final double xSpeed = -xLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftYAxis(), 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
final double ySpeed = -yLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftXAxis(), 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
final double rot = -rotLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getRightXAxis(), 0.02) * Units.feetToMeters(Math.PI));
final double xSpeed = -xLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftXAxis() * -0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
final double ySpeed = -yLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftYAxis() * 0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
final double rot = -rotLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getRightXAxis() * 0.3, 0.02) * Units.feetToMeters(Math.PI));
m_robotContainer.m_robotSwerveDrive.drive(xSpeed, ySpeed, rot, fieldRelative);
}
src/main/java/frc4388/robot/RobotContainer.java
+17 -5
View File
@@ -7,6 +7,11 @@
package frc4388.robot;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -75,7 +80,6 @@ public class RobotContainer {
// m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
}
/**
* Use this method to define your button->command mappings. Buttons can be
@@ -95,11 +99,14 @@ public class RobotContainer {
// .whileTrue(new RunCommand(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW), m_robotLED))
// .whileFalse(new RunCommand(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN), m_robotLED));
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whileTrue(new RunCommand(() -> m_robotSwerveDrive.rotateCANCodersToAngle(0), m_robotSwerveDrive));
// new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
// .whileTrue(new RunCommand(() -> m_robotSwerveDrive.rotateCANCodersToAngle(0), m_robotSwerveDrive));
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetCANCoders(0), m_robotSwerveDrive));
// new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetCANCoders(0), m_robotSwerveDrive));
// new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
// .onTrue()
//New interupt button
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
@@ -113,6 +120,11 @@ public class RobotContainer {
*/
public Command getAutonomousCommand() {
// no auto
// TrajectoryConfig trajConfig = new TrajectoryConfig(1, 1).setKinematics(m_robotSwerveDrive.getKinematics()); // TODO: make these AutoConstants
// Trajectory traj = TrajectoryGenerator.generateTrajectory(
// new Pose2d(0, 0, new Rotation2d(0)), null, null, trajConfig);
return new InstantCommand();
}
src/main/java/frc4388/robot/RobotMap.java
+10 -4
View File
@@ -127,10 +127,16 @@ public class RobotMap {
// rightBackEncoder.configMagnetOffset(225.0); //180.0); //270.0);//267.01171875);//SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET);
// initialize SwerveModules
this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -180.0);//, 270);//, SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET);
this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -267.0);//, 90);//, SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET);
this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -245.0);//, 90);//, SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET);
this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -42.0);//, 273);//, SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET);
this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder);//, -180.0);//, 270);//, SwerveDriveConstants.Configurations.LEFT_FRONT_ENCODER_OFFSET);
this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder);//, -267.0);//, 90);//, SwerveDriveConstants.Configurations.RIGHT_FRONT_ENCODER_OFFSET);
this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder);//, -245.0);//, 90);//, SwerveDriveConstants.Configurations.LEFT_BACK_ENCODER_OFFSET);
this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder);//, -42.0);//, 273);//, SwerveDriveConstants.Configurations.RIGHT_BACK_ENCODER_OFFSET);
// FINAL OFFSETS (I THINK)
// LEFT FRONT: -181.230469
// RIGHT FRONT: -270.615234
// LEFT BACK: -240.029297
// RIGHT BACK: -40.869142
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+4 -2
View File
@@ -29,8 +29,6 @@ public class SwerveDrive extends SubsystemBase {
private SwerveModule[] modules;
// private RobotGyro gyro3
private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
@@ -207,6 +205,10 @@ public class SwerveDrive extends SubsystemBase {
}
}
public SwerveDriveKinematics getKinematics() {
return this.kinematics;
}
@Override
public void periodic() {
// This method will be called once per scheduler run
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+8 -2
View File
@@ -16,6 +16,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.motorcontrol.Talon;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
@@ -30,7 +31,7 @@ public class SwerveModule extends SubsystemBase {
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
/** Creates a new SwerveModule. */
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, /*CANCoder canCoder*/CANCoder encoder, double offset) {
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder) {//, double offset) {
this.driveMotor = driveMotor;
this.angleMotor = angleMotor;
this.encoder = encoder;
@@ -47,9 +48,11 @@ public class SwerveModule extends SubsystemBase {
angleMotor.configAllSettings(angleConfig);
CANCoderConfiguration canCoderConfig = new CANCoderConfiguration();
canCoderConfig.magnetOffsetDegrees = offset;
// canCoderConfig.magnetOffsetDegrees = offset;
encoder.configAllSettings(canCoderConfig);
// driveMotor.config_kP(0, 0.4);
// canCoderConfig.magnetOffsetDegrees = 270;
}
@@ -134,7 +137,10 @@ public class SwerveModule extends SubsystemBase {
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
double inchesPerSecond = Units.metersToInches(state.speedMetersPerSecond);
driveMotor.set(/*angleMotor.get() + */feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND);
// driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME);
}
public void reset(double position) {