Testing testing with swerve testing

.vscode/settings.json

@@ -38,7 +38,7 @@
     "java.format.comments.enabled": false,
     "java.format.onType.enabled": true,
     "editor.formatOnSave": true,
-    "java.saveActions.organizeImports": true,
+    "java.saveActions.organizeImports": false,
     "editor.codeActionsOnSave": {
         "source.organizeImports": true
     }

build.gradle

@@ -3,6 +3,7 @@ plugins {
     id "edu.wpi.first.GradleRIO" version "2022.2.1"
 }
 
+
 sourceCompatibility = JavaVersion.VERSION_11
 targetCompatibility = JavaVersion.VERSION_11
 
@@ -43,7 +44,7 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcJava
 wpi.java.debugJni = false
 
 // Set this to true to enable desktop support.
-def includeDesktopSupport = false
+def includeDesktopSupport = true
 
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 4.
@@ -64,7 +65,7 @@ dependencies {
     nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop)
     nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
     simulationRelease wpi.sim.enableRelease()
-
+    testImplementation "org.mockito:mockito-core:2.+"
     testImplementation 'junit:junit:4.12'
 }
 
@@ -80,7 +81,9 @@ jar {
     manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS)
     duplicatesStrategy = DuplicatesStrategy.INCLUDE
 }
-
+test {
+    useJUnit()
+}
 // Configure jar and deploy tasks
 deployArtifact.jarTask = jar
 wpi.java.configureExecutableTasks(jar)

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

