Merge branch 'master' into GalacticSearch

PathWeaver/Paths/Barrel

@@ -1,15 +1,12 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Name
 1.1943219738452617,-2.286,1.0,0.0,true,
-3.396233460027056,-2.2025005475745667,1.6761741931327716,0.12370289248212352,true,
-4.466263479997423,-3.0436802164530046,0.15462861560265395,-1.1071408877150035,true,
-3.618898666494878,-3.6374541003671967,-1.2555843586935511,0.21029491721960936,true,
-3.4951957740127546,-2.530313212652193,1.2184734909489143,0.8226242350061197,true,
-6.105326805385555,-2.208685692198673,1.5215455775301168,0.4824412806802809,true,
-6.847544160278296,-1.5035792050505696,0.08040688011338037,0.6679956194034656,true,
-6.1362525285060885,-0.8788795980158475,-1.261769503317658,0.012370289248212263,true,
-5.4868123429749405,-1.7757255685112412,0.42058983443921694,-0.9030311151194992,true,
-6.760952135540809,-3.3405671584101007,1.7689513624943638,-0.7174767763963148,true,
-8.096943374347742,-3.3900483154029506,0.6061441731624058,0.4824412806802809,true,
-8.053647361978998,-2.4375360432906006,-0.7360322102686343,0.5690333054177668,true,
-5.598144946208851,-2.190130258326354,-5.115114604135798,0.06803659086516767,true,
-1.194,-2.060242221220125,-1.0700300199703663,0.0061851446241063535,false,
+3.1673831089351285,-2.3571291631772207,0.24740578496424614,-0.04948115699284905,true,
+4.237413128905495,-2.7839041422405457,0.4391452683115382,-0.7793282226373774,true,
+3.736416414352895,-3.55086207562971,-1.1442517554596408,0.20410977259550345,true,
+3.581787798750241,-2.6354606712619977,0.6679956194034653,0.6679956194034657,true,
+6.476435482831926,-1.7571701346389224,0.5752184500418727,0.8844756812471817,true,
+6.117697094633767,-0.7737321394060425,-1.168992333956064,-0.012370289248212263,true,
+5.802254718804354,-1.95509476261032,0.6432550409070421,-1.2184734909489143,true,
+7.020728209753269,-3.3096414352895698,1.6019524576434971,-0.8164390903820133,true,
+8.239201700702182,-3.0746059395735355,0.2102949172196098,0.5009967145525991,true,
+7.608316949043354,-2.286,-1.7503959286220452,0.73,true,

PathWeaver/Paths/BarrelStart

@@ -1,14 +1,14 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Name
 1.1943219738452617,-2.286,1.0,0.0,true,
-3.396233460027056,-2.2025005475745667,1.6761741931327716,0.12370289248212352,true,
-4.602336661727758,-2.9818287702119433,0.15462861560265395,-1.1071408877150035,true,
-3.656009534239515,-3.810638149842169,-1.2555843586935511,0.21029491721960936,true,
-3.3714928815306315,-2.5241280680280873,1.2184734909489143,0.8226242350061197,true,
+3.396233460027056,-2.2025005475745667,1.5833970237711785,0.02474057849642497,true,
+4.540485215486696,-3.006569348708368,0.15462861560265395,-1.1071408877150035,true,
+3.451899761644012,-3.60034323262256,-0.7607727887650584,0.7051064871481025,true,
+3.5941580879984536,-2.4808320556593437,1.2184734909489143,0.8226242350061197,true,
 6.105326805385555,-2.208685692198673,1.5215455775301168,0.4824412806802809,true,
-7.144431102235392,-1.4231723249371897,0.08040688011338037,0.6679956194034656,true,
-6.17954854087483,-0.6376589576757069,-0.7793282226373774,-0.17318404947497257,true,
-5.295072859627648,-1.6829483991496488,0.42058983443921694,-0.9030311151194992,true,
-6.575397796817625,-3.4766403401404364,1.7689513624943638,-0.7174767763963148,true,
-8.22683141145397,-3.557047220253817,0.6061441731624058,0.4824412806802809,true,
-8.301053146943245,-2.2396114153192035,-0.5999590285382981,0.3587383881981576,true,
-5.598144946208851,-2.190130258326354,-5.115114604135798,0.06803659086516767,true,
+6.971247052760418,-1.2623585647104294,-0.3216275204535206,1.7070999162533012,true,
+6.154807962378405,-0.6933252592926624,-0.7793282226373774,-0.17318404947497257,true,
+5.523923210719577,-1.4602831926818267,0.42058983443921694,-0.9030311151194992,true,
+6.612508664562261,-3.303456290665464,1.7689513624943638,-0.7174767763963148,true,
+8.239201700702182,-3.4766403401404364,0.9648825613605609,0.8968459704953937,true,
+8.381460027056624,-2.2334262706950976,-0.5442927269213422,0.2474057849642466,true,
+5.598144946208851,-2.12,-5.115114604135798,0.06803659086516767,true,

PathWeaver/Paths/Bounce1

@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+1.2190625523416865,-2.295277716936159,2.4431321265219355,-0.012370289248212707,true,
+2.295277716936159,-0.6685846807962377,-0.061851446241061314,0.5442927269213425,true,

