Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

sum changes

Browse Source
This commit is contained in:
aarav18
2022-03-16 14:49:41 -06:00
parent 39ee9f979d
commit 633a443194
6 changed files with 292 additions and 287 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Claws.java
+58 -57
View File
@@ -7,18 +7,23 @@ import com.revrobotics.CANSparkMax;
import com.revrobotics.SparkMaxLimitSwitch;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.ClawConstants;
import frc4388.robot.Constants.ClimberConstants;
public class Claws extends SubsystemBase {
public CANSparkMax m_leftClaw;
public CANSparkMax m_rightClaw;
private SparkMaxLimitSwitch m_leftLimitSwitchF;
private SparkMaxLimitSwitch m_rightLimitSwitchF;
private SparkMaxLimitSwitch m_leftLimitSwitchR;
private SparkMaxLimitSwitch m_rightLimitSwitchR;
public Servo m_leftClaw;
public Servo m_rightClaw;
// public CANSparkMax m_leftClaw;
// public CANSparkMax m_rightClaw;
// private SparkMaxLimitSwitch m_leftLimitSwitchF;
// private SparkMaxLimitSwitch m_rightLimitSwitchF;
// private SparkMaxLimitSwitch m_leftLimitSwitchR;
// private SparkMaxLimitSwitch m_rightLimitSwitchR;
private double m_leftOffset;
private double m_rightOffset;
@@ -26,21 +31,22 @@ public class Claws extends SubsystemBase {
private boolean m_open;
public static enum ClawType {LEFT, RIGHT}
public Claws(CANSparkMax leftClaw, CANSparkMax rightClaw) {
public Claws(/*CANSparkMax*/Servo leftClaw, /*CANSparkMax*/Servo rightClaw) {
m_leftClaw = leftClaw;
m_rightClaw = rightClaw;
m_leftLimitSwitchF = m_leftClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_rightLimitSwitchF = m_rightClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_leftLimitSwitchR = m_leftClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyClosed); //Wired wrong lol
m_rightLimitSwitchR = m_rightClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_leftLimitSwitchF = m_leftClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_rightLimitSwitchF = m_rightClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
// m_leftLimitSwitchR = m_leftClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyClosed); //Wired wrong lol
// m_rightLimitSwitchR = m_rightClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
m_leftLimitSwitchF.enableLimitSwitch(true);
m_rightLimitSwitchF.enableLimitSwitch(true);
m_leftLimitSwitchR.enableLimitSwitch(true);
m_rightLimitSwitchR.enableLimitSwitch(true);
leftClaw.setInverted(true);
rightClaw.setInverted(true);
// m_leftLimitSwitchF.enableLimitSwitch(true);
// m_rightLimitSwitchF.enableLimitSwitch(true);
// m_leftLimitSwitchR.enableLimitSwitch(true);
// m_rightLimitSwitchR.enableLimitSwitch(true);
// m_leftClaw.setInverted(true);
// m_rightClaw.setInverted(true);
m_open = false;
@@ -48,11 +54,6 @@ public class Claws extends SubsystemBase {
// m_rightClaw.set(ClawConstants.CALIBRATION_SPEED);
}
public void setSpeed(double speed){
m_leftClaw.set(speed);
m_rightClaw.set(speed);
}
/**
* Run a specific claw to open or close.
* @param which Which claw to run.
@@ -66,38 +67,33 @@ public class Claws extends SubsystemBase {
// double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset;
// m_leftClaw.getEncoder().setPosition(setPos);
m_leftClaw.set(direction * 0.1);
m_leftClaw.setSpeed(direction * 0.1);
} else if (which == Claws.ClawType.RIGHT) {
// double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset;
// m_rightClaw.getEncoder().setPosition(setPos);
m_rightClaw.set(direction * 0.1);
m_rightClaw.setSpeed(direction * 0.1);
}
m_open = open;
}
public void runClaw(ClawType which, double input) {
if (which == Claws.ClawType.LEFT) {
m_leftClaw.set(input);
m_leftClaw.setSpeed(input);
} else if (which == Claws.ClawType.RIGHT) {
m_rightClaw.set(input);
m_rightClaw.setSpeed(input);
}
}
public void runClaws(double input)
{
m_leftClaw.set(input);
m_rightClaw.set(input);
m_leftClaw.setSpeed(input);
m_rightClaw.setSpeed(input);
}
/**
* Sets the state of both hooks
* @param open The state of the hooks
@@ -114,8 +110,6 @@ public class Claws extends SubsystemBase {
m_leftClaw.set(-0.1);
m_rightClaw.set(-0.1);
}
// m_open = open;
}
public double[] getOffsets() {
@@ -123,12 +117,12 @@ public class Claws extends SubsystemBase {
}
public boolean fullyOpen() {
return Math.abs(m_leftClaw.getEncoder().getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD &&
Math.abs(m_rightClaw.getEncoder().getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; }
return Math.abs(m_leftClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD &&
Math.abs(m_rightClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; }
public boolean fullyClosed() {
return Math.abs(m_leftClaw.getEncoder().getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD &&
Math.abs(m_rightClaw.getEncoder().getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD;
return Math.abs(m_leftClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD &&
Math.abs(m_rightClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD;
}
/**
@@ -137,26 +131,33 @@ public class Claws extends SubsystemBase {
* @param limit The current limit.
* @return Whether to interrupt the RunClaw command or not.
*/
public boolean checkSwitchAndCurrent(ClawType which) {
if (which == ClawType.LEFT) {
if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) {
return true;
}
}
else if (which == ClawType.RIGHT) {
if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) {
return true;
}
}
return false;
}
// public boolean checkSwitchAndCurrent(ClawType which) {
// if (which == ClawType.LEFT) {
// if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) {
// return true;
// }
// }
// else if (which == ClawType.RIGHT) {
// if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) {
// return true;
// }
// }
// return false;
// }
@Override
public void periodic() {
if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed())
m_leftOffset = m_leftClaw.getEncoder().getPosition();
if (fullyOpen() || fullyClosed()) {
m_leftClaw.setSpeed(0.0);
m_rightClaw.setSpeed(0.0);
}
// if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed())
// // m_leftOffset = m_leftClaw.getEncoder().getPosition();
// m_leftOffset = m_leftClaw.getPosition();
if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed())
m_rightOffset = m_rightClaw.getEncoder().getPosition();
// if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed())
// // m_rightOffset = m_rightClaw.getEncoder().getPosition();
// m_rightOffset = m_rightClaw.getPosition();
}
}