@@ -1,9 +1,9 @@
-// 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 edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.util.Units;
+
 /**
  * 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
@@ -13,4 +13,203 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
+    public static final class ElectricalConstants {
+        // public static final PneumaticsModuleType kPneumaticHub = PneumaticsModuleType.CTREPCM;
+        // public static final ModuleType kPowerDistributionModule = ModuleType.kCTRE;
+
+        public static final int kRobotControllerPort = 0;
+        public static final int kPowerDistributionPort = 1;
+
+        // public static final int kRearRightDriveMotorPort = 10;
+        // public static final int kFrontRightDriveMotorPort = 12;
+        // public static final int kFrontLeftDriveMotorPort = 22;
+        // public static final int kRearLeftDriveMotorPort = 24;
+
+        // public static final int kRearRightTurningMotorPort = 11;
+        // public static final int kFrontRightTurningMotorPort = 13;
+        // public static final int kFrontLeftTurningMotorPort = 23;
+        // public static final int kRearLeftTurningMotorPort = 25;
+
+        // public static final int kRearRightTurningEncoderPort = 31;
+        // public static final int kFrontRightTurningEncoderPort = 33;
+        // public static final int kFrontLefTurningEncoderPort = 43;
+        // public static final int kRearLeftTurningEncoderPort = 45;
+
+        public static final int kGyroPort = 20;
+
+        // public static final int kIntakeRetractPort = 1; // Pneumatic Control Module Port, not CAN ID
+        // public static final int kIntakeDeployPort = 0; // Pneumatic Control Module Port, not CAN ID
+        // public static final int kIntakeBottomPort = 21;
+        // public static final int kIntakeTopPort = 20;
+
+        // public static final int kTriggerPort = 14;
+
+        // public static final int kShooterHoodPort = 15;
+        // public static final int kShooterLeaderPort = 16;
+        // public static final int kShooterFollowerPort = 17;
+
+        // public static final int kClimberDeployPort = 2;
+        // public static final int kClimberRetractPort = 3;
+
+    }
+
+    public static final class DriveConstants {
+
+        // Define the conventional order of our modules when putting them into arrays
+        // public static final int FRONT_LEFT = 0;
+        // public static final int FRONT_RIGHT = 1;
+        // public static final int REAR_LEFT = 2;
+        // public static final int REAR_RIGHT = 3;
+
+        // public static final class CANCoder {
+        //     public static final double kRearRightTurningEncoderOffset = 180.0;
+        //     public static final double kFrontRightTurningEncoderOffset = 180.0;
+        //     public static final double kFrontLefTurningEncoderOffset = 180.0;
+        //     public static final double kRearLeftTurningEncoderOffset = 180.0;
+        // }
+
+        // public static final boolean kFrontLeftDriveEncoderReversed = false;
+        // public static final boolean kFrontRightDriveEncoderReversed = false;
+        // public static final boolean kRearLeftDriveEncoderReversed = true;
+        // public static final boolean kRearRightDriveEncoderReversed = true;
+        // public static final int kSwerveTestMotorTurning = 38;
+        // public static final int analogEncoderSwerveTesting = 3;
+        // public static final int kSwerveTestMotorDrive = 39;
+        // public static final int kSwerveTestMotorTurning2 = 40; // SET REAL VALUES
+        // public static final int kSwerveTestMotorDrive2 = 41;
+        // public static final int analogEncoderSwerveTesting2 = 42;
+        // public static final boolean kFrontLeftTurningEncoderReversed = false;
+        // public static final boolean kFrontRightTurningEncoderReversed = false;
+        // public static final boolean kRearLeftTurningEncoderReversed = true;
+        // public static final boolean kRearRightTurningEncoderReversed = true;
+        public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
+
+        // Units are meters.
+        // Distance between centers of right and left wheels on robot
+        public static final double kTrackWidth = Units.inchesToMeters(22.5); // 22.5 in
+
+        // Distance between front and back wheels on robot
+        public static final double kWheelBase = Units.inchesToMeters(22.5); // 24.5 in
+
+        // Units are meters per second
+        public static final double kMaxTranslationalVelocity = 3.5; //max 4.5
+
+        // Units are radians per second
+        public static final double kMaxRotationalVelocity = 3.75; //max 5.0
+        public static final double kMaxSpeedMetersPerSecond = 3.5;
+        // // Max velocity
+
+        // //The locations for the modules must be relative to the center of the robot. 
+        // // Positive x values represent moving toward the front of the robot 
+        // // Positive y values represent moving toward the left of the robot.
+        public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
+                new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0), // front left
+                new Translation2d(kWheelBase / 2.0, -kTrackWidth / 2.0), // front right
+                new Translation2d(-kWheelBase / 2.0, kTrackWidth / 2.0), // rear left
+                new Translation2d(-kWheelBase / 2.0, -kTrackWidth / 2.0) // rear right
+        );
+        public static final int kFrontRightDriveMotor = 39;
+        public static final int kFrontRightTurningMotor = 38;
+        public static final int kFrontRightEncoder = 3;
+
+        public static final int kFrontLeftDriveMotor = 2;
+        public static final int kFrontLeftTurningMotor = 23;
+        public static final int kFrontLeftEncoder = 1;
+
+        public static final int kRearRightDriveMotor = 3;
+        public static final int kRearRightTurningMotor = 32;
+        public static final int kRearRightEncoder = 2;
+
+        public static final int kRearLeftDriveMotor = 16;
+        public static final int kRearLeftTurningMotor = 1;
+        public static final int kRearLeftEncoder = 0;
+        public static final int kGyroPort = 0;
+
+        // public static final boolean kGyroReversed = false;
+    }
+
+    public static final class ModuleConstants {
+        public static final double kDriveP = 0.01;
+        public static final double kDriveI = 0.0;
+        public static final double kDriveD = 0.0;
+        public static final double kDriveFF = 2.96;
+
+        public static final double kTurningP = 0.01;
+        public static final double kTurningI = 0.0;
+        public static final double kTurningD = 0.005;
+
+        public static final double kMaxModuleAngularSpeedRadiansPerSecond = 0.000005 * Math.PI;
+        public static final double kMaxModuleAngularAccelerationRadiansPerSecondSquared = 0.000005 * Math.PI;
+
+        // public static final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(0.254, 0.137);
+
+        public static final double kWheelDiameterMeters = 0.098; // 0.09398; // 3.7 in
+
+        // The drive encoder reports in RPM by default. Calculate the conversion factor
+        // to make it report in meters per second.
+        public static final double kDriveGearRatio = 8.33;
+        public static final double kDriveConversionFactor = (kWheelDiameterMeters * Math.PI) / kDriveGearRatio;
+
+        public static final double kTurnPositionConversionFactor = 18;
+
+        public static final double kMaxSpeedMetersPerSecond = 4.0;
+    }
+
+    //   public static final class IntakeConstants {
+    //     public static final double kRollerSpeed = -0.85; // power percentage
+    //     public static final double kRollerEjectSpeed = 0.50; // power percentage
+    //   }
+
+    //   public static final class ShooterConstants {
+    //     public static final double kTriggerSpeed = 1.0; // power percentage
+    //     public static final double kTriggerDeadband = 0.7;
+
+    //     public static final int kShootHighSpeed = 3500; // RPM
+    //     public static final int kShootLowSpeed = 2000;  // RPM
+
+    //     public static final double kHoodSpeed = 0.25; // May want to convert to RPM
+    //     public static final double kHoodGearingRatio = 4.075 / 5.0;  // 1 rev of the motor = 48/4240 revs of the hood = ~4.075 deg
+
+    //     public static final double kHoodMinAngle = 60.0;
+    //     public static final double kHoodMaxAngle = 115.0;
+
+    //     // public static final double kHoodMinAngle = 45.0;
+    //     // public static final double kHoodMaxAngle = 70.0;
+
+    //     public static final double kHoodP = 0.1;
+    //     public static final double kHoodI = 0.0;
+    //     public static final double kHoodD = 0.0;
+
+    //     public static final double kShooterP = 0.0;
+    //     public static final double kShooterI = 0.0;
+    //     public static final double kShooterD = 0.0;
+    //     public static final double kShooterFF = 0.00025;
+
+    //     public static final double kHoodSafetyCurrentLimit = 30.0;
+    //     public static final double kHoodStopCurrentLimit = 25.0;
+
+    //   }
+
+    //   public static final class OIConstants {
+    //     public static final String kXbox = "XBOX";
+    //     public static final String kPS4 = "P";
+    //     public static final String kRadioMaster = "TX16S";
+    //     public static final String kZorro = "Zorro";
+
+    //     public static final String[] kDriverControllers = new String[]{kRadioMaster, kZorro};
+    //     public static final String[] kOperatorControllers = new String[]{kXbox};
+
+    //     public static final int kDriverPort = 0;
+    //     public static final int kOperatorPort = 1;
+    //   }
+
+    //   public static final class AutoConstants {
+    //     public static final double kMaxSpeedMetersPerSecond = 2.0;
+    //     public static final double kMaxAccelerationMetersPerSecondSquared = 2.0;
+    //     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI / 2;
+    //     public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
+
+    //     public static final double kPTranslationController = 6.0;
+    //     public static final double kPThetaController = 6.0;
+    //   }
 }

src/main/java/frc/robot/Robot.java

@@ -85,6 +85,7 @@ public void teleopInit() {
     /** This function is called periodically during operator control. */
     @Override
     public void teleopPeriodic() {
+        m_robotContainer.printDriveBaseVals();
     }
 
     @Override

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

