Users/ian/swerve to trunk

networktables.json

@@ -0,0 +1 @@
+[]

src/main/java/frc/robot/Constants.java

@@ -1,19 +1,191 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
 package frc.robot;
 
-/**
- * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants. This class should not be used for any other purpose. All constants should be declared
- * globally (i.e. public static). Do not put anything functional in this class.
- *
- * <p>It is advised to statically import this class (or one of its inner classes) wherever the
- * constants are needed, to reduce verbosity.
- */
+import com.ctre.phoenixpro.signals.SensorDirectionValue;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
+
+public final class Constants {
+    public static final class ModuleConstants {
+        // these constants should be correct for the current 4ki module we are using 
+        public static final double kWheelDiameterMeters = Units.inchesToMeters(4);
+        public static final double kDriveMotorGearRatio = 1/6.75;
+        public static final double kTurningMotorGearRatio = 1/21.0;
+        public static final double kDriveEncoderRot2Meter = kDriveMotorGearRatio * Math.PI * kWheelDiameterMeters;
+        public static final double kTurningEncoderRot2Deg = kTurningMotorGearRatio * 360;
+        public static final double kDriveEncoderRPM2MeterPerSec = kDriveEncoderRot2Meter / 60;
+        public static final double kTurningEncoderRPM2DegPerSec = kTurningEncoderRot2Deg / 60;
+
+
+        // We may need to tune this for the PID turning
+
+        // we may need to tune it so this is not set in stone
+        public static final double kPTurning = .01;
+        public static final double kDTurning = 0;
+
+        public static final double kPModuleDriveController = .01;
+        public static final double kDModuleDriveController = 0;
+    }
+
+    public static final class DriveConstants {
+
+        // we need to update this // no longer needs to be updated: I measured from center of axle to center of axle
+        //thse seem to be based off of the base dimensions
+        public static final double kTrackWidth = Units.inchesToMeters(21); 
+        public static final double kWheelBase = Units.inchesToMeters(21);
+
+        //PID constants
+        public static final double Ks = 0.10729;
+        public static final double Kv = 0.85456;
+        public static final double Ka = 0.047848;
+
+        public static final double kP = 0.025;
+        public static final double kI = 0;
+        public static final double kD = 0;
+
+        //This should be relative to the center, but still check documentation about how the grid is set up for swerve kinetics
+            //Ive changed it. It looked wrong and I fixed it based on the coordinate system at https://hhs-team670.github.io/MustangLib/frc/team670/robot/utils/math/Translation2d.html
+        public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
+                new Translation2d(kWheelBase / 2, kTrackWidth / 2), //front left
+                new Translation2d(kWheelBase / 2, -kTrackWidth / 2), //front right
+                new Translation2d(-kWheelBase / 2, kTrackWidth / 2), //back left
+                new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); //backright
+
+        //We need to update these motors
+        // frontLeft Module
+        // offsets should be drawn from no offest vaule on x tuner
+        public static final int kFrontLeftDriveMotorPort = 57; //motors updated
+        public static final int kFrontLeftTurningMotorPort = 56; //motors updated
+        public static final boolean kFrontLeftDriveMotorReversed = true; //re updated //updated
+        public static final boolean kFrontLeftTurningMotorReversed = true; //updated 
+        public static final int kFrontLeftDriveAbsoluteEncoderPort = 30; //updated
+        public static final boolean kFrontLeftDriveAbsoluteEncoderReversed = false; //updated
+        public static final double kFrontLeftDriveAbsoluteEncoderOffsetDeg = -0.134521; //updated, in degrees // they want this to be the negative of the reported values?
+        public static final SensorDirectionValue kFrontLeftTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive;
+
+        //we need to updats these motors
+        // frontRight Module
+        // offset -0.266357
+        public static final int kFrontRightDriveMotorPort = 51; //motors updated
+        public static final int kFrontRightTurningMotorPort = 50; //motors updated
+        public static final boolean kFrontRightDriveMotorReversed = false; //updated
+        public static final boolean kFrontRightTurningMotorReversed = true; //updated
+        public static final int kFrontRightDriveAbsoluteEncoderPort = 31; //updated
+        public static final boolean kFrontRightDriveAbsoluteEncoderReversed = false; //updated
+        public static final double kFrontRightDriveAbsoluteEncoderOffsetDeg = -0.281494; //updated, in degrees
+        public static final SensorDirectionValue kFrontRightTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive;
+
+        //we need to update these motors
+        // backLeft Module
+        // offset -0.480713
+        public static final int kBackLeftDriveMotorPort = 55; //motors updated
+        public static final int kBackLeftTurningMotorPort = 54; //motors updated
+        public static final boolean kBackLeftDriveMotorReversed = true; //updated
+        public static final boolean kBackLeftTurningMotorReversed = true; //updated
+        public static final int kBackLeftDriveAbsoluteEncoderPort = 33; //updated
+        public static final boolean kBackLeftDriveAbsoluteEncoderReversed = false; //updated
+        public static final double kBackLeftDriveAbsoluteEncoderOffsetDeg = -0.027832; //updated, in degrees
+        public static final SensorDirectionValue kBackLeftTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive;
+
+        //we need to update these motors
+        // backRight Module
+        //0.480957
+        public static final int kBackRightDriveMotorPort = 52; //motors updated
+        public static final int kBackRightTurningMotorPort = 53; //motors updated
+        public static final boolean kBackRightDriveMotorReversed = false; //updated
+        public static final boolean kBackRightTurningMotorReversed = true; //updated
+        public static final int kBackRightDriveAbsoluteEncoderPort = 32; //updated
+        public static final boolean kBackRightDriveAbsoluteEncoderReversed = false; //updated
+        public static final double kBackRightDriveAbsoluteEncoderOffsetDeg = 0.389160; //updated, in degrees
+        public static final SensorDirectionValue kBackRightTurningForwardDirection = SensorDirectionValue.CounterClockwise_Positive;
+
+
+        //NOTE: these are not used in actual code they are just used to define max based on physical contraints
+        //If you want to ignore these then change the limit on the max speeds manually 
+        //these seem to be mostly fine but we may need change some things
+        // also we need to change the physical dimensions of our base if we are going to use this 
+        public static final double kPhysicalMaxSpeedMetersPerSecond = 5676.0 / 60.0 * ModuleConstants.kDriveEncoderRot2Meter;
+        public static final double kPhysicalMaxAngularSpeedDegreesPerSecond = 360; //2 * Math.PI; //kPhysicalMaxSpeedMetersPerSecond / Math.hypot(DriveConstants.kTrackWidth / 2.0, DriveConstants.kWheelBase / 2.0 * 3);
+
+        //These are the variables that determine the max speeds of our swerve drive
+        public static final double kTeleDriveMaxSpeedMetersPerSecond = kPhysicalMaxSpeedMetersPerSecond / 4;
+        public static final double kTeleDriveMaxAngularSpeedDegreesPerSecond = kPhysicalMaxAngularSpeedDegreesPerSecond / 4;
+
+        public static final double kTeleDriveMaxAccelerationUnitsPerSecond = 3;
+        public static final double kTeleDriveMaxAngularAccelerationUnitsPerSecond = 3;
+    }
+
+    //these we can ignore for now since they are only for autonmous 
+    public static final class AutoConstants {
+        public static final double kMaxSpeedMetersPerSecond = DriveConstants.kPhysicalMaxSpeedMetersPerSecond / 4;
+        public static final double kMaxAngularSpeedDegreesPerSecond = DriveConstants.kPhysicalMaxAngularSpeedDegreesPerSecond / 10;
+
+        public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+        public static final double kMaxAngularAccelerationDegreesPerSecondSquared = 45.0;
+        public static final double kPXController = 1.5;
+        public static final double kPYController = 1.5;
+        public static final double kPThetaController = 3;
+
+        public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
+                new TrapezoidProfile.Constraints(
+                        kMaxAngularSpeedDegreesPerSecond,
+                        kMaxAngularAccelerationDegreesPerSecondSquared);
+    }
+
+    //this is just to set a deadzone so we don't need to move the stick
+    public static final class OIConstants {
+        public static final double kDeadband = 0.05;
+    }
+}
+
+ /**
 public final class Constants {
   public static class OperatorConstants {
     public static final int kDriverControllerPort = 0;
+
+    //these numbers may be incorrect 
   }
+
+    public static final int kDriverControllerPort = 1;
+
+    public static final double kTrackWidth = Units.inchesToMeters(21);
+        // Distance between right and left wheels
+    public static final double kWheelBase = Units.inchesToMeters(25.5);
+        // Distance between front and back wheels
+
+
+    //please update these with the accurate dimensions for robot
+    public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
+    new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+    new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+    new Translation2d(-kWheelBase / 2, -kTrackWidth / 2),
+    new Translation2d(-kWheelBase / 2, kTrackWidth / 2));
+
+
+
+    public static final double kDriveEncoderRot2Meter = kDriveMotorGearRatio * Math.PI * kWheelDiameterMeters;
+    public static final double kTurningEncoderRot2Rad = kTurningMotorGearRatio * 2 * Math.PI;
+    public static final double kDriveEncoderRPM2MeterPerSec = kDriveEncoderRot2Meter / 60;
+    public static final double kTurningEncoderRPM2RadPerSec = kTurningEncoderRot2Rad / 60;
+    public static final double kPturning = 0.5;
+    public static final double kMaxSpeed = 5;
+    public static final double kTeleDriveMaxAccelerationUnitsPerSecond = 5;
+    public static final double kTeleDriveMaxAngularAccelerationUnitsPerSecond = 5;
+
+    public static final double kPhysicalMaxSpeedMetersPerSecond = 5;
+    public static final double kPhysicalMaxAngularSpeedRadiansPerSecond = 2 * 2 * Math.PI;
+
+    public static final int kDriverYAxis = 1;
+    public static final int kDriverXAxis = 0;
+    public static final int kDriverRotAxis = 4;
+    public static final int kDriverFieldOrientedButtonIdx = 1;
+    public static final double kDeadband = 0.05;
+    public static final double kTeleDriveMaxSpeedMetersPerSecond = kPhysicalMaxSpeedMetersPerSecond / 4;
+    public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI / 4;
+    public static final double kTeleDriveMaxAngularSpeedRadiansPerSecond = //
+                kPhysicalMaxAngularSpeedRadiansPerSecond / 4;
 }
+*/
+

