From 01d6a7ef435dd9f6835fc3916a7f623395288490 Mon Sep 17 00:00:00 2001 From: kartofen Date: Fri, 8 Mar 2024 10:55:54 +0200 Subject: changes --- src/main/java/frc/robot/subsystems/Hooks.java | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) (limited to 'src/main/java/frc/robot/subsystems/Hooks.java') diff --git a/src/main/java/frc/robot/subsystems/Hooks.java b/src/main/java/frc/robot/subsystems/Hooks.java index b6c26c5..6e42322 100644 --- a/src/main/java/frc/robot/subsystems/Hooks.java +++ b/src/main/java/frc/robot/subsystems/Hooks.java @@ -1,25 +1,29 @@ package frc.robot.subsystems; -import java.lang.constant.Constable; +import org.littletonrobotics.junction.Logger; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkLowLevel.MotorType; import frc.robot.Constants; - - +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Hooks extends SubsystemBase { + // initial angle 237 private final CANSparkMax m_hookL = new CANSparkMax(Constants.hookL_ID, MotorType.kBrushed); private final CANSparkMax m_hookR = new CANSparkMax(Constants.hookR_ID, MotorType.kBrushed); + private final Encoder m_hookL_encoder = new Encoder(Constants.hookL_PINS[0], Constants.hookL_PINS[1]); + private final Encoder m_hookR_encoder = new Encoder(Constants.hookR_PINS[0], Constants.hookR_PINS[1]); + public Hooks() { // TODO: possibly invert to move forward upon positive votage - // TODO: invert one hook m_hookL.setInverted(true); + m_hookL_encoder.setReverseDirection(true); } public void atPercentage(double percL, double percR) @@ -28,4 +32,10 @@ public class Hooks extends SubsystemBase m_hookR.set(percR); } + @Override + public void periodic() + { + Logger.recordOutput(getName() + "/encoderL", m_hookL_encoder.getDistance()); + Logger.recordOutput(getName() + "/encoderR", m_hookR_encoder.getDistance()); + } } -- cgit v1.2.3