@@ -4,11 +4,16 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.commands.ExampleCommand;
-import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.commands.FullSwerveDrive;
+import frc.robot.commands.Turn;
+import frc.robot.subsystems.FullSwerveBase;
+import frc.robot.subsystems.SwerveModule;
+import frc.robot.utils.MotorFactory;
+import frc.robot.utils.SubsystemFactory;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -18,23 +23,56 @@
  */
 public class RobotContainer {
     // The robot's subsystems and commands are defined here...
-    private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
+    // private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
+    // private final SwerveModule mTest;
+    // private final SwerveModule mTest2;
+    SwerveModule m_RightFrontSwerveModule;
+    SwerveModule m_LeftFrontSwerveModule;
+    SwerveModule m_RightRearSwerveModule;
+    SwerveModule m_LeftRearSwerveModule;
+    SwerveModule[] m_SwerveModules;
 
-    private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem);
+    FullSwerveBase m_SwerveBase;
+
+    private XboxController myController;
+    private SubsystemFactory subsystemFactory;
+
+    ADXRS450_Gyro m_Gyro;
+
+    // private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem);
 
     /** The container for the robot. Contains subsystems, OI devices, and commands. */
-    public RobotContainer() {
-        // Configure the button bindings
+    public RobotContainer(SubsystemFactory subsystemFactory) {
+        this.subsystemFactory = subsystemFactory;
+        m_SwerveBase = this.subsystemFactory.createFullSwerveBase();
+
         configureButtonBindings();
     }
 
+    public RobotContainer() {
+        MotorFactory motorFactory = new MotorFactory();
+        this.subsystemFactory = new SubsystemFactory(motorFactory);
+        m_SwerveBase = this.subsystemFactory.createFullSwerveBase();
+    }
+
+    public void printDriveBaseVals() {
+        m_SwerveBase.printAllVals();
+    }
+
     /**
      * Use this method to define your button->command mappings. Buttons can be created by
      * instantiating a {@link GenericHID} or one of its subclasses ({@link
      * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
      * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
      */
     private void configureButtonBindings() {
+        myController = new XboxController(0);
+        // new JoystickButton(myController, 1).whenHeld(new DriveOneModule(mTest, () -> myController.getLeftX(),
+        //         () -> myController.getLeftY(), () -> myController.getRightX()));
+        FullSwerveDrive driveCommand = new FullSwerveDrive(m_SwerveBase, () -> -myController.getLeftX(),
+                () -> myController.getLeftY(), () -> -myController.getRightX());
+        m_SwerveBase.setDefaultCommand(driveCommand);
+
     }
 
     /**
@@ -44,6 +82,10 @@ private void configureButtonBindings() {
      */
     public Command getAutonomousCommand() {
         // An ExampleCommand will run in autonomous
-        return m_autoCommand;
+        // return m_autoCommand;
+        // mTest.setDesiredState(Double);
+        // return new AutoTestSequence(mTest2, mTest, 0.2);
+        return new Turn(m_SwerveBase, 50);
+        // return null;
     }
 }

src/main/java/frc/robot/commands/AutoTestSequence.java

@@ -0,0 +1,26 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.SwerveModule;
+
+public class AutoTestSequence extends SequentialCommandGroup {
+    /**
+     * Creates a new AutoTestSequence.
+     */
+    private final SwerveModule m_swerveModule;
+    private final SwerveModule m_swerveModule2;
+    private final double m_speed;
+
+    public AutoTestSequence(SwerveModule module, SwerveModule module2, double speed) {
+        // Add your commands in the super() call, e.g.
+        // super(new FooCommand(), new BarCommand());
+        super();
+        m_swerveModule = module;
+        m_swerveModule2 = module2;
+        m_speed = speed;
+        // new Turn(m_swerveModule, m_speed),
+        // new Turn(m_swerveModule2, m_speed));
+
+    }
+
+}