claw is correct now

This commit is contained in:
Aarav
2023-03-21 16:17:20 -06:00
parent 39640662d9
commit 0129594f89
4 changed files with 129 additions and 15 deletions
@@ -7,7 +7,12 @@
package frc4388.robot;
import com.revrobotics.CANSparkMax;
import com.revrobotics.REVLibError;
import com.revrobotics.CANSparkMax.FaultID;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -45,7 +50,7 @@ public class RobotContainer {
public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder);
public final Claw m_robotClaw = new Claw(m_robotMap.servo);
public final Claw m_robotClaw = new Claw(m_robotMap.leftClaw, m_robotMap.rightClaw, m_robotMap.spinnyspin);
public final Limelight m_limeLight = new Limelight();
@@ -154,17 +159,21 @@ public class RobotContainer {
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> m_limeLight.toggleLEDs(), m_limeLight));
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
.whileTrue(new LimeAlign(m_robotSwerveDrive, m_limeLight));
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
// SmartDashboard.putBoolean("isn't SparkMAX", );
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue (new InstantCommand(() -> m_robotClaw.yesspinnyspin()))
.onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin()));
// new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm));
+7 -3
View File
@@ -11,8 +11,11 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import edu.wpi.first.wpilibj.PWM;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.robot.Constants.ArmConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.subsystems.SwerveModule;
@@ -168,6 +171,7 @@ public class RobotMap {
}
// claw stuff (WHAT IS A SOAP ENGINEER)
PWM servo = new PWM(0);
}
PWM leftClaw = new PWM(0);
PWM rightClaw = new PWM(1);
CANSparkMax spinnyspin = new CANSparkMax(18, MotorType.kBrushless);
}
@@ -1,23 +1,44 @@
package frc4388.robot.subsystems;
import edu.wpi.first.hal.PWMJNI;
import java.util.Timer;
import java.util.TimerTask;
import com.revrobotics.CANSparkMax;
import edu.wpi.first.wpilibj.PWM;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Claw extends SubsystemBase {
private PWM m_clawMotor;
private boolean m_open = false;
private boolean m_disabled = false;
private final PWM m_leftMotor;
private final PWM m_rightMotor;
private final CANSparkMax m_spinnyspin;
private boolean m_open = false;
// Opens claw
public Claw(PWM m_clawMotor) {
this.m_clawMotor = m_clawMotor;
setClaw(true);
public Claw(PWM leftMotor, PWM rightMotor, CANSparkMax spinnyspin) {
m_leftMotor = leftMotor;
m_rightMotor = rightMotor;
m_spinnyspin = spinnyspin;
setClaw(true);
}
public void setClaw(boolean open) {
// Open claw
m_open = open;
m_clawMotor.setRaw(m_open ? 1000 : 2000);
m_leftMotor.setRaw(m_open ? 1000 : 2000);
m_rightMotor.setRaw(m_open ? 2000 : 1000);
if (!m_open) {
m_spinnyspin.set(-0.2);
new Timer().schedule(new TimerTask() {
@Override
public void run() {
nospinnyspin();
}
}, 750);
}
}
public void toggle() {
@@ -28,4 +49,11 @@ public class Claw extends SubsystemBase {
return m_open;
}
public void yesspinnyspin() {
m_spinnyspin.set(0.2);
}
public void nospinnyspin() {
m_spinnyspin.set(0);
}
}