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/Constants.java
+12
View File
@@ -87,6 +87,18 @@ public final class Constants {
public static final double MIN_ARM_LENGTH = Math.abs(LOWER_ARM_LENGTH + UPPER_ARM_LENGTH);
public static final double MOVE_SPEED = 50; // cm per second
// PID Constants
public static final int SHOULDER_SLOT_IDX = 0;
public static final int SHOULDER_PID_LOOP_IDX = 1;
public static final int ELBOW_SLOT_IDX = 0;
public static final int ELBOW_PID_LOOP_IDX = 1;
public static final Gains SHOULDER_GAINS = new Gains(1.0, 1.0, 1.0, 1.0, 0, 1.0);
public static final Gains ELBOW_GAINS = new Gains(1.0, 1.0, 1.0, 1.0, 0, 1.0);
public static final int CLIMBER_TIMEOUT_MS = 100;
}
/**
* The OIConstants class contains the ID for the XBox controllers
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) {