Brought in Drive and Limelight subsystem and adjusted controller

Constants.java

@@ -22,6 +22,24 @@ public static final class ArmMoveConstants{
         public static final int CLAW_CONE_POSITION = 100;//13; 100 Goes right to the max set in armConstants
         public static final int CLAW_CUBE_POSITION = 5;
     }
+    public static final class DriveConstants {
+        public static final int MOTORLEFT1PORT = 0;
+        public static final int MOTORLEFT2PORT = 1;
+        public static final int MOTORRIGHT1PORT = 2;
+        public static final int MOTORRIGHT2PORT = 3;
+
+        public static final int[] ENCODERPORTSLEFT = {0, 1};
+        public static final int[] ENCODERPORTSRIGHT = {2, 3};
+        public static final boolean ENCODERREVERSEDLEFT = false;
+        public static final boolean ENCODERREVERSEDRIGHT= true;
+
+        public static final double ADAPTIVESTEERINGSENSITIVITY = 0.002;
+
+        public static final int ENCODERTICKSPERREV= 1024;
+
+        public static final double ENCODERDISTANCEPERPULSEINCHES =3.0;
+
+    }
 
 
 

Controllers/PersonalizedController.java

@@ -3,6 +3,7 @@
 public interface PersonalizedController {
     PersonalizedController getcontroller();
     int enableFineControlButton();
+    int enableDriveButton();
     int goToHighPosButton();
     int goToMedPosButton();
     int goToGroundPickupPosButton();

Controllers/ps4Brandon.java

@@ -53,6 +53,10 @@ public PersonalizedController getcontroller() {
     public int enableFineControlButton() {
         return Button.kL1.value;
     }
+    @Override
+    public int enableDriveButton() {
+        return Button.kR1.value;
+    }
 
     @Override
     public boolean povPressed() {

Controllers/xBoxBrandon.java

@@ -51,6 +51,10 @@ public int enableFineControlButton() {
         return buttonMappings[0];
     }
     @Override
+    public int enableDriveButton() {
+        return XboxController.Button.kRightBumper.value;
+    }
+    @Override
     public int goToHighPosButton() {
         return buttonMappings[1];
     }
@@ -84,9 +88,9 @@ public int shiftHighButton() {
     public int openClawButton() {
         return buttonMappings[8];
     }
-    //@Override
+         //@Override
     public int closeClawCubeButton() {
-        return buttonMappings[9];
+   return buttonMappings[9];
     }
     //@Override
     public int closeClawConeButton() {

RobotContainer.java

@@ -7,7 +7,10 @@
 import frc.robot.Controllers.*;
 import frc.robot.commands.*;
 import frc.robot.Constants.ArmMoveConstants;
+import frc.robot.subsystems.LimelightSubsystem;
 import frc.robot.subsystems.ArmSubsystem.*;
+import frc.robot.subsystems.DriveSubsystem.DriveControlCommand;
+import frc.robot.subsystems.DriveSubsystem.DriveSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -20,7 +23,8 @@
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
-
+  static final DriveSubsystem m_DriveSubsystem = new DriveSubsystem();
+  static final LimelightSubsystem m_LimelightSubsystem = new LimelightSubsystem();
   static final ArmSubsystem m_ArmSubsystem = new ArmSubsystem();
   //private final ps4Brandon m_PersonalizedController =new ps4Brandon(0);
   static final xBoxBrandon m_PersonalizedController =new xBoxBrandon(0);
@@ -51,6 +55,9 @@ private void configureBindings() {
 
     new JoystickButton(m_PersonalizedController, m_PersonalizedController.enableFineControlButton())
       .whileTrue(new ArmFineControlCommand(m_ArmSubsystem,m_PersonalizedController));
+
+    new JoystickButton(m_PersonalizedController, m_PersonalizedController.enableDriveButton())
+      .whileTrue(new DriveControlCommand(m_DriveSubsystem, m_PersonalizedController));
 
     new JoystickButton(m_PersonalizedController, m_PersonalizedController.goToHomeButton())// go to home (arm doesn't fall when turned off)
       .onTrue(new ArmFollowLineCommand(m_ArmSubsystem, 12, 1, 15));
@@ -86,18 +93,26 @@ private void configureBindings() {
 
     // new JoystickButton(m_PersonalizedController, m_PersonalizedController.openClawButton())
     //   .onTrue(m_ArmSubsystem.setClawPositionCommand(Constants.ArmMoveConstants.CLAW_OPEN_POSITION));
-    new JoystickButton(m_PersonalizedController, m_PersonalizedController.openClawButton())
+    new JoystickButton(m_PersonalizedController, m_PersonalizedController.closeClawCubeButton())
       .onTrue(MultiLine.MultiLineHHTest(m_ArmSubsystem));
 
-    new JoystickButton(m_PersonalizedController, m_PersonalizedController.closeClawCubeButton())
-      .onTrue(m_ArmSubsystem.setClawPositionCommand(Constants.ArmMoveConstants.CLAW_CUBE_POSITION));
+    // new JoystickButton(m_PersonalizedController, m_PersonalizedController.closeClawCubeButton())
+    //   .onTrue(m_ArmSubsystem.setClawPositionCommand(Constants.ArmMoveConstants.CLAW_CUBE_POSITION));
 
     new JoystickButton(m_PersonalizedController, m_PersonalizedController.closeClawConeButton())
       .onTrue(m_ArmSubsystem.setClawPositionCommand(Constants.ArmMoveConstants.CLAW_CONE_POSITION));
 
   }
 
   private void setDefaultCommands(){
+    m_LimelightSubsystem.setDefaultCommand(
+      m_LimelightSubsystem.checkForTargetsCommand()
+    );
+
+    m_DriveSubsystem.setDefaultCommand(
+      m_DriveSubsystem.arcadeDriveSquaredCommand(
+          () -> 0, () -> 0)
+    );
 
     m_ArmSubsystem.setDefaultCommand(
       m_ArmSubsystem.RunJointsToSetAnglesCommand()

commands/Autos.java

@@ -0,0 +1,26 @@
+// 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.commands;
+
+import frc.robot.subsystems.ArmSubsystem.ArmSubsystem;
+import frc.robot.subsystems.DriveSubsystem.DriveSubsystem;
+//import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+
+public final class Autos {
+  /** Example static factory for an autonomous command. */
+  public static Command exampleAutoDrive(DriveSubsystem subsystem) {
+    return Commands.sequence(subsystem.driveDistanceCommand(3, .5), subsystem.driveDistanceCommand(-3, 0.5));
+  }
+
+  public static Command exampleAutoArm(ArmSubsystem subsystem) {
+    return Commands.sequence(subsystem.RunJointsToThetaRZCommand(0,22,10), subsystem.RunJointsToThetaRZCommand(10,20,20));
+  }
+
+  private Autos() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+}

commands/FollowTargets.java

@@ -0,0 +1,52 @@
+package frc.robot.commands;
+
+import frc.robot.subsystems.LimelightSubsystem;
+import frc.robot.subsystems.DriveSubsystem.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class FollowTargets extends Command {
+    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+    private DriveSubsystem m_DriveSubsystem;
+    private LimelightSubsystem m_LimelightSubsystem;
+
+    /**
+     * Creates a new ExampleCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    public FollowTargets(DriveSubsystem subsystem_drive, LimelightSubsystem subsystem_limelight) {
+       m_DriveSubsystem = subsystem_drive;
+       m_LimelightSubsystem= subsystem_limelight;
+      // // Use addRequirements() here to declare subsystem dependencies.
+       addRequirements(subsystem_drive);
+      // 
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+
+      // Use addRequirements() here to declare subsystem dependencies.
+      double targetPosition = m_LimelightSubsystem.getTargets()[0];
+      //double rot = Math.min(Math.max((targetPosition/5)*(Math.abs(targetPosition)/5),-0.8),0.6);//limit speed 
+      double rot = Math.min(Math.max((targetPosition/Math.abs(targetPosition))*Math.sqrt(Math.abs(targetPosition/20)),-0.8),0.8);
+      m_DriveSubsystem.arcadeDriveSquared(0.0,-rot);
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {}
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+      return false;
+    }
+  }
+

subsystems/ArmSubsystem/ArmSubsystem.java

@@ -15,43 +15,62 @@
 public class ArmSubsystem extends SubsystemBase {
 
   // Define all of the Joints
-  // public Joint(int deviceID,Boolean limitSwitchDirection, 
-  // double l_max_output, double l_min_output, 
-  // double l_homingSpeed, int l_startingAngle,
-  // int l_maxAngle, int l_minAngle,
-  // int l_homePositionAngle, double l_degreesPerRev){
-  private static Joint ShoulderJoint = new Joint(ArmConstants.SHOULDER_MOTOR_ID, ArmConstants.SHOULDER_LIMIT_SWITCH_DIRECTION,
-  ArmConstants.SHOULDER_JOINT_SPEED,ArmConstants.SHOULDER_JOINT_SPEED,
-  ArmConstants.SHOULDER_HOMING_SPEED, ArmConstants.THETA1_START_OFFSET,
-  ArmConstants.THETA1_MAX, ArmConstants.THETA1_MIN,
-  ArmConstants.THETA1_HOMED_OFFSET, ArmConstants.SHOULDER_DEGREES_PER_REVOLUTION,
-  false);
+  private static Joint ShoulderJoint = 
+    new Joint(ArmConstants.SHOULDER_MOTOR_ID, 
+              ArmConstants.SHOULDER_LIMIT_SWITCH_DIRECTION,
+              ArmConstants.SHOULDER_JOINT_SPEED,
+              ArmConstants.SHOULDER_JOINT_SPEED,
+              ArmConstants.SHOULDER_HOMING_SPEED, 
+              ArmConstants.THETA1_START_OFFSET,
+              ArmConstants.THETA1_MAX, 
+              ArmConstants.THETA1_MIN,
+              ArmConstants.THETA1_HOMED_OFFSET, 
+              ArmConstants.SHOULDER_DEGREES_PER_REVOLUTION,
+     false);
 
-  private static Joint ElbowJoint = new Joint(ArmConstants.ELBOW_MOTOR_ID, ArmConstants.ELBOW_LIMIT_SWITCH_DIRECTION,
-  ArmConstants.ELBOW_JOINT_SPEED,ArmConstants.ELBOW_JOINT_SPEED,
-  ArmConstants.ELBOW_HOMING_SPEED, ArmConstants.THETA2_START_OFFSET,
-  ArmConstants.THETA2_MAX, ArmConstants.THETA2_MIN,
-  ArmConstants.THETA2_HOMED_OFFSET, ArmConstants.ELBOW_DEGREES_PER_REVOLUTION,
-  false);
-
-  private static Joint AzimuthJoint = new Joint(ArmConstants.AZIMUTH_MOTOR_ID, ArmConstants.AZIMUTH_LIMIT_SWITCH_DIRECTION,
-  ArmConstants.AZIMUTH_JOINT_SPEED,ArmConstants.AZIMUTH_JOINT_SPEED,
-  ArmConstants.AZIMUTH_HOMING_SPEED, ArmConstants.AZIMUTH_START_OFFSET,
-  ArmConstants.AZIMUTH_MAX, ArmConstants.AZIMUTH_MIN,
-  ArmConstants.AZIMUTH_HOMED_OFFSET, ArmConstants.AZIMUTH_DEGREES_PER_REVOLUTION,
-  true);
-
-  private static Joint ClawJoint = new Joint(ArmConstants.CLAW_MOTOR_ID, ArmConstants.CLAW_LIMIT_SWITCH_DIRECTION,
-  0.5,ArmConstants.CLAW_JOINT_SPEED,//OVERIDE MAX SPEED FOR CLOSING
-  ArmConstants.CLAW_HOMING_SPEED, ArmConstants.CLAW_START_OFFSET,
-  ArmConstants.CLAW_MAX, ArmConstants.CLAW_MIN,
-  ArmConstants.CLAW_HOMED_OFFSET, ArmConstants.CLAW_DEGREES_PER_REVOLUTION,
-  false);
-
-  private static double azimuthAngle = 0;
+  private static Joint ElbowJoint 
+    = new Joint(ArmConstants.ELBOW_MOTOR_ID, 
+                ArmConstants.ELBOW_LIMIT_SWITCH_DIRECTION,
+                ArmConstants.ELBOW_JOINT_SPEED,
+                ArmConstants.ELBOW_JOINT_SPEED,
+                ArmConstants.ELBOW_HOMING_SPEED, 
+                ArmConstants.THETA2_START_OFFSET,
+                ArmConstants.THETA2_MAX, 
+                ArmConstants.THETA2_MIN,
+                ArmConstants.THETA2_HOMED_OFFSET, 
+                ArmConstants.ELBOW_DEGREES_PER_REVOLUTION,
+       false);
+
+  private static Joint AzimuthJoint 
+    = new Joint(ArmConstants.AZIMUTH_MOTOR_ID, 
+                ArmConstants.AZIMUTH_LIMIT_SWITCH_DIRECTION,
+                ArmConstants.AZIMUTH_JOINT_SPEED,
+                ArmConstants.AZIMUTH_JOINT_SPEED,
+                ArmConstants.AZIMUTH_HOMING_SPEED, 
+                ArmConstants.AZIMUTH_START_OFFSET,
+                ArmConstants.AZIMUTH_MAX, 
+                ArmConstants.AZIMUTH_MIN,
+                ArmConstants.AZIMUTH_HOMED_OFFSET, 
+                ArmConstants.AZIMUTH_DEGREES_PER_REVOLUTION,
+       true);
+
+  private static Joint ClawJoint 
+    = new Joint(ArmConstants.CLAW_MOTOR_ID, 
+                ArmConstants.CLAW_LIMIT_SWITCH_DIRECTION,
+   0.5,
+                ArmConstants.CLAW_JOINT_SPEED,//OVERIDE MAX SPEED FOR CLOSING
+                ArmConstants.CLAW_HOMING_SPEED, 
+                ArmConstants.CLAW_START_OFFSET,
+                ArmConstants.CLAW_MAX, 
+                ArmConstants.CLAW_MIN,
+                ArmConstants.CLAW_HOMED_OFFSET, 
+                ArmConstants.CLAW_DEGREES_PER_REVOLUTION,
+       false);
+
+  private static double azimuthAngle  = 0;
   private static double ShoulderAngle = 100;
-  private static double elbowAngle =25;
-  private static double clawPosition =0;
+  private static double elbowAngle    = 25;
+  private static double clawPosition  = 0;
 
   public void initialize() {
     //force claw and azimuth into homed condition since they have no limit switch
@@ -64,26 +83,26 @@ public double[] getArmAngles() {
     return l_currentPos;
   }
   public double[] getArmPositionThetaRZ() {
-    // Calculations done in Radians for 2 segment arm of equal segment lengths
-    //Length from shoulder to claw
-    double l_Hypotenus = 2 * ArmConstants.ARM_LENGTH1 * Math.sin(elbowAngle / 2 * Math.PI / 180);
-    //intetior angle between humerous and claw
+    double l_shoulderToClaw = 2 * ArmConstants.ARM_LENGTH1 * Math.sin(elbowAngle / 2 * Math.PI / 180);
+
+    // Intetior angle between humerous and claw
     double l_Theta1_1 = 90-elbowAngle/2;
-    //angle between ground and claw
+
+    // Angle between ground and claw
     double l_ThetaGroundClaw = ShoulderAngle - l_Theta1_1;
 
     // Define values
-    double l_radius = l_Hypotenus * Math.cos(l_ThetaGroundClaw * Math.PI / 180); 
-    double l_height = l_Hypotenus * Math.sin(l_ThetaGroundClaw * Math.PI / 180);
+    double l_radius = l_shoulderToClaw * Math.cos(l_ThetaGroundClaw * Math.PI / 180); 
+    double l_height = l_shoulderToClaw * Math.sin(l_ThetaGroundClaw * Math.PI / 180);
 
-    //azimuth does not need to be converted
-    double[] l_currentPos = {azimuthAngle,l_radius,l_height};
+    // Azimuth does not need to be converted
+    double[] l_currentPos = {azimuthAngle, l_radius, l_height};
     return l_currentPos;
   }
   public double[] getArmPositionXYZ() {
     // Calculations done in Radians for 2 segment arm of equal segment lengths
     //Length from shoulder to claw
-    double l_x,l_y,l_z;
+    double l_x, l_y, l_z;
     double l_Hypotenus = 2 * ArmConstants.ARM_LENGTH1 * Math.sin(elbowAngle / 2 * Math.PI / 180);
     //intetior angle between humerous and claw
     double l_Theta1_1 = 90-elbowAngle/2;

subsystems/ArmSubsystem/SparkmaxMotor.java

@@ -1,30 +1,30 @@
 package frc.robot.subsystems.ArmSubsystem;
 
 import com.revrobotics.RelativeEncoder;
-import com.revrobotics.SparkMaxLimitSwitch;
-import com.revrobotics.SparkMaxPIDController;
+import com.revrobotics.SparkLimitSwitch;
+import com.revrobotics.SparkPIDController;
 import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import com.revrobotics.CANSparkLowLevel.MotorType;
 
 public class SparkmaxMotor  {
 
   protected CANSparkMax m_motor;
-  protected SparkMaxPIDController m_pidControllerPos;
+  protected SparkPIDController m_pidControllerPos;
   protected RelativeEncoder m_encoder;
   protected double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput;
 
   protected boolean m_limitSwitchDirection=false; //false reverse true forward
-  protected SparkMaxLimitSwitch limitSwitch;
+  protected SparkLimitSwitch limitSwitch;
 
   protected SparkmaxMotor(int deviceID,Boolean limitSwitchDirection, double l_max_output, double l_min_output,boolean l_invert) {
     m_motor = new CANSparkMax(deviceID, MotorType.kBrushless);
     m_motor.restoreFactoryDefaults();
     m_pidControllerPos = m_motor.getPIDController();
     m_encoder = m_motor.getEncoder();
     if(m_limitSwitchDirection){
-      limitSwitch = m_motor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+      limitSwitch = m_motor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
     }else{
-      limitSwitch = m_motor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+      limitSwitch = m_motor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen);
     }
     initializePID(l_max_output,l_min_output);
     m_motor.setInverted(l_invert);