PathWeaver/Paths/Bounce2

@@ -0,0 +1,5 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+2.295,-0.7613618501578302,-0.2474057849642466,-2.133874895316627,true,
+3.804453005218064,-3.7920827159698507,1.6328781807640258,0.25359092958835294,true,
+4.521929781614379,-2.9447179024673065,-0.04948115699285083,0.7422173548927398,true,
+4.521929781614379,-0.68095497004445,-0.04329601236874314,0.377293822070476,true,

PathWeaver/Paths/Bounce3

@@ -0,0 +1,5 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+4.589966372479545,-0.68095497004445,0.03711086774463812,-0.8349945242543321,true,
+5.622885524705275,-3.68075011273594,2.770944791599561,0.15462861560265395,true,
+6.692915544675642,-2.882866456226244,0.12988803710623031,0.9339568382400305,true,
+6.8537293049024015,-0.6933252592926624,-0.06803659086516767,0.5937738839141918,true,

PathWeaver/Paths/Galactic Search ABLUE

@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+0.0,0.0,10.0,0.0,true,
+10.0,-10.0,0.0,-10.0,true,

PathWeaver/Paths/Slalom

@@ -1,14 +1,11 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Name
-1.1633962507247309,-3.693120401984152,2.30034323262256,0.058245899632803244,true,
-2.85812587772982,-2.412795464794176,0.8102539457579074,0.6494401855311474,true,
-4.571410938607228,-2.004575919603169,2.022542292082716,0.05566630161695518,true,
-6.105326805385555,-2.3509440185531143,0.8734927301599473,-0.5329783419602319,false,
-7.150616246859498,-3.6683798234877276,1.1442517554596412,-0.7174767763963148,true,
-8.202090832957547,-3.618898666494878,0.6741807640275734,0.6370698962829353,true,
-8.43712632867358,-2.456091477162919,-0.8721053919989696,1.107140887715003,true,
-7.076394511370224,-2.4808320556593437,-0.5195521484249168,-1.2122883463248084,true,
-6.204289119371255,-3.581787798750241,-0.9215865489918196,-0.6185144624106163,true,
-4.7384098434580935,-3.903415319203762,-1.0282413438142206,-0.047219421650161054,false,
-3.1179019519422786,-3.674564968111833,-1.3854723957997805,0.5319224376731304,true,
-2.1282788120852927,-2.7839041422405457,-1.2679546479417638,1.6019524576434963,true,
-0.5572520775623268,-2.140649101333505,-0.7236619210204216,0.04948115699284905,true,
+1.2,-3.85,2.084900856793146,0.0,true,
+2.8890516008503506,-2.536498357276299,1.3978426850479937,0.1484434709785476,true,
+6.278510854860529,-2.9199773239708815,1.107140887715004,-1.9050245442246987,true,
+7.98561077111383,-3.649824389615409,1.1628071893319598,0.45151555755975004,true,
+8.282497713070926,-2.7282378406235903,-0.6370698962829353,0.797883656509695,true,
+7.360911164079107,-2.8395704438575016,-0.36492353282226375,-0.7731430780132702,true,
+6.569212652193518,-3.625083811118984,-0.9958082844810923,-0.5752184500418731,true,
+4.107525091799265,-4.039488500934097,-2.0287274367068218,-0.14225832635444213,true,
+2.412795464794176,-2.9756436255878373,-0.5628481607936611,1.230843780197127,true,
+0.8232132963988917,-2.3262034400566898,-1.1380666108355342,-0.043296012368743586,true,

PathWeaver/Paths/SlalomClose

@@ -0,0 +1,12 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Name
+1.2,-3.55,3.847667074663401,0.07954480448366974,true,
+2.790089286864652,-2.54,1.861728531855955,0.1519451137022485,true,
+5.171369967145526,-2.369499452425433,2.3503549571603415,0.024740578496424526,true,
+7.020728209753269,-3.4890106293886487,0.8411796688784383,-0.7978836565096956,true,
+8.171165109837014,-3.458084906268118,0.6061441731624058,0.5195521484249181,true,
+8.301053146943245,-2.6911269728789535,-0.5319224376731313,0.9648825613605616,true,
+7.32380029633447,-2.752978419120015,-0.4329601236874323,-0.5071818591767059,true,
+6.457880048959607,-3.5632323648779227,-0.6803659086516785,-0.2597760742124593,true,
+4.658002963344714,-3.6683798234877276,-2.968869419570958,-0.12988803710622943,true,
+2.5612389357727237,-3.0374950718288987,-0.40203440056690054,1.1813626232042773,true,
+0.7984727179024672,-2.3,-0.7669579333891645,-0.02474057849642497,false,

PathWeaver/pathweaver.json

@@ -1,9 +1,9 @@
 {
   "lengthUnit": "Meter",
   "exportUnit": "Always Meters",
-  "maxVelocity": 2.7,
-  "maxAcceleration": 4.0,
+  "maxVelocity": 2.6,
+  "maxAcceleration": 2.7,
   "wheelBase": 0.648,
-  "gameName": "Galactic Search B",
+  "gameName": "Galactic Search A",
   "outputDir": ".."
-}
+}

