-
Notifications
You must be signed in to change notification settings - Fork 16
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #19 from Team3128/manip+wrist-inverted
Wrist
- Loading branch information
Showing
4 changed files
with
272 additions
and
7 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,95 @@ | ||
| package frc.team3128.subsystems; | ||
|
|
||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
| import frc.team3128.common.hardware.motorcontroller.NAR_TalonSRX; | ||
| import frc.team3128.common.utility.NAR_Shuffleboard; | ||
| import static frc.team3128.Constants.ManipulatorConstants.*; | ||
| import com.ctre.phoenix.motorcontrol.NeutralMode; | ||
|
|
||
| public class Manipulator extends SubsystemBase { | ||
|
|
||
| public NAR_TalonSRX m_roller; | ||
|
|
||
| private static Manipulator instance; | ||
|
|
||
| public static boolean isCone = true; | ||
|
|
||
| public boolean isOuttaking = false; | ||
|
|
||
| public static Manipulator getInstance() { | ||
| if (instance == null){ | ||
| instance = new Manipulator(); | ||
| } | ||
|
|
||
| return instance; | ||
| } | ||
|
|
||
| public Manipulator(){ | ||
| configMotor(); | ||
| initShuffleboard(); | ||
| } | ||
|
|
||
| //TODO Move this to a trigger in robot container | ||
| @Override | ||
| public void periodic() { | ||
| if (Math.abs(getCurrent()) > ABSOLUTE_THRESHOLD + 40 && !isOuttaking) | ||
| stallPower(); | ||
| } | ||
|
|
||
| private void configMotor(){ | ||
| m_roller = new NAR_TalonSRX(ROLLER_MOTOR_ID); | ||
| m_roller.setInverted(true); | ||
| m_roller.setNeutralMode(NeutralMode.Brake); | ||
| } | ||
|
|
||
| public void intake(boolean cone) { | ||
| isOuttaking = false; | ||
| isCone = cone; | ||
| if (isCone) reverse(); | ||
| else forward(); | ||
| } | ||
|
|
||
| public void outtake(){ | ||
| isOuttaking = true; | ||
| if (!isCone) reverse(); | ||
| else forward(); | ||
| } | ||
|
|
||
| public void stallPower() { | ||
| set(isCone ? -STALL_POWER_CONE : STALL_POWER_CUBE); | ||
| } | ||
|
|
||
| public void set(double power){ | ||
| m_roller.set(power); | ||
| } | ||
|
|
||
| public void forward(){ | ||
| m_roller.set(ROLLER_POWER); | ||
| } | ||
|
|
||
| public void reverse(){ | ||
| m_roller.set(-ROLLER_POWER); | ||
| } | ||
|
|
||
| public void stopRoller(){ | ||
| m_roller.set(0); | ||
| } | ||
|
|
||
| public boolean hasObjectPresent(){ | ||
| return isCone ? Math.abs(getCurrent()) > CURRENT_THRESHOLD_CONE : Math.abs(getCurrent()) > CURRENT_THRESHOLD_CUBE; | ||
| } | ||
|
|
||
| public double getCurrent(){ | ||
| return m_roller.getStatorCurrent(); | ||
| } | ||
|
|
||
| public double getVoltage() { | ||
| return m_roller.getMotorOutputVoltage(); | ||
| } | ||
|
|
||
| public void initShuffleboard() { | ||
| NAR_Shuffleboard.addData("Manipulator", "Manip current", () -> getCurrent(), 0, 1); | ||
| NAR_Shuffleboard.addData("Manipulator", "get", () -> m_roller.getMotorOutputPercent(), 0, 3); | ||
| NAR_Shuffleboard.addData("Manipulator", "ObjectPresent", ()-> hasObjectPresent(), 1, 1); | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,89 @@ | ||
| package frc.team3128.subsystems; | ||
|
|
||
| import com.revrobotics.CANSparkMax.IdleMode; | ||
| import com.revrobotics.CANSparkMaxLowLevel.MotorType; | ||
|
|
||
| import edu.wpi.first.math.MathUtil; | ||
| import edu.wpi.first.math.controller.PIDController; | ||
| import edu.wpi.first.math.util.Units; | ||
|
|
||
| import static frc.team3128.Constants.WristConstants.*; | ||
|
|
||
| import frc.team3128.common.hardware.motorcontroller.NAR_CANSparkMax; | ||
| import frc.team3128.common.hardware.motorcontroller.NAR_CANSparkMax.EncoderType; | ||
| import frc.team3128.common.utility.NAR_Shuffleboard; | ||
|
|
||
| public class Wrist extends NAR_PIDSubsystem { | ||
| public NAR_CANSparkMax m_wrist; | ||
|
|
||
| private static Wrist instance; | ||
|
|
||
| public enum WristPosition { | ||
| SCORE_CONE(0), | ||
| SCORE_CUBE(0), | ||
|
|
||
| NEUTRAL(0), | ||
|
|
||
| SINGLE_SHELF_CONE(0), | ||
| SINGLE_SHELF_CUBE(0), | ||
| DOUBLE_SHELF_CONE(0), | ||
| DOUBLE_SHELF_CUBE(0), | ||
| GROUND_PICKUP_CONE(0), | ||
| GROUND_PICKUP_CUBE(0); | ||
|
|
||
| public final double wristAngle; | ||
|
|
||
| private WristPosition(double wristAngle) { | ||
| this.wristAngle = wristAngle; | ||
| } | ||
| } | ||
|
|
||
| public static Wrist getInstance() { | ||
| if (instance == null){ | ||
| instance = new Wrist(); | ||
| } | ||
| return instance; | ||
| } | ||
|
|
||
| public Wrist() { | ||
| super(new PIDController(kP, kI, kD), kS, kV, kG); | ||
| setkG_Function(()-> Math.cos(Units.degreesToRadians(getMeasurement()))); | ||
| setConstraints(MIN_ANGLE, MAX_ANGLE); | ||
| configMotor(); | ||
| } | ||
|
|
||
| private void configMotor() { | ||
| m_wrist = new NAR_CANSparkMax(WRIST_ID, EncoderType.Absolute, MotorType.kBrushless); | ||
| m_wrist.setInverted(false); | ||
| m_wrist.setIdleMode(IdleMode.kBrake); | ||
| m_wrist.setSmartCurrentLimit(40); | ||
| resetEncoder(); | ||
| } | ||
|
|
||
| @Override | ||
| protected void useOutput(double output, double setpoint) { | ||
| m_wrist.set(MathUtil.clamp(output / 12.0, -1, 1)); | ||
| } | ||
|
|
||
| @Override | ||
| public double getMeasurement() { | ||
| // return m_wrist.getSelectedSensorPosition() * ROTATION_TO_DEGREES / GEAR_RATIO + ANGLE_OFFSET; | ||
| return 12.3; | ||
| // return MathUtil.inputModulus(-m_encoder.get() * ENCODER_CONVERSION_FACTOR_TO_DEGREES - ANGLE_OFFSET,-180, 180); | ||
| } | ||
|
|
||
| public void resetEncoder() { | ||
| m_wrist.setSelectedSensorPosition(0); | ||
| } | ||
|
|
||
| public void set(double power) { | ||
| m_wrist.set(power); | ||
| } | ||
|
|
||
| public void initShuffleboard() { | ||
| // NAR_Shuffleboard.addData("Manipulator", "Manip current", () -> getCurrent(), 0, 1); | ||
| // NAR_Shuffleboard.addData("Manipulator", "get", () -> m_roller.getMotorOutputPercent(), 0, 3); | ||
| NAR_Shuffleboard.addData("Wrist", "angle", ()-> m_wrist.getSelectedSensorPosition(), 1, 1); | ||
| } | ||
|
|
||
| } |