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

added requests

Browse Source 
non field relative, but secretly field relative rotation
Arm speeds
This commit is contained in:
Aarav
2023-03-06 16:35:20 -07:00
parent 9111eefe68
commit 7d7d9898c9
5 changed files with 66 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Arm.java
+47 -10
View File
@@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
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.SubsystemBase;
@@ -23,6 +24,9 @@ public class Arm extends SubsystemBase {
m_pivot = pivot;
m_pivotEncoder = encoder;
tele.configFactoryDefault();
m_pivot.configFactoryDefault();
TalonFXConfiguration pivotConfig = new TalonFXConfiguration();
pivotConfig.slot0.kP = ArmConstants.kP;
pivotConfig.slot0.kI = ArmConstants.kI;
@@ -31,19 +35,13 @@ public class Arm extends SubsystemBase {
pivotConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
pivotConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
pivotConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
pivot.configAllSettings(pivotConfig);
m_pivot.configAllSettings(pivotConfig);
pivot.configForwardSoftLimitThreshold(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
pivot.configReverseSoftLimitThreshold(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
pivot.configForwardSoftLimitEnable(true);
pivot.configReverseSoftLimitEnable(true);
resetTeleSoftLimit();
CANCoderConfiguration config = new CANCoderConfiguration();
config.magnetOffsetDegrees = ArmConstants.OFFSET;
m_pivotEncoder.configAllSettings(config);
tele.configFactoryDefault();
pivot.configFactoryDefault();
}
public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) {
@@ -55,7 +53,7 @@ public class Arm extends SubsystemBase {
}
public void setTeleVel(double vel) {
m_tele.set(ControlMode.PercentOutput, vel);
m_tele.set(ControlMode.PercentOutput, -0.25 * vel);
}
public void armSetRotation(double rot) {
@@ -115,10 +113,49 @@ public class Arm extends SubsystemBase {
return true;
}
boolean tele_softLimit = false;
public void resetTeleSoftLimit() {
if (!tele_softLimit) {
var tele_soft = m_tele.getSelectedSensorPosition();
m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
m_tele.configReverseSoftLimitThreshold(tele_soft);
m_tele.configForwardSoftLimitEnable(true);
m_tele.configReverseSoftLimitEnable(true);
} else {
m_tele.configForwardSoftLimitEnable(false);
m_tele.configReverseSoftLimitEnable(false);
}
tele_softLimit = !tele_softLimit;
}
boolean resetable = true;
@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(true);
m_pivot.configReverseSoftLimitEnable(true);
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;
}
double x = Math.cos(degrees);
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 AbsPos", m_pivotEncoder.getAbsolutePosition());
SmartDashboard.putNumber("Pivot CANCoder", m_pivotEncoder.getAbsolutePosition());
SmartDashboard.putNumber("Pivot IntegratedSensor", m_pivot.getSelectedSensorPosition());
SmartDashboard.putNumber("Telescope Encoder", m_tele.getSelectedSensorPosition());
}
}