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

commit

Browse Source
This commit is contained in:
Aarav
2023-03-11 14:03:36 -07:00
parent dfc6978186
commit 5521349510
7 changed files with 143 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Arm.java
+64 -6
View File
@@ -10,6 +10,8 @@ import com.ctre.phoenix.sensors.CANCoderConfiguration;
import frc4388.robot.Constants.ArmConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Arm extends SubsystemBase {
@@ -42,6 +44,32 @@ public class Arm extends SubsystemBase {
CANCoderConfiguration config = new CANCoderConfiguration();
config.magnetOffsetDegrees = ArmConstants.OFFSET;
m_pivotEncoder.configAllSettings(config);
// m_pivot.configFactoryDefault();
// m_pivot.clearStickyFaults();
// m_pi
SmartDashboard.putNumber("kP Pivot", 0);
SmartDashboard.putNumber("kI Pivot", 0);
SmartDashboard.putNumber("kD Pivot", 0);
SmartDashboard.putData("Set PID", new InstantCommand(() -> {
TalonFXConfiguration pivotConfig_ = new TalonFXConfiguration();
pivotConfig_.slot0.kP = SmartDashboard.getNumber("kP Pivot", 0);
pivotConfig_.slot0.kI = SmartDashboard.getNumber("kI Pivot", 0);
pivotConfig_.slot0.kD = SmartDashboard.getNumber("kD Pivot", 0);
// pivotConfig_.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
// pivotConfig_.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
// pivotConfig_.configSelectedFeedbackFilter();// FeedbackDevice.RemoteSensor0;
m_pivot.configAllSettings(pivotConfig_);
m_pivot.configRemoteFeedbackFilter(encoder.getDeviceID(), RemoteSensorSource.CANCoder, 0);
m_pivot.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0);
// m_pivot.selectProfileSlot(0, 0);
}));
SmartDashboard.putData("set pos 1", new RunCommand(() -> m_pivot.set(ControlMode.Position, 225), this));
SmartDashboard.putData("set pos 2", new RunCommand(() -> m_pivot.set(ControlMode.Position, 135), this));
SmartDashboard.putData("Kill Self", new InstantCommand(() -> {}, this));
}
public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) {
@@ -49,11 +77,11 @@ public class Arm extends SubsystemBase {
}
public void setRotVel(double vel) {
m_pivot.set(ControlMode.PercentOutput, vel / 5);
m_pivot.set(ControlMode.PercentOutput, .4 * vel);
}
public void setTeleVel(double vel) {
m_tele.set(ControlMode.PercentOutput, -0.25 * vel);
m_tele.set(ControlMode.PercentOutput, -0.5 * vel);
}
public void armSetRotation(double rot) {
@@ -129,7 +157,8 @@ public class Arm extends SubsystemBase {
tele_softLimit = !tele_softLimit;
}
boolean resetable = true;
boolean resetable = true;
boolean tele_reset = true;
@Override
public void periodic() {
@@ -141,8 +170,8 @@ public class Arm extends SubsystemBase {
SmartDashboard.putNumber("start pivot", pivot_soft);
SmartDashboard.putNumber("start tele", tele_soft);
m_pivot.configForwardSoftLimitEnable(true);
m_pivot.configReverseSoftLimitEnable(true);
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;
@@ -150,12 +179,41 @@ public class Arm extends SubsystemBase {
resetable = true;
}
if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) {
var tele_soft = m_tele.getSelectedSensorPosition();
m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
m_tele.configReverseSoftLimitThreshold(1000 - tele_soft);
m_tele.configForwardSoftLimitEnable(true);
m_tele.configReverseSoftLimitEnable(true);
tele_reset = false;
} else if (m_tele.isFwdLimitSwitchClosed() == 0) {
tele_reset = true;
}
SmartDashboard.putBoolean("reverse", m_tele.isFwdLimitSwitchClosed() == 1);
double x = Math.cos(degrees);
SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MIN_ROT_SPEED + x * (SwerveDriveConstants.MAX_ROT_SPEED - SwerveDriveConstants.MIN_ROT_SPEED);
// SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MIN_ROT_SPEED + x * (SwerveDriveConstants.MAX_ROT_SPEED - SwerveDriveConstants.MIN_ROT_SPEED);
// if (m_debug)
// SmartDashboard.putNumber("Arm Motor", m_tele.getSelectedSensorPosition());
SmartDashboard.putNumber("Pivot CANCoder", m_pivotEncoder.getAbsolutePosition());
SmartDashboard.putNumber("Pivot IntegratedSensor", m_pivot.getSelectedSensorPosition());
SmartDashboard.putNumber("Telescope Encoder", m_tele.getSelectedSensorPosition());
}
boolean soft_limits = true;
public void killSoftLimits() {
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);
soft_limits = !soft_limits;
}
}
src/main/java/frc4388/robot/subsystems/Claw.java
+25 -2
View File
@@ -1,10 +1,12 @@
package frc4388.robot.subsystems;
import edu.wpi.first.hal.PWMJNI;
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_open = false;
private boolean m_disabled = false;
// Opens claw
public Claw(PWM m_clawMotor) {
@@ -13,16 +15,37 @@ public class Claw extends SubsystemBase {
}
public void setClaw(boolean open) {
if (m_disabled) return;
// Open claw
// m_clawMotor.setRaw(150);
m_open = open;
m_clawMotor.setRaw(open ? 0 : 2000);
System.out.println("setClaw()");
// m_clawMotor.setPosition(0.5);
// m_clawMotor.setRaw(0);
// m_clawMotor.setRaw(m_open ? 0 : 255);
// m_clawMotor.setSpeed(m_open ? -1 : 1);
PWMJNI.setPWMSpeed(m_clawMotor.getHandle(), m_open ? -1 : 1);
// PWMJNI.setPWMDisabled(0);
System.out.println("Claw Pos: " + m_clawMotor.getRaw());
}
public void toggle() {
System.out.println("toggle()");
setClaw(!m_open);
}
public boolean isClawOpen() {
return m_open;
}
public void disable() {
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());
}
public void enable() {
m_disabled = false;
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+17 -7
View File
@@ -82,19 +82,21 @@ public class SwerveDrive extends SubsystemBase {
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
double rot = 0;
if (rightStick.getNorm() > 0.1) {
rotTarget = gyro.getRotation2d();
rot = rightStick.getX();
} else {
rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
}
double rot = rightStick.getX();
// Use the left joystick to set speed. Apply a quadratic curve and the set max speed.
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
} else {
// Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
@@ -235,8 +237,16 @@ public class SwerveDrive extends SubsystemBase {
* Shifts gear from high to low, or vice versa.
* @param shift true to shift to high, false to shift to low
*/
public void highSpeed(boolean shift) {
this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
// public void highSpeed(boolean shift) {
// this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
// }
public void toggleGear() {
if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
} else {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
}
}
}