Merge pull request #10 from Decatur-High-GlobalDynamics/testing

ElevatorSysIDConfig.json

@@ -0,0 +1,42 @@
+{
+  "counts per rotation": 2048.0,
+  "encoder type": "Built-in",
+  "encoding": false,
+  "gearing denominator": 20.0,
+  "gearing numerator": 1.0,
+  "gyro": "Analog Gyro",
+  "gyro ctor": "0",
+  "is drivetrain": false,
+  "motor controllers": [
+    "TalonFX",
+    "TalonFX"
+  ],
+  "number of samples per average": 1,
+  "primary encoder inverted": false,
+  "primary encoder ports": [
+    0,
+    1
+  ],
+  "primary motor ports": [
+    18,
+    17
+  ],
+  "primary motors inverted": [
+    false,
+    true
+  ],
+  "secondary encoder inverted": false,
+  "secondary encoder ports": [
+    2,
+    3
+  ],
+  "secondary motor ports": [
+    2,
+    3
+  ],
+  "secondary motors inverted": [
+    false,
+    false
+  ],
+  "velocity measurement period": 100
+}

src/main/java/frc/robot/Ports.java

@@ -14,6 +14,7 @@ public class Ports {
    public final static int ELEVATOR_POTENTIOMETER = 2;
    public final static int ELEVATOR_LIMIT_SWITCH = 9;
    public final static Port NAV_X = Port.kMXP; // Need to check this
+   public final static int GYRO = 0; // CHECK THIS!!!
 
    public class SwervePorts { // TODO: Assign port values for each module
 

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

@@ -7,14 +7,18 @@
 import edu.wpi.first.cameraserver.CameraServer;
 
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.commands.ClawGrabberCommand;
 
 /**
- * The VM is configured to automatically run this class, and to call the functions corresponding to
- * each mode, as described in the TimedRobot documentation. If you change the name of this class or
- * the package after creating this project, you must also update the build.gradle file in the
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the
+ * name of this class or
+ * the package after creating this project, you must also update the
+ * build.gradle file in the
  * project.
  */
 public class Robot extends TimedRobot {
@@ -28,58 +32,78 @@ public class Robot extends TimedRobot {
   public static CTREConfigs ctreConfigs;
 
   /**
-   * This function is run when the robot is first started up and should be used for any
+   * This function is run when the robot is first started up and should be used
+   * for any
    * initialization code.
    */
   @Override
   public void robotInit() {
-    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // Instantiate our RobotContainer. This will perform all our button bindings,
+    // and put our
     // autonomous chooser on the dashboard.
     ctreConfigs = new CTREConfigs();
     m_robotContainer = new RobotContainer();
     CameraServer.startAutomaticCapture();
   }
 
   /**
-   * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
+   * This function is called every 20 ms, no matter the mode. Use this for items
+   * like diagnostics
    * that you want ran during disabled, autonomous, teleoperated and test.
-   *
-   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
-   * SmartDashboard integrated updating.
+   * <p>
+   * This runs after the mode specific periodic functions, but before LiveWindow
+   * and SmartDashboard
+   * integrated updating.
    */
   @Override
   public void robotPeriodic() {
-    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
-    // commands, running already-scheduled commands, removing finished or interrupted commands,
-    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // Runs the Scheduler. This is responsible for polling buttons, adding
+    // newly-scheduled
+    // commands, running already-scheduled commands, removing finished or
+    // interrupted commands,
+    // 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();
   }
 
   /** This function is called once each time the robot enters Disabled mode. */
   @Override
   public void disabledInit() {
-    if(RobotContainer.instance != null)
+    if (RobotContainer.instance != null)
       // RobotContainer.instance.drivetrain.driveStraight = false;
-    
+
       isEnabled = false;
-      isTest = false;
+    isTest = false;
   }
 
   @Override
-  public void disabledPeriodic() {}
+  public void disabledPeriodic() {
+  }
 
-  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+  /**
+   * This autonomous runs the autonomous command selected by your
+   * {@link RobotContainer} class.
+   */
   @Override
+
   public void autonomousInit() {
-    // m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+    m_robotContainer.swerveDrive.setAngleOffsets(
+        SmartDashboard.getBoolean("Invert Swerve", false));
+
+    m_robotContainer.swerveDrive.zeroGyro();
+
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
     System.out.println("Auto Command: " + m_autonomousCommand);
-    // if(m_autonomousCommand == null) m_autonomousCommand = new NormalAutoCommand();
+    // if(m_autonomousCommand == null) m_autonomousCommand = new
+    // NormalAutoCommand();
 
-    // new ClawGrabberCommand(Value.kForward, RobotContainer.instance.clawIntake, true);
+    // new ClawGrabberCommand(Value.kForward, RobotContainer.instance.clawIntake,
+    // true);
     // isEnabled = true;
-    // RobotContainer.instance.elevator.resetTarget(); TODO: uncomment with working swerve
-
+    // RobotContainer.instance.elevator.resetTarget(); TODO: uncomment with working
+    // swerve
+
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
@@ -88,7 +112,8 @@ public void autonomousInit() {
 
   /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {}
+  public void autonomousPeriodic() {
+  }
 
   @Override
   public void teleopInit() {
@@ -97,39 +122,51 @@ public void teleopInit() {
     // continue until interrupted by another command, remove
     // this line or comment it out.
     // if (m_autonomousCommand != null) {
-    //   m_autonomousCommand.cancel();
+    // m_autonomousCommand.cancel();
     // }
     CommandScheduler.getInstance().cancelAll();
 
-    // RobotContainer.instance.elevator.setSpeed(0); TODO: uncomment with working swerve
+    m_robotContainer.swerveDrive.setAngleOffsets(
+        SmartDashboard.getBoolean("Invert Swerve", false));
+
+    // RobotContainer.instance.elevator.setSpeed(0); TODO: uncomment with working
+    // swerve
     // RobotContainer.instance.elevator.resetTarget();
 
-    // new ClawGrabberCommand(Value.kForward, RobotContainer.instance.clawIntake, true);
+    // new ClawGrabberCommand(Value.kForward, RobotContainer.instance.clawIntake,
+    // true);
     isEnabled = true;
   }
 
   /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+  }
 
   @Override
   public void testInit() {
     // Cancels all running commands at the start of test mode.
     CommandScheduler.getInstance().cancelAll();
 
+    m_robotContainer.swerveDrive.setAngleOffsets(
+        SmartDashboard.getBoolean("Invert Swerve", false));
+
     isEnabled = true;
     isTest = true;
   }
 
   /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {}
+  public void testPeriodic() {
+  }
 
   /** This function is called once when the robot is first started up. */
   @Override
-  public void simulationInit() {}
+  public void simulationInit() {
+  }
 
   /** This function is called periodically whilst in simulation. */
   @Override
-  public void simulationPeriodic() {}
+  public void simulationPeriodic() {
+  }
 }