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

Added Input control and PIDs

Browse Source 
Still need to configure PIDs
PIDs are currently set to max values.
Please do not run.
This commit is contained in:
dj12ha
2022-01-20 18:13:11 -07:00
parent 94a0af64d0
commit 7949205cc4
2 changed files with 43 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Climber.java
+31 -2
View File
@@ -4,6 +4,8 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
@@ -16,11 +18,17 @@ public class Climber extends SubsystemBase {
private WPI_PigeonIMU m_gyro;
private double[] position = {0.d, 0.d};
private double[] position = {0.d, 0.d}; // TODO change position
public Climber(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_PigeonIMU gyro) {
m_shoulder = shoulder;
m_shoulder.configFactoryDefault();
m_shoulder.setNeutralMode(NeutralMode.Brake);
m_elbow = elbow;
m_elbow.configFactoryDefault();
m_elbow.setNeutralMode(NeutralMode.Brake);
m_gyro = gyro;
}
@@ -82,12 +90,33 @@ public class Climber extends SubsystemBase {
return Math.toRadians(ypr[1]); // Pitch
}
public void setClimberGains() {
// shoulder PIDs
m_shoulder.selectProfileSlot(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_PID_LOOP_IDX);
m_shoulder.config_kF(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.m_kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_shoulder.config_kP(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.m_kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_shoulder.config_kI(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.m_kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_shoulder.config_kD(ClimberConstants.SHOULDER_SLOT_IDX, ClimberConstants.SHOULDER_GAINS.m_kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
// elbow PIDs
m_elbow.selectProfileSlot(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_PID_LOOP_IDX);
m_elbow.config_kF(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.m_kF, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_elbow.config_kP(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.m_kP, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_elbow.config_kI(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.m_kI, ClimberConstants.CLIMBER_TIMEOUT_MS);
m_elbow.config_kD(ClimberConstants.ELBOW_SLOT_IDX, ClimberConstants.ELBOW_GAINS.m_kD, ClimberConstants.CLIMBER_TIMEOUT_MS);
}
public void setJointAngles(double[] angles) {
setJointAngles(angles[0], angles[1]);
}
public void setJointAngles(double shoulderAngle, double elbowAngle) {
// Set PIDs
// Convert radians to ticks
double shoulderPosition = (shoulderAngle * 1024.d) / Math.PI;
double elbowPosition = (elbowAngle * 1024.d) / Math.PI;
m_shoulder.set(TalonFXControlMode.Position, shoulderPosition);
m_elbow.set(TalonFXControlMode.Position, elbowPosition);
}
public void controlWithInput(double xInput, double yInput) {