src/main/deploy/Robot Data - Distances.csv

@@ -1,7 +1,16 @@
 Distance (in),Hood Ext. (u),Drum Velocity (u/ds),Center Displacment (deg)
-70,20,7000,2.5,
-127,27,8467,2,
-223,31.25,10398,2.75,
-272,32.4,11776,2.5,
-342,33,13733,2,
-458,30.5,17000,0,
+0,16,12000,1,
+65.9,16,12000,1,
+103,19,12000,1,
+126.6,20.28,12000,1.5,
+156.6,28,12000,1.5,
+174,28,12000,1.5,
+178,28,12000,1.3
+180,28.5,12000,1.3,
+185.85,28.5,12000,1.3,
+187,28.5,12000,1.3
+200,28.4,12000,1.3
+231,28.4,12000,1.8,
+245,28.8,12000,1.8,
+262,28.8,12000,1.8,
+999,28.8,12000,1.8,

src/main/deploy/Robot Data - DistancesOLD.csv

@@ -1,8 +1,63 @@
 Distance (in),Hood Ext. (u),Drum Velocity (u/ds),Center Displacment (deg)
-1,10,7769,0
-70,21,7769,0, check
-100,24,8580,0
-145,28,9390,0
-230,31,11010,0,Add a 270
-397,33,14250,0
-415,33,14452,0
+LOBING
+0,16,6000,1,
+65.9,16,6000,1,
+126.6,20.25,7000,0.95,
+180,21,7300,1,
+185.85,23.5,7500,1.625,
+245,26,9000,1,
+262,28,9000,1.25,
+999,28,9000,1.25,
+
+LASER
+0,16,6000,1,
+65.9,16,6000,1,
+126.6,20.25,7000,0.95,
+185.85,30,13000,1,
+245,31,18000,1,
+250,31,18000,1,
+999,31,18000,1,
+
+IN BETWEEN
+0,16,6000,1,
+65.9,16,6000,1,
+126.6,20.25,7000,0.95,
+180,24,8000,1,
+185.85,26.5, 9000,1.625,
+245,27,11000,1,
+262,27,11000,1.25,
+999,27,11000,1.25,
+
+HOLD
+0,16,6000,1,
+65.9,16,6000,1,
+103,19,6600,1,
+126.6,20.25,7000,1.5,
+156.6,22,7500,1.5,
+174,23,8000,1.5,
+180,23,8000,1.3,
+185.85,26.5,9000,1.3,
+231,28.8,9500,1.8,
+245,28.8,9500,1.8,
+262,28.8,9500,1.8,
+999,28.8,9500,1.8,
+
+
+
+
+
+
+Distance (in),Hood Ext. (u),Drum Velocity (u/ds),Center Displacment (deg)
+0,16,6000,1,
+65.9,16,6000,1,
+103,19,6600,1,
+126.6,20.25,7000,1.5,
+156.6,23,7500,1.5,
+174,28.5,13000,1.5,
+180,28.5,13000,1.3,
+185.85,28.5,13000,1.3,
+190,28.5,13000,1.9
+231,28.5,13000,1.8,
+245,28.8,13000,1.8,
+262,28.8,13000,1.8,
+999,28.8,13000,1.8,

src/main/driverStation/GOAT DRIVERSTATION.json

@@ -624,6 +624,12 @@
                   "_title": "DigitalInput[12]/Value",
                   "Colors/Color when true": "#7CFC00FF",
                   "Colors/Color when false": "#8B0000FF"
+                },
+                {
+                  "_type": "Speed Controller",
+                  "_source0": "network_table:///LiveWindow/Ungrouped/Talon FX [15]",
+                  "_title": "Talon FX [15]",
+                  "Visuals/Orientation": "HORIZONTAL"
                 }
               ]
             }
@@ -1003,6 +1009,138 @@
           }
         }
       }
