Andrew t branch

.OutlineViewer/outlineviewer.json

@@ -0,0 +1,8 @@
+{
+  "Connections": {
+    "open": true
+  },
+  "Retained Values": {
+    "open": false
+  }
+}

simgui-ds.json

@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,16 @@
+{
+  "HALProvider": {
+    "Other Devices": {
+      "SPARK MAX [10]": {
+        "header": {
+          "open": true
+        }
+      }
+    }
+  },
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo"
+    }
+  }
+}

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

@@ -1,4 +1,4 @@
-// Copyright (c) FIRST and other WPILib contributors.
+ // 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.
 
@@ -13,21 +13,18 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-  public static final int FrontRightID = 0;
-  public static final int FrontLeftID = 3;
-  public static final int BackRightID = 5;
-  public static final int BackLeftID = 2;
-
+
+
+  //general xbox pins
   public static final int XBOX_R_XAXIS = 4;
   public static final int XBOX_R_YAXIS = 5;
   public static final int XBOX_L_XAXIS = 0;
   public static final int XBOX_L_YAXIS = 1;
-  public static final int XBOX_pin = 0;
   public static final int Xbox_LT = 2;
   public static final int Xbox_RT = 3;
 
-
-  //Controller Button IDs
+  //We need some flight stick constants I guess
+  //Controller Button IDs CONTROLLER 1 (port 0)
   public static final int Xbox_Button_A = 1;
   public static final int Xbox_Button_B = 2;
   public static final int Xbox_Button_X = 3;