src/main/java/frc/robot/RobotContainer.java

@@ -1,63 +1,34 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
 package frc.robot;
 
-import frc.robot.Constants.OperatorConstants;
-import frc.robot.commands.Autos;
-import frc.robot.commands.ExampleCommand;
-import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
-
-/**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
- * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
- * subsystems, commands, and trigger mappings) should be declared here.
- */
+import frc.robot.commands.SwerveCommand;
+import frc.robot.subsystems.SwerveSubsystem;
+import frc.robot.Constants;
+import frc.robot.Constants.DriveConstants;
+
 public class RobotContainer {
-  // The robot's subsystems and commands are defined here...
-  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
-
-  // Replace with CommandPS4Controller or CommandJoystick if needed
-  private final CommandXboxController m_driverController =
-      new CommandXboxController(OperatorConstants.kDriverControllerPort);
-
-  /** The container for the robot. Contains subsystems, OI devices, and commands. */
-  public RobotContainer() {
-    // Configure the trigger bindings
-    configureBindings();
-  }
-
-  /**
-   * Use this method to define your trigger->command mappings. Triggers can be created via the
-   * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary
-   * predicate, or via the named factories in {@link
-   * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link
-   * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller
-   * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight
-   * joysticks}.
-   */
-  private void configureBindings() {
-    // Schedule `ExampleCommand` when `exampleCondition` changes to `true`
-    new Trigger(m_exampleSubsystem::exampleCondition)
-        .onTrue(new ExampleCommand(m_exampleSubsystem));
-
-    // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed,
-    // cancelling on release.
-    m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand());
-  }
-
-  /**
-   * Use this to pass the autonomous command to the main {@link Robot} class.
-   *
-   * @return the command to run in autonomous
-   */
-  public Command getAutonomousCommand() {
-    // An example command will be run in autonomous
-    return Autos.exampleAuto(m_exampleSubsystem);
-  }
-}
+
+    private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem();
+    private final XboxController driverOne = new XboxController(0);
+
+    public static final boolean ABSOLUTE_TURNING_MODE = true;
+
+    public RobotContainer() {
+        swerveSubsystem.setDefaultCommand(new SwerveCommand(swerveSubsystem, driverOne));
+        configureButtonBindings();
+        // System.out.println(DriveConstants.kTeleDriveMaxSpeedMetersPerSecond);
+    }
+
+    private void configureButtonBindings() {
+
+    }
+
+    public SwerveSubsystem getSwerve() {
+        return swerveSubsystem;
+    }
+
+    public Command getAutonomousCommand() {
+        return null;
+    }
+}