starting encoder stuff

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

@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -45,9 +46,7 @@ public class RobotContainer {
   private final Tank m_Tank = new Tank();
   private final PID m_PID = new PID();
   private final PDP m_PDP = new PDP();
-  private final Vision m_Vision = new Vision(m_Tank);
-
-
+  private final Vision m_Vision = new Vision(m_Tank, m_Claw);
 
   // INSTANTIATES ALL COMMANDS
   private final MoveClawCommand m_MoveClawCommand = new MoveClawCommand(m_Claw);
@@ -63,8 +62,8 @@ public class RobotContainer {
   private final FineDriveCommand m_FineDriveCommand = new FineDriveCommand(m_Tank);
   private final PlaceCubeSecondLevelCommand m_PlaceCubeSecondLevelCommand = new PlaceCubeSecondLevelCommand(m_Tank,
       m_Arm, m_Claw);
-  private final TrackPiece m_TrackPiece = new TrackPiece(m_Vision, m_Tank);
-  
+  private final TrackPiece m_TrackPiece = new TrackPiece(m_Vision, m_Tank, m_Claw);
+
   // private final XTracking m_XTracking = new XTracking(m_Tank, null, m_Target);
   // private final YTracking m_YTracking = new YTr
 
@@ -100,9 +99,6 @@ public RobotContainer() {
 
     Logger.configureLoggingAndConfig(this, false);
 
-
-
-
   }
 
   /**
@@ -128,11 +124,8 @@ private void configureButtonBindings() {
         .whileTrue(m_FineDriveCommand);
     new JoystickButton(m_armController2, XboxController.Button.kY.value).whileTrue(m_PlaceConeSecondLevelCommand);
 
-
     new JoystickButton(m_armController2, XboxController.Button.kX.value).whileTrue(m_TrackPiece);
 
-
-
     // new JoystickButton(m_armController2, XboxController.Button.kB.value)
     // .whileTrue(m_PlaceConeSecondLevelCommand);
   }
@@ -250,6 +243,19 @@ public static double getControllerRightStickX() {
     }
     return axis;
   }
+
+  public static double getControllerRightStickY() {
+    double axis = m_armController2.getRawAxis(5);
+    if (Math.abs(axis) < 0.4) {
+      axis = 0;
+    }
+    return -axis;
+  }
+
+  public static void rumbleGabeController(double rumble) {
+    XboxController controller = m_driverController;
+    controller.setRumble(RumbleType.kBothRumble, rumble);
+  }
 
   /**
    * gets value from the right trigger axis (yes, it's an axis) for grabbing up
@@ -301,6 +307,4 @@ public void robotPeriodic() {
     timeWidget.setDouble(DriverStation.getMatchTime());
   }
 
-
-
 }

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

@@ -8,6 +8,8 @@
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.RebindHat;
+import frc.robot.Robot;
+import frc.robot.RobotContainer;
 import frc.robot.subsystems.Tank;
 
 public class FineDriveCommand extends CommandBase {
@@ -30,13 +32,15 @@ public void initialize() {
   @Override
   public void execute() {
     // double check getMaxSpeed(), might be wrong
+    RobotContainer.rumbleGabeController(1);
     m_drivebase.arcadeDrive(
         RebindHat.ControllerToYAxis() * multWidget.getDouble(0), RebindHat.ControllerToXAxis() * multWidget.getDouble(0));
   }
 
   @Override
   public void end(boolean interrupted) {
     m_drivebase.arcadeDrive(0, 0);
+    RobotContainer.rumbleGabeController(0);
   }
 
   @Override

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

@@ -6,18 +6,22 @@
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Claw;
 import frc.robot.subsystems.Tank;
 import frc.robot.subsystems.Vision;
 
 public class TrackPiece extends CommandBase {
   /** Creates a new TrackPiece. */
   private Vision m_Vision;
   private Tank m_Tank;
-  public TrackPiece(Vision vision, Tank tank) {
+  private Claw m_Claw;
+  public TrackPiece(Vision vision, Tank tank, Claw claw) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_Vision = vision;
     m_Tank = tank;
-    addRequirements(m_Vision, m_Tank);
+    m_Claw = claw;
+    addRequirements(m_Vision, m_Tank, m_Claw);
   }
 
   // Called when the command is initially scheduled.
@@ -26,25 +30,30 @@ public void initialize() {
     //m_Tank.arcadeDrive(0.05, m_Vision.getController().calculate(m_Vision.getMeasurement(), m_Vision.getSetpoint()));
     m_Vision.enable();
     SmartDashboard.putString("a", "pid on");
+    m_Claw.closeClaw();
 
   }
 
   @Override
   public void execute() {
+    //m_Tank.setAllMotors(-0.2);
+    RobotContainer.rumbleGabeController(1);
 
     SmartDashboard.putNumber(getName(), m_Vision.getController().calculate(m_Vision.getMeasurement(), m_Vision.getSetpoint()));
   }
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
+    RobotContainer.rumbleGabeController(0);
     m_Vision.disable();
     m_Tank.arcadeDrive(0,0);
     SmartDashboard.putString("a", "pid off");
+    m_Claw.stopClaw();
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return m_Vision.atSetpoint();
+    return false;
   }
 }