+    },
+    {
+      "title": "Tab 5",
+      "autoPopulate": false,
+      "autoPopulatePrefix": "",
+      "widgetPane": {
+        "gridSize": 128.0,
+        "showGrid": true,
+        "hgap": 16.0,
+        "vgap": 16.0,
+        "tiles": {
+          "3,0": {
+            "size": [
+              3,
+              3
+            ],
+            "content": {
+              "_type": "Camera Stream",
+              "_source0": "camera_server://limelight",
+              "_title": "limelight",
+              "Crosshair/Show crosshair": true,
+              "Crosshair/Crosshair color": "#FFFFFFFF",
+              "Controls/Show controls": true,
+              "Controls/Rotation": "NONE",
+              "compression": -1.0,
+              "fps": -1,
+              "imageWidth": 0,
+              "imageHeight": 0
+            }
+          },
+          "7,1": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/Fire Angle CSV",
+              "_title": "/SmartDashboard/Fire Angle CSV"
+            }
+          },
+          "7,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/Drum Velocity CSV",
+              "_title": "/SmartDashboard/Drum Velocity CSV"
+            }
+          },
+          "6,1": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/Distance to Target",
+              "_title": "/SmartDashboard/Distance to Target"
+            }
+          },
+          "3,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/Distance to Target",
+              "_title": "/SmartDashboard/Distance to Target"
+            }
+          },
+          "0,0": {
+            "size": [
+              2,
+              2
+            ],
+            "content": {
+              "_type": "Graph",
+              "_source0": "network_table:///SmartDashboard/Drum Velocity",
+              "_title": "/SmartDashboard/Drum Velocity",
+              "Graph/Visible time": 30.0,
+              "Y-axis/Automatic bounds": true,
+              "Y-axis/Upper bound": 1.0,
+              "Y-axis/Lower bound": -1.0,
+              "Visible data/SmartDashboard/Drum Velocity": true
+            }
+          },
+          "6,2": {
+            "size": [
+              4,
+              2
+            ],
+            "content": {
+              "_type": "Graph",
+              "_source0": "network_table:///SmartDashboard/Drum Velocity",
+              "_title": "/SmartDashboard/Drum Velocity",
+              "Graph/Visible time": 30.0,
+              "Y-axis/Automatic bounds": true,
+              "Y-axis/Upper bound": 1.0,
+              "Y-axis/Lower bound": -1.0,
+              "Visible data/SmartDashboard/Drum Velocity": true
+            }
+          },
+          "4,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/Turret Angle Raw",
+              "_title": "/SmartDashboard/Turret Angle Raw"
+            }
+          },
+          "6,0": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Boolean Box",
+              "_source0": "network_table:///SmartDashboard/Drum Ready",
+              "_title": "/SmartDashboard/Drum Ready",
+              "Colors/Color when true": "#7CFC00FF",
+              "Colors/Color when false": "#8B0000FF"
+            }
+          }
+        }
+      }
     }
   ],
   "windowGeometry": {

src/main/java/frc4388/robot/Constants.java

@@ -36,7 +36,7 @@ public final class Constants {
         public static final boolean isRightMotorInverted = true;
         public static final boolean isLeftMotorInverted = false;
         public static final boolean isRightArcadeInverted = false;
-        public static final boolean isAuxPIDInverted = true;
+        public static final boolean isAuxPIDInverted = false;
 
         /* Drive Configuration */
         public static final int DRIVE_TIMEOUT_MS = 30; // Use for all motor config
@@ -112,7 +112,8 @@ public final class Constants {
   
     public static final class ShooterConstants {
         /* Motor IDs */
-        public static final int SHOOTER_FALCON_ID = 8;
+        public static final int SHOOTER_FALCON_BALLER_ID = 8;
+        public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 15;
         public static final int SHOOTER_ANGLE_ADJUST_ID = 10;
         public static final int SHOOTER_ROTATE_ID = 9;
 
@@ -120,7 +121,7 @@ public final class Constants {
         public static final int SHOOTER_SLOT_IDX = 0;
         public static final int SHOOTER_PID_LOOP_IDX = 1;
         public static final int SHOOTER_TIMEOUT_MS = 30;
-        public static final Gains DRUM_SHOOTER_GAINS = new Gains(1.5, 0.0, 100, 0.055, 0, 1.0);
+        public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.34, 0.0, 0.0, 0.055, 0, 1.0); //Ff was 0.055
         //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.55, 0.0, 100, 0.0, 0, 1.0);
         public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0);
         public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3);
@@ -156,7 +157,7 @@ public final class Constants {
     }
   
     public static final class LevelerConstants {
-        public static final int LEVELER_CAN_ID = 15;
+        public static final int LEVELER_CAN_ID = 30;
     }
 
     public static final class IntakeConstants {;
@@ -171,7 +172,7 @@ public final class Constants {
         public static final int STORAGE_CAN_ID = 11;
         public static final double STORAGE_PARTIAL_BALL = 2;
         public static final double STORAGE_FULL_BALL = 7;
-        public static final double STORAGE_SPEED = 0.5;
+        public static final double STORAGE_SPEED = 0.75;
         public static final double STORAGE_TIMEOUT = 3000;
 
         /* Storage Characteristics */
@@ -211,10 +212,10 @@ public final class Constants {
     
     public static final class VisionConstants {
         public static final double FOV = 29.8; //Field of view of limelight
-        public static final double TARGET_HEIGHT = 71.5;
+        public static final double TARGET_HEIGHT = 67.5;
         public static final double LIME_ANGLE = 24.7;
         public static final double TURN_P_VALUE = 0.8;
-        public static final double X_ANGLE_ERROR = 1.3;
+        public static final double X_ANGLE_ERROR = 0.5;
         public static final double MOTOR_DEAD_ZONE = 0.2;
         public static final double DISTANCE_ERROR_EQUATION_M = 1.1279;
         public static final double DISTANCE_ERROR_EQUATION_B = -15.0684;

src/main/java/frc4388/robot/Robot.java

@@ -70,6 +70,7 @@ public class Robot extends TimedRobot {
     /* Builds Autos */
     m_robotContainer.buildAutos();
     SmartDashboard.putString("Is Auto Start?", "NAH");
+    m_robotContainer.m_robotLime.limeOff();
   }
 
   @Override

src/main/java/frc4388/robot/RobotContainer.java

@@ -107,7 +107,7 @@ public class RobotContainer {
     /* Cameras */
     private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40);
     private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40);
-    private final LimeLight m_robotLime = new LimeLight();
+    public final LimeLight m_robotLime = new LimeLight();
 
     /* Controllers */
 
@@ -184,7 +184,7 @@ public class RobotContainer {
         // runs the hood with joystick
         m_robotShooterHood.setDefaultCommand(new RunHoodWithJoystick(m_robotShooterHood, getOperatorController()));
         // moves the drum not
-        m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0), m_robotShooter));
+        m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(12000), m_robotShooter));
         // drives climber with input from triggers on the opperator controller
         m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController()));
         // drives the leveler with an axis input from the driver controller