@@ -36,44 +33,58 @@ public final class Constants {
   public static final int Xbox_Button_RB = 6;
   public static final int Xbox_Button_LS = 9;
   public static final int Xbox_Button_RS = 10;
-
-  //Flightstick ID's
-  public static final int kflightStick = 1;
-  public static final int Flight_Stick_X = 0;
-  public static final int Flight_Stick_Y = 1;
-  public static final int Flight_Stick_Z = 2;
-
-  //We need some flight stick constants I guess
+
+  //Controller Button IDs FOR CONTROLLER 2 (port 1)
+  public static final int Xbox_Button_AX2 = 1;
+  public static final int Xbox_Button_BX2 = 2;
+  public static final int Xbox_Button_XX2 = 3;
+  public static final int Xbox_Button_YX2 = 4;
+  public static final int Xbox_Button_LBX2 = 5;
+  public static final int Xbox_Button_RBX2 = 6;
+  public static final int Xbox_Button_LSX2 = 9;
+  public static final int Xbox_Button_RSX2 = 10;
+
+  public static final int XBOX_R_XAXISX2 = 4;
+  public static final int XBOX_R_YAXISX2 = 5;
+  public static final int XBOX_L_XAXISX2 = 0;
+  public static final int XBOX_L_YAXISX2 = 1;
+  public static final int XBOX_pinX2 = 0;
+  public static final int Xbox_LTX2 = 2;
+  public static final int Xbox_RTX2 = 3;
+
   //Also some motor id constant for the intake
+
   public static final int deviceIdIntakeM1 = 9;
   public static final int deviceIdIntakeM2 = 10;
-
-  public static final double intakeMotorSpeed = .4;
+  public static final double intakeMotorSpeed = .2;
   public static final int ampSpike= 8;
   public static final int logitechPort6 = 6;
   public static final int logitechPort7 = 7;
   //Controller Stick Constants
-
-
   public static final int kDriverControllerPort = 0;
-
-
-  public static class OperatorConstants {
-    public static final int kDriverControllerPort = 0;
-
-  }
-
   public static final int talonArmPivot = 1;
   public static final int talonLift1 = 2;
   public static final int talonLift2 = 3;
   public static final int extensionMotorID = 0;
+  //arm constant
+  public static final int armMotorID = 7;
+  public static final double armFrontEncoderLimit = 30000;
+  public static final double armBackEncoderLimit = -0;
+  public static final double kArmP = 0.00005;
+  public static final double kArmI = 0.00000001;
+  public static final double kArmD = 0;
+  public static final double armGearRatio = 75;// 75:1
+  public static final double kArmPID = 30000;
 
   //DriveTrain Motor ID's
-  public static final int FRid = 0;
-  public static final int BLid = 1;
-  public static final int FLid = 3;
-  public static final int BRid = 6;
-
+  public static final int FRid = 2;
+  public static final int BLid = 3;
+  public static final int FLid = 0;
+  public static final int BRid = 5;
+  // public static final int FRid = 0;
+  // public static final int BLid = 1;
+  // public static final int FLid = 3;
+  // public static final int BRid = 6;
   //DriveTrain Constants
   public static final double turnAdjustment = 0.5;
   public static final double powerAdjustment = 0.5;
@@ -85,24 +96,45 @@ public static class OperatorConstants {
   public static final int xLeftid = 0;
   public static final int yLeftid = 1;
 
+ //Controller 2 ID's
+ public static final int xBoxControllerid2 = 1;
+ public static final int xRightidX2 = 4;
+ public static final int yRightidX2 = 5;
+ public static final int xLeftidX2 = 0;
+ public static final int yLeftidX2 = 1;
+
   // turn PID constants
-  public static final double turnkP = 0.005;
-  public static final double turnkI = 0.0008;
-  public static final double turnkD = 0.007;
+  public static final double turnkP = 0.0034;
+  public static final double turnkI = 0.0328;
+  public static final double turnkD = 0;
+  public static final double maximumDriveError = 1000.0;
 
   // balance PID constants
-  public static final double balancekP = 0.007;
+  public static final double balancekP = 0.001;
   public static final double balancekI = 0;
   public static final double balancekD = 0;
+  public static final double levelConstant = 2;
 
   // auton drive PID constants
   //public static final int kTimeoutMs = 20;
   public static final double driveAdjustment = 0.85;
   public static final int kPIDLoopIdx = 0;//run primary loop
-  public static final double kF = 0;
-  public static final double kP = 0.00005;
-  public static final double kI = 0.0;
-  public static final double kD = 0;
-
-  public static final double IactZone = 0;
-}
+  public static final double kDriveF = 0;
+  public static final double kDriveP = 0.00005;
+  public static final double kDriveI = 0.0;
+  public static final double kDriveD = 0;
+
+  public static final double IactZone = 0;//was 0.5
+
+  //pi constants
+  public static final double CAMERA_HEIGHT_METERS = 0;
+  public static final double TARGET_HEIGHT_METERS = 0;
+  public static final double CAMERA_PITCH_RADIANS = 0;
+
+  //limelight constants
+  public static final double goalHeight = 107; //what are the units????
+  public static final double limeLightHeight = 32;
+  public static final double limeLightInitAngle = 0;
+  public static double balanceTarget = 0.9;
+  public static final double cameraOffset = 0; // degrees horizontal angle
+}

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