src/main/java/frc/robot/subsystems/Arm.java

@@ -42,6 +42,9 @@ public class Arm extends SubsystemBase {
 
   public AbsoluteEncoder encoder;
 
+  double lastEncoderValue;
+  int rotations;
+
   public Arm() {
 
     kP = 0.0001;
@@ -71,15 +74,17 @@ public Arm() {
     armPID.setOutputRange(kMinOutput, kMaxOutput);
 
     // set the spark max to alternate encoder mode
-
 
-    // configure the data port on top to be used with the REV Through Bore Encoder using the absolute encoder adapter
+    // configure the data port on top to be used with the REV Through Bore Encoder
+    // using the absolute encoder adapter
     encoder = armMotor.getAbsoluteEncoder(Type.kDutyCycle);
 
     encoderWidget = Shuffleboard.getTab("Motors").add("Arm Encoder", armMotor.getEncoder().getPosition()).getEntry();
 
     Shuffleboard.getTab("Motors").add("Arm", armMotor);
 
+    lastEncoderValue = armMotor.getEncoder().getPosition();
+
     // configure the PID loop to use the alternate encoder
     armPID.setFeedbackDevice(encoder);
 
@@ -94,8 +99,18 @@ public Arm() {
   @Override
   public void periodic() {
     // SmartDashboard.putBoolean("zeroDeg", allTheWayDownRear.get());
+    double encoderValue = armMotor.getEncoder().getPosition();
+    double change = encoderValue - lastEncoderValue;
+    if (Math.abs(change) > 0.5) {
+      if (change < 0) {
+        rotations++;
+      } else {
+        rotations--;
+      }
+    }
+    lastEncoderValue = encoderValue;
 
-    encoderWidget.setDouble(armMotor.getEncoder().getPosition());
+    encoderWidget.setDouble(encoderValue);
   }
 
   /**

src/main/java/frc/robot/subsystems/Claw.java

@@ -25,7 +25,7 @@ public class Claw extends SubsystemBase {
   public Claw() {
     clawMotor = new CANSparkMax(8, MotorType.kBrushless);
     clawMotor.setIdleMode(IdleMode.kBrake);
-    clawMotor.setSmartCurrentLimit(17);
+    clawMotor.setSmartCurrentLimit(20);
     clawMotor.burnFlash();
     addChild("Claw Motor", clawMotor);
 
@@ -39,11 +39,11 @@ public void periodic() {
   }
 
   public void closeClaw() {
-    clawMotor.set(1);
+    clawMotor.set(0.75);
   }
 
   public void openClaw() {
-    clawMotor.set(-1);
+    clawMotor.set(-0.75);
   }
 
   public void stopClaw() {

src/main/java/frc/robot/subsystems/Vision.java

@@ -9,25 +9,28 @@
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.PIDSubsystem;
+import frc.robot.RobotContainer;
 
 public class Vision extends PIDSubsystem {
   /** Creates a new Vision. */
-  private static double kP = 0.02;
-  private static double kI = 0.05;
-  private static double kD = 0.0007;
+  private static double kP = 0.1;
+  private static double kI = 0;
+  private static double kD = 0;
 
   public PIDController pidController;
   public Tank m_tank;
+  public Claw m_claw;
 
-  public Vision(Tank tank) {
+  public Vision(Tank tank, Claw claw) {
     super(
         // The PIDController used by the subsystem
         new PIDController(kP, kI, kD));
 
     addChild(getName(), m_controller);
     m_tank = tank;
+    m_claw = claw;
     m_controller.setSetpoint(0);
-    m_controller.setTolerance(2);
+    m_controller.setTolerance(1);
     m_controller.setIntegratorRange(-0.43, 0.43);
 
     Shuffleboard.getTab(getName()).add(m_controller);
@@ -36,20 +39,34 @@ public Vision(Tank tank) {
 
   @Override
   public void useOutput(double output, double setpoint) {
+
+    double ty = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0.0);
+
     // Use the output here
+    double forward = 1 - Math.abs(output) / 30;
+    if (forward < .1) {
+      forward = .1;
+    }
+    forward *= 0.6;
+    if (ty < 0) {
+      forward = 0.5;
+    }
+    double stick = RobotContainer.getControllerRightStickY();
+    if (stick > .5) {
+      forward += (stick / 2) - .25;
+    }
 
-    if (0.40 < output) {
-      output = 0.40;
-    } else if (-0.40 > output) {
-      output = -0.40;
-    } else if (0 < output && output < 0.40) {
+    if (0.50 < output) {
+      output = 0.50;
+    } else if (-0.50 > output) {
+      output = -0.50;
+    } else if (0.1 < output && output < 0.40) {
       output = 0.40;
-    } else if (-0.40 < output && output < 0) {
+    } else if (-0.40 < output && output < -0.1) {
       output = -0.40;
     }
-
 
-    m_tank.arcadeDrive(0, -output * 1);
+    m_tank.arcadeDrive(-forward, -output * 1);
     SmartDashboard.putNumber("will this work?", output);
     SmartDashboard.putNumber("setpoint", setpoint);