@@ -192,9 +192,9 @@ public class RobotContainer {
         // continually sends updates to the Blinkin LED controller to keep the lights on
         m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
         // runs the storage not
-        //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage));
         m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage));
-        //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
+        m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime));
+
     }
 
     /**
@@ -205,14 +205,20 @@ public class RobotContainer {
      */
     private void configureButtonBindings() {
         /* Test Buttons */
+
         // A driver test button
+        new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
+            .whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim))
+            .whenReleased(new InterruptSubystem(m_robotShooter))
+            .whenReleased(new InterruptSubystem(m_robotShooterHood));
 
         // B driver test button
         new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
-            .whenPressed(new TurnDegrees(m_robotDrive, 45));
+            .whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive));
+
         // Y driver test button
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-            .whenPressed(new Wait(m_robotDrive, 0, 0));
+            .whenPressed(new InstantCommand(() -> m_robotDrive.runTurningPID(1000), m_robotDrive));
 
         // X driver test button
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
@@ -250,17 +256,19 @@ public class RobotContainer {
             //.whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false);
             //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET));
             //.whenReleased(new RunCommand(() -> m_robotLime.limeOff()));
-            .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage))
+            .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage))
             .whenReleased(new InterruptSubystem(m_robotStorage));
 
         // extends or retracts the extender
         new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
             .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5)))
             .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
-            
+            //.whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(0.2), m_robotShooterHood));
+
         new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
             .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5)))
             .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
+            //.whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood));
 
         // safety for climber and leveler
         new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
@@ -271,10 +279,10 @@ public class RobotContainer {
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whileHeld(new TrackTarget(m_robotShooterAim))
             .whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle())))
-            //.whenPressed(new StoragePrep(m_robotStorage))
-            //.whenReleased(new InterruptSubystem(m_robotStorage))
             .whenReleased(new InstantCommand(() -> m_robotLime.limeOff()));
-            //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(13000)));
+
+            //.whileHeld(new RunCommand(() -> m_robotShooterAim.runshooterRotatePID()));
+            //.whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(11000)));
             //.whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim))
             //.whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30)));
 
@@ -287,6 +295,7 @@ public class RobotContainer {
             .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood));
 
 
+
         //Run drum
         new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, false)
             .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false)
@@ -339,6 +348,51 @@ public class RobotContainer {
 
         m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
 
+        String[] sixBallAutoMiddle0Paths = new String[]{
+            "SixBallMid0"
+        };
+
+        m_sixBallAutoMiddle0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0Paths));
+
+        String[] sixBallAutoMiddle1Paths = new String[]{
+            "SixBallMid1"
+        };
+
+        m_sixBallAutoMiddle1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1Paths));
+
+        String[] slalom = new String[]{
+            "Slalom"
+        };
+
+        m_slalom = new Slalom(m_robotDrive, buildPaths(slalom));
+
+        String[] barrel = new String[]{
+            "BarrelStart"
+        };
+
+        m_barrel = new Barrel(m_robotDrive, buildPaths(barrel));
+
+        String[] barrelStart = new String[]{
+            "Barrel"
+        };
+
+        m_barrelStart = new BarrelStart(m_robotDrive, buildPaths(barrelStart));
+
+        String[] bounce = new String[]{
+            "Bounce1",
+            "Bounce2",
+            "Bounce3",
+            "Bounce4"
+        };
+
+        m_bounce = new Bounce(m_robotDrive, this, buildPaths(bounce));
+
+        String[] barrelMany = new String[]{
+            "BarrelManyWaypoints"
+        };
+
+        m_barrelMany = new BarrelMany(m_robotDrive, buildPaths(barrelMany));
+
         String[] eightBallAutoMiddlePaths = new String[]{
             "EightBallMidComplete"
         };
@@ -349,14 +403,14 @@ public class RobotContainer {
             "DriveOffLineForward"
         };
 
-        m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, buildPaths(driveOffLineForwardPaths));
+        m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, this, buildPaths(driveOffLineForwardPaths));
 
         String[] driveOffLineBackwardPaths = new String[]{
             "DriveOffLineBackward"
         };
 
         m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths));
-        
+
         String[] fiveBallAutoMiddlePaths = new String[]{
             "FiveBallMidComplete"
         };
@@ -367,17 +421,16 @@ public class RobotContainer {
             "SixBallMidComplete",
             "TenBallMidComplete"
         };
-
         m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter,
                 m_robotShooterAim, m_robotDrive,buildPaths(tenBallAutoMiddlePaths));
