Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

artificial soft limits

Browse Source
This commit is contained in:
Abhishrek05
2023-03-14 14:26:47 -06:00
parent 29c6a29702
commit 03c938348f
3 changed files with 22 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+3 -2
View File
@@ -23,6 +23,7 @@ import frc4388.robot.subsystems.Arm;
import frc4388.robot.subsystems.Claw;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.commands.JoystickRecorder;
import frc4388.robot.commands.PivotCommand;
import frc4388.robot.commands.PlaybackChooser;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.XboxController;
@@ -145,13 +146,13 @@ public class RobotContainer {
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
.whileTrue(new RunCommand(() -> m_robotArm.m_pivot.set(TalonFXControlMode.Position, 200), m_robotArm));
.whileTrue(new RunCommand(() -> new PivotCommand(m_robotArm, 135)));
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
.whileTrue(new RunCommand(() -> m_robotArm.m_pivot.set(TalonFXControlMode.Position, 135), m_robotArm));
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
.whileTrue(new RunCommand(() -> m_robotArm.m_pivot.set(TalonFXControlMode.Position, 225), m_robotArm));
.onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
src/main/java/frc4388/robot/subsystems/Arm.java
+14 -18
View File
@@ -18,7 +18,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Arm extends SubsystemBase {
private WPI_TalonFX m_tele;
public WPI_TalonFX m_pivot;
public WPI_TalonFX m_pivot;
private CANCoder m_pivotEncoder;
private boolean m_debug;
@@ -32,7 +32,7 @@ public class Arm extends SubsystemBase {
m_pivot.configFactoryDefault();
// * Example of deferred code
new DeferredBlock(() -> resetTeleSoftLimit());
// new DeferredBlock(() -> resetTeleSoftLimit());
// TalonFXConfiguration pivotConfig = new TalonFXConfiguration();
// pivotConfig.slot0.kP = 0.5;//ArmConstants.kP;
@@ -84,7 +84,17 @@ public class Arm extends SubsystemBase {
}
public void setRotVel(double vel) {
m_pivot.set(ControlMode.PercentOutput, .4 * vel);
var degrees = Math.abs(getArmRotation()) - 135;
SmartDashboard.putNumber("arm degrees", degrees);
SmartDashboard.putNumber("arm rot vel", vel);
if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) {
m_pivot.set(ControlMode.PercentOutput, 0);
} else if (degrees > 90 && vel > 0) {
m_pivot.set(ControlMode.PercentOutput, .2 * vel);
} else {
m_pivot.set(ControlMode.PercentOutput, .4 * vel);
}
}
public void setTeleVel(double vel) {
@@ -167,21 +177,6 @@ public class Arm extends SubsystemBase {
@Override
public void periodic() {
double degrees = Math.abs(m_pivotEncoder.getAbsolutePosition() - 135);
if (degrees < 2 && resetable) {
var pivot_soft = m_pivot.getSelectedSensorPosition();
var tele_soft = m_tele.getSelectedSensorPosition();
SmartDashboard.putNumber("start pivot", pivot_soft);
SmartDashboard.putNumber("start tele", tele_soft);
m_pivot.configForwardSoftLimitEnable(soft_limits);
m_pivot.configReverseSoftLimitEnable(soft_limits);
SmartDashboard.putNumber("fwd err", m_pivot.configForwardSoftLimitThreshold(1200 + pivot_soft).value);
SmartDashboard.putNumber("rvs err", m_pivot.configReverseSoftLimitThreshold(pivot_soft).value);
resetable = false;
} else if (degrees > 2) {
resetable = true;
}
if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) {
var tele_soft = m_tele.getSelectedSensorPosition();
@@ -207,6 +202,7 @@ public class Arm extends SubsystemBase {
boolean soft_limits = true;
public void killSoftLimits() {
resetTeleSoftLimit();
var pivot_soft = m_pivot.getSelectedSensorPosition();
var tele_soft = m_tele.getSelectedSensorPosition();
src/main/java/frc4388/robot/subsystems/Claw.java
+5 -5
View File
@@ -22,9 +22,9 @@ public class Claw extends SubsystemBase {
System.out.println("setClaw()");
// m_clawMotor.setPosition(0.5);
// m_clawMotor.setRaw(0);
// m_clawMotor.setRaw(m_open ? 0 : 255);
m_clawMotor.setRaw(m_open ? 1000 : 2000);
// m_clawMotor.setSpeed(m_open ? -1 : 1);
PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), m_open ? -1 : 1);
// PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), m_open ? -1 : 1);
// PWMJNI.setPWMDisabled(0);
System.out.println("Claw Pos: " + m_clawMotor.getRaw());
}
@@ -39,9 +39,9 @@ public class Claw extends SubsystemBase {
}
public void disable() {
m_disabled = true;
// PWMJNI.setPWMRaw(m_clawMotor.getHandle(), PWMJNI.getPWMRaw(m_clawMotor.getHandle()));
PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), 0.5);
// m_disabled = true;
// // PWMJNI.setPWMRaw(m_clawMotor.getHandle(), PWMJNI.getPWMRaw(m_clawMotor.getHandle()));
// PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), 0.5);
// PWMJNI.setPWMDisabled(m_clawMotor.getHandle());
}