@@ -31,7 +31,7 @@ public void robotInit() {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
-    m_robotContainer.LimeLightSubsystem.getAprilId();
+    m_robotContainer.armSubsystem.resetArmEncoders();
   }
 
   /**
@@ -49,9 +49,28 @@ public void robotPeriodic() {
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
     // SmartDashboard.putBoolean("Has Target", m_robotContainer.rasberryPiCamera.getHasTarget());
-    // SmartDashboard.putNumber("Fiducial ID", m_robotContainer.rasberryPiCamera.aprilTagID());
+    // SmartDashboard.putNumber("Fiducial ID", m_robotContainer.rasberryPiCamera.getaprilTagID());
+
     // SmartDashboard.putNumber("Target Area", m_robotContainer.rasberryPiCamera.getTargetArea());
     // SmartDashboard.putNumber("Target Pose", m_robotContainer.rasberryPiCamera.getTargetPose());
+    // SmartDashboard.putNumber("Target Yaw", m_robotContainer.rasberryPiCamera.getTargetYaw());
+    // SmartDashboard.putNumber("Target Skew", m_robotContainer.rasberryPiCamera.getTargetSkew());
+    // SmartDashboard.putNumber("Target Pitch", m_robotContainer.rasberryPiCamera.getTargetPitch());
+    // SmartDashboard.putNumber("Target Distance", m_robotContainer.rasberryPiCamera.targetDistance());
+
+    SmartDashboard.putNumber("V Angle", m_robotContainer.LimeLightSubsystem.getV_angle());
+    SmartDashboard.putNumber("H Angle", m_robotContainer.LimeLightSubsystem.getH_angle());
+    SmartDashboard.putNumber("Z Distance", m_robotContainer.LimeLightSubsystem.calculateXdistance());
+    SmartDashboard.putNumber("X Distance", m_robotContainer.LimeLightSubsystem.calculateZdistance());
+    SmartDashboard.putNumberArray("Bot Pose", m_robotContainer.LimeLightSubsystem.getBotPose());
+    SmartDashboard.putNumber("AprilTag ID", m_robotContainer.LimeLightSubsystem.getaprilTagID());
+    SmartDashboard.putNumber("NavX Yaw", m_robotContainer.navX.getYaw());
+    SmartDashboard.putNumber("drive output", m_robotContainer.driveTrain.getDriveOutput());
+    SmartDashboard.putNumber("getTurn output", m_robotContainer.driveTrain.getTurnOutput());
+    SmartDashboard.putNumber("Right Encoder avg", m_robotContainer.driveTrain.getRightEncoders());
+    SmartDashboard.putNumber("Left Encoder avg", m_robotContainer.driveTrain.getLeftEncoders());
+    SmartDashboard.putBoolean("navx connected", m_robotContainer.navX.isConnected());
+    SmartDashboard.putNumber("Arm Position", m_robotContainer.armSubsystem.getArmPosition());
   }
 
   /** This function is called once each time the robot enters Disabled mode. */
@@ -83,6 +102,11 @@ public void autonomousPeriodic() {
     SmartDashboard.putNumber("Y-Displacement:", (double)m_robotContainer.navX.getDisplacementY());
     SmartDashboard.putNumber("X Position:", m_robotContainer.navX.getXPosition());
     SmartDashboard.putNumber("Y Position:", m_robotContainer.navX.getYPosition());
+    SmartDashboard.putNumber("FR: Position", m_robotContainer.driveTrain.getFRid());
+    SmartDashboard.putNumber("FL: Position", m_robotContainer.driveTrain.getFLid());
+    SmartDashboard.putNumber("BR: Position", m_robotContainer.driveTrain.getBRid());
+    SmartDashboard.putNumber("BL: Position", m_robotContainer.driveTrain.getBLid());
+    SmartDashboard.putNumber("Arm Position", m_robotContainer.armSubsystem.getArmPosition());
 
   }
 
@@ -108,6 +132,11 @@ public void teleopPeriodic() {
     SmartDashboard.putNumber("Y-Displacement:", (double)m_robotContainer.navX.getDisplacementY());
     SmartDashboard.putNumber("X Position:", m_robotContainer.navX.getXPosition());
     SmartDashboard.putNumber("Y Position:", m_robotContainer.navX.getYPosition());
+    SmartDashboard.putNumber("FR: Position", m_robotContainer.driveTrain.getFRid());
+    SmartDashboard.putNumber("FL: Position", m_robotContainer.driveTrain.getFLid());
+    SmartDashboard.putNumber("BR: Position", m_robotContainer.driveTrain.getBRid());
+    SmartDashboard.putNumber("BL: Position", m_robotContainer.driveTrain.getBLid());
+    SmartDashboard.putNumber("Arm Position", m_robotContainer.armSubsystem.getArmPosition());
   }
 
   @Override