-      
+
         String[] galacticSearchPaths = new String[]{
               "GSC_ARED",
               "GSC_ABLUE",
               "GSC_BRED",
               "GSC_BBLUE"
         };
-        idenPath();
+
         m_galacticSearch = new GalacticSearch(m_robotLime, m_robotIntake, buildPaths(galacticSearchPaths));
 
     }
@@ -402,8 +455,8 @@ public class RobotContainer {
             //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_sixBallAutoMiddle1.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
-            //return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
-            //return m_driveOffLinfeBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+            return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+            //return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
@@ -417,7 +470,7 @@ public class RobotContainer {
             System.err.println("ERROR");
         }
 
-        return new InstantCommand();    
+        return new InstantCommand();
     }
 
     TrajectoryConfig getTrajectoryConfig() {
@@ -451,7 +504,7 @@ public class RobotContainer {
                 String path = paths[0];
                 String trajectoryJSON = "paths/" + path + ".wpilib.json";
                 Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
-                
+
                 SmartDashboard.putString("trajectoryPath Initial", trajectoryPath.toString());
 
                 Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
@@ -499,7 +552,7 @@ public class RobotContainer {
     }
 
     /**
-     * 
+     *
      */
     public void shiftClimberRachet(boolean state) {
         m_robotClimber.shiftServo(state);

src/main/java/frc4388/robot/commands/auto/BarrelStart.java

@@ -7,8 +7,10 @@
 
 package frc4388.robot.commands.auto;
 
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.RobotContainer;
 import frc4388.robot.subsystems.Drive;
 
@@ -23,7 +25,10 @@ public class BarrelStart extends SequentialCommandGroup {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
     addCommands(
-      paths[0]
+      paths[0],
+      //new Wait(drive, 0.01, 1),
+      new TankDriveVelocity(drive, 5, 5, 1.2) //my life be like oooooo aaaaaa ooooo aaaa
     );
+    
   }
 }

src/main/java/frc4388/robot/commands/auto/Bounce.java

@@ -7,8 +7,12 @@
 
 package frc4388.robot.commands.auto;
 
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.RobotContainer;
 import frc4388.robot.subsystems.Drive;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
@@ -18,7 +22,7 @@ public class Bounce extends SequentialCommandGroup {
   /**
    * Creates a new Bounce.
    */
-  public Bounce(Drive drive, RamseteCommand[] paths) {
+  public Bounce(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
     addCommands(

src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java

@@ -7,8 +7,11 @@
 
 package frc4388.robot.commands.auto;
 
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.RobotContainer;
 import frc4388.robot.subsystems.Drive;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
@@ -18,11 +21,13 @@ public class DriveOffLineForward extends SequentialCommandGroup {
   /**
    * Creates a new DriveOffLineForward.
    */
-  public DriveOffLineForward(Drive drive, RamseteCommand[] paths) {
+  public DriveOffLineForward(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
 
     addCommands(
+      new InstantCommand(() -> drive.switchReversed(true)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
       paths[0]
     );
   }

src/main/java/frc4388/robot/commands/auto/TankDriveVelocity.java

@@ -15,10 +15,10 @@ public class TankDriveVelocity extends CommandBase {
   double m_leftTargetVel;
   double m_rightTargetVel;
 
-  double m_targetTime;
-  double m_firstTimeSec;
-  double m_currentTimeSec;
-  double m_diffSec;
+  long m_targetTime;
+  long m_firstTime;
+  long m_currentTime;
+  long m_diffTime;
 
   /**
    * Creates a new TankDriveVelocity.
@@ -28,24 +28,24 @@ public class TankDriveVelocity extends CommandBase {
     m_drive = subsystem;
     m_leftTargetVel = leftTargetVel;
     m_rightTargetVel = rightTargetVel;
-    m_targetTime = targetTime;
+    m_targetTime = (long) (targetTime * 1000);
     addRequirements(subsystem);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_firstTimeSec = (System.currentTimeMillis() / 1000);
-    m_diffSec = 0;
+    m_firstTime = System.currentTimeMillis();
+    m_diffTime = 0;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_currentTimeSec = (System.currentTimeMillis() / 1000);
-    m_diffSec = m_currentTimeSec - m_firstTimeSec;
+    m_currentTime = System.currentTimeMillis();
+    m_diffTime = m_currentTime - m_firstTime;
     
-    if (m_diffSec < m_targetTime) {
+    if (m_diffTime < m_targetTime) {
       m_drive.tankDriveVelocity(m_leftTargetVel, m_rightTargetVel);
     }
 
@@ -59,7 +59,7 @@ public class TankDriveVelocity extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_diffSec >= m_targetTime) {
+    if (m_diffTime >= m_targetTime) {
       return true;
     }
     return false;

src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java

@@ -29,7 +29,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
+    m_targetGyro = (m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
   }
 
   // Called every time the scheduler runs while the command is scheduled.

src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java

@@ -94,7 +94,7 @@ public class DriveWithJoystick extends CommandBase {
     } 
   */
     
-    m_drive.driveWithInput(-moveOutput, steerOutput);
+    m_drive.driveWithInput(moveOutput, steerOutput * .8);
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc4388/robot/commands/drive/GotoCoordinatesRobotRelative.java

@@ -46,7 +46,7 @@ public class GotoCoordinatesRobotRelative extends SequentialCommandGroup {
     addCommands(  new TurnDegrees(m_drive, m_currentAngle),
                   new Wait(m_drive, 0, 0),
                   new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), 
-                  new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
+                  new TurnDegrees(m_drive, (m_endAngle - m_currentAngle) % 360));
   }
 
   public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) {

src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java

@@ -61,7 +61,7 @@ public class CalibrateShooter extends CommandBase {
   @Override
   public boolean isFinished() {
     if (m_shooterAim.m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get() &&
-      m_shooterHood.m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get()) {
+      m_shooterHood.m_hoodDownLimit.get()) {
       return true;
     }
     return false;

src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java

@@ -35,7 +35,7 @@ public class ShooterGoalPosition extends CommandBase {
   @Override
   public void execute() {
     m_shooter.runDrumShooterVelocityPID(5000);
-    m_hood.runAngleAdjustPID(3);
+    m_hood.runAngleAdjustPID(4);
     m_aim.runshooterRotatePID(-26.5);
   }
 

src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java

@@ -34,9 +34,9 @@ public class ShooterTrenchPosition extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_shooter.runDrumShooterVelocityPID(5000);
-    m_hood.runAngleAdjustPID(3);
-    m_aim.runshooterRotatePID(-26.5);
+    m_shooter.runDrumShooterVelocityPID(5500);
+    m_hood.runAngleAdjustPID(11);
+    //m_aim.runshooterRotatePID(-28);
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java

@@ -35,7 +35,7 @@ public class ShooterVelocityControlPID extends CommandBase {
   @Override
   public void execute() {
     //Tells whether the target velocity has been reached
-    m_actualVel = m_shooter.m_shooterFalcon.getSelectedSensorPosition();
+    m_actualVel = m_shooter.m_shooterFalconLeft.getSelectedSensorPosition();
     m_targetVel = m_shooter.addFireVel();
     double error = m_actualVel - m_targetVel;
     if (Math.abs(error) < ShooterConstants.DRUM_VELOCITY_BOUND){

src/main/java/frc4388/robot/commands/shooter/TrackTarget.java

@@ -70,24 +70,26 @@ public class TrackTarget extends CommandBase {
     yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
 
     // Finding Distance
-    distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
+    distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180));
     realDistance = (1.09 * distance) - 12.8;
 
 
     if (target == 1.0) { // If target in view
       // Aiming Left/Right
       xAngle = xAngle + m_shooter.m_shooterTable.getCenterDisplacement(realDistance);
-      turnAmount = (xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE;
+      turnAmount = ((xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE);
       if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) {
         turnAmount = 0;
       } // Angle Error Zone
       // Deadzones
-      else if (turnAmount > 0 && turnAmount < VisionConstants.MOTOR_DEAD_ZONE) {
-        turnAmount = VisionConstants.MOTOR_DEAD_ZONE;
-      } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) {
-        turnAmount = -VisionConstants.MOTOR_DEAD_ZONE;
+      else if (turnAmount > 0 && turnAmount < 0.1){// VisionConstants.MOTOR_DEAD_ZONE) {
+        turnAmount = 0.1;//VisionConstants.MOTOR_DEAD_ZONE;
+      } else if (turnAmount < 0 && turnAmount > -0.1){//-VisionConstants.MOTOR_DEAD_ZONE) {
+        turnAmount = -0.1;//-VisionConstants.MOTOR_DEAD_ZONE;
       }
+
       m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim);
+      //m_shooterAim.runshooterRotatePID(targetAngle);
 
       SmartDashboard.putNumber("Distance to Target", realDistance);
       SmartDashboard.putNumber("Center Displacement", m_shooter.m_shooterTable.getCenterDisplacement(realDistance));

src/main/java/frc4388/robot/subsystems/Drive.java

@@ -46,6 +46,7 @@ public class Drive extends SubsystemBase {
   public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
   public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
   public static GyroBase m_pigeonGyro;
+  public boolean m_isReversed;
 
   /* Drive objects to manage Drive Train */
   public DifferentialDrive m_driveTrain;
@@ -312,7 +313,7 @@ public class Drive extends SubsystemBase {
   public void passRequiredSubsystem(Pneumatics subsystem, Shooter shooter) {
     m_pneumaticsSubsystem = subsystem;
     m_shooter = shooter;
-    m_orchestra.addInstrument(m_shooter.m_shooterFalcon);
+    m_orchestra.addInstrument(m_shooter.m_shooterFalconLeft);
   }
 
   public void updateTime() {
@@ -339,9 +340,27 @@ public class Drive extends SubsystemBase {
     m_totalRightDistanceInches += ticksToInches(m_currentRightPosTicks - m_lastRightPosTicks);
     m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks);
 
-    m_odometry.update(Rotation2d.fromDegrees( getHeading()),
+    updateOdometry(m_isReversed);
+  }
+
+  public void updateOdometry(boolean reversed){
+    if (reversed){
+
+      m_odometry.update(Rotation2d.fromDegrees( -getGyroYaw()-180),
+                                              -inchesToMeters(getDistanceInches(m_rightFrontMotor)),
+                                              inchesToMeters(getDistanceInches(m_leftFrontMotor)));
+    }
+    else
+    {
+      m_odometry.update(Rotation2d.fromDegrees( getHeading()),
                                               inchesToMeters(getDistanceInches(m_leftFrontMotor)),
                                               -inchesToMeters(getDistanceInches(m_rightFrontMotor)));
+    }
+  }
+
+  public void switchReversed(boolean reversed)
+  {
+    m_isReversed = reversed;
   }
 
   /**
@@ -401,7 +420,7 @@ public class Drive extends SubsystemBase {
     m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
     m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
 
-    m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro);
+    m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, targetGyro); 
     m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
     m_leftBackMotor.follow(m_leftFrontMotor);
     m_rightBackMotor.follow(m_rightFrontMotor);
@@ -426,7 +445,7 @@ public class Drive extends SubsystemBase {
     m_rightBackMotor.follow(m_rightFrontMotor);
 
     m_driveTrain.feedWatchdog();
-
+  
   }
 
   /**

src/main/java/frc4388/robot/subsystems/LimeLight.java

@@ -23,15 +23,15 @@ public class LimeLight extends SubsystemBase {
   }
 
   public void limeOff(){
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
+    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1);
   }
-  
+
   public void limeOn(){
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
   }
-  
+
   public void changePipeline(int pipelineId)
   {
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId);

src/main/java/frc4388/robot/subsystems/Shooter.java

@@ -21,8 +21,8 @@ import frc4388.utility.controller.IHandController;
 
 public class Shooter extends SubsystemBase {
 
-  public WPI_TalonFX m_shooterFalcon = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_ID);
-
+  public WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID);
+  public WPI_TalonFX m_shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_FOLLOWER_ID);
   public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
   public static Shooter m_shooter;
   public static IHandController m_controller;
@@ -50,24 +50,42 @@ public class Shooter extends SubsystemBase {
     //SmartDashboard.putNumber("Velocity Target", 10000);
     //SmartDashboard.putNumber("Angle Target", 3);
 
-    m_shooterFalcon.configFactoryDefault();
-    m_shooterFalcon.setNeutralMode(NeutralMode.Coast);
-    m_shooterFalcon.setInverted(true);
-    m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalcon.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    //LEFT FALCON
+    m_shooterFalconLeft.configFactoryDefault();
+    m_shooterFalconLeft.setNeutralMode(NeutralMode.Coast);
+    m_shooterFalconLeft.setInverted(true);
+    m_shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
+
+    //RIGHT FALCON
+    m_shooterFalconRight.configFactoryDefault();
+    m_shooterFalconRight.setNeutralMode(NeutralMode.Coast);
+    m_shooterFalconRight.setInverted(false);
+    m_shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS);
     setShooterGains();
 
-    m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    
-    m_shooterFalcon.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    
     int closedLoopTimeMs = 1;
-    m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    //LEFT FALCON
+    m_shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    
+    m_shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    
+    m_shooterFalconLeft.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
+
+    m_shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
+
+    
+      //RIGHT FALCON
+    //m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    
+    m_shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    
+    m_shooterFalconRight.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
+
+    m_shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
 
     m_shooterTable = new ShooterTables();
-
-    m_shooterFalcon.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS);
-
     //SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10));
     //SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200));
     //SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250));
@@ -83,13 +101,13 @@ public class Shooter extends SubsystemBase {
   public void periodic() {
     // This method will be called once per scheduler run
     try{
-      SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity());
+      SmartDashboard.putNumber("Drum Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
 
       SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel);
 
-      SmartDashboard.putNumber("Shooter Temp C", m_shooterFalcon.getTemperature());
+      SmartDashboard.putNumber("Shooter Temp C", m_shooterFalconLeft.getTemperature());
 
-      SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent());
+      SmartDashboard.putNumber("Shooter Current", m_shooterFalconLeft.getSupplyCurrent());
 
       SmartDashboard.putBoolean("Drum Ready" , m_isDrumReady);
     }
@@ -118,18 +136,20 @@ public class Shooter extends SubsystemBase {
    * @param speed Speed to set the motor at
    */
   public void runDrumShooter(double speed) {
-    m_shooterFalcon.set(speed);
+    m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed);
+    m_shooterFalconRight.follow(m_shooterFalconLeft);
+    //m_shooterFalconRight.set(TalonFXControlMode.PercentOutput, speed);
   }
 
   /**
    * Configures gains for shooter PID.
    */
   public void setShooterGains() {
-    m_shooterFalcon.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
-    m_shooterFalcon.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalcon.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalcon.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalcon.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
+    m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
   }
 
   /**
@@ -138,6 +158,7 @@ public class Shooter extends SubsystemBase {
    */
   public void runDrumShooterVelocityPID(double targetVel) {
     //System.out.println("Target Velocity" + targetVel);
-    m_shooterFalcon.set(TalonFXControlMode.Velocity, targetVel); //Init PID
+    m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init PID
+    m_shooterFalconRight.follow(m_shooterFalconLeft);
   }
 }