Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

custom pid for shoot command, needs testing

Browse Source
This commit is contained in:
aarav18
2022-02-26 15:00:09 -07:00
parent a358492c06
commit 343f423bb2
1 changed files with 103 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/Shoot.java
+103 -7
View File
@@ -4,44 +4,139 @@
package frc4388.robot.commands;
import java.nio.file.SecureDirectoryStream;
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Hood;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
public class Shoot extends CommandBase {
// subsystems
public SwerveDrive m_swerve;
public BoomBoom m_boomBoom;
public Turret m_turret;
public Hood m_hood;
public double m_targetVel;
public double m_targetHood;
// given
public double m_gyroAngle;
public double m_odoX;
public double m_odoY;
public double m_distance;
// targets
public double m_targetVel;
public double m_targetHood;
public double m_targetAngle;
public double m_driveTargetAngle;
// pid
public double error;
public double prevError;
public double kP, kI, kD;
public double integral, derivative;
public double time;
public double output;
public double tolerance = 5.0;
// // dummy motor
// public WPI_TalonFX dummy = new WPI_TalonFX(69 - 420);
// public TalonFXConfiguration dummyConfiguration = new TalonFXConfiguration();
/** Creates a new Shoot. */
public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Hood sHood) {
public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) {
// Use addRequirements() here to declare subsystem dependencies.
m_swerve = sDrive;
m_boomBoom = sShooter;
m_turret = sTurret;
m_hood = sHood;
addRequirements(m_swerve, m_boomBoom, m_hood);
addRequirements(m_swerve, m_boomBoom, m_turret, m_hood);
kP = 0.1;
kI = 0.0;
kD = 0.0;
integral = 0;
derivative = 0;
time = 0.02;
}
/**
* Updates error for custom PID.
*/
public void updateError() {
error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
m_distance = 0; //TODO: get this value using odometry
m_odoX = 0; //TODO: get this value using odometry
m_odoY = 0; //TODO: get this value using odometry
m_distance = Math.sqrt(Math.pow(m_odoX, 2) + Math.pow(m_odoY, 2));
// get targets (shooter tables)
m_targetVel = m_boomBoom.getVelocity(m_distance);
m_targetHood = m_boomBoom.getHood(m_distance);
m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.;
m_driveTargetAngle = m_targetAngle + m_turret.getBoomBoomAngleDegrees();
// // normal (i think) PID stuff
// dummyConfiguration.primaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.IntegratedSensor.toFeedbackDevice();
// dummyConfiguration.remoteFilter0.remoteSensorDeviceID = dummy.getDeviceID();
// dummyConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor;
// dummyConfiguration.slot0.kP = 0.1;
// dummyConfiguration.slot0.kI = 0;
// dummyConfiguration.slot0.kD = 0;
// dummyConfiguration.slot0.kF = 0;
// // weird PID stuff
// dummyConfiguration.auxiliaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.SoftwareEmulatedSensor.toFeedbackDevice();
// dummyConfiguration.remoteFilter1.remoteSensorDeviceID = ShooterConstants.TURRET_MOTOR_CAN_ID;
// dummyConfiguration.remoteFilter1.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor;
// // dummyConfiguration.auxiliaryPID.selectedFeedbackCoefficient = 0;
// dummyConfiguration.slot1.kP = 0.1;
// dummyConfiguration.slot1.kI = 0;
// dummyConfiguration.slot1.kD = 0;
// dummyConfiguration.slot1.kF = 0;
// dummy.configAllSettings(dummyConfiguration);
// initial error
updateError();
prevError = error;
}
/**
* Run custom PID.
*/
public void runPID() {
prevError = error;
updateError();
integral = integral + error * time;
derivative = (error - prevError) / time;
output = kP * error + kI * integral + kD * derivative;
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
// dummy.selectProfileSlot(0, 0);
// dummy.selectProfileSlot(1, 1);
// dummy.set(TalonFXControlMode.Position, m_driveTargetAngle, DemandType.AuxPID, m_targetAngle);
// m_swerve.driveWithInput(0, 0, m_driveTargetAngle, true);
// m_swerve.driveWithInput(0, 0, Math.cos(m_driveTargetAngle), Math.sin(m_driveTargetAngle), true); // only works for new DWI in swerve branch
// custom pid
runPID();
m_swerve.driveWithInput(0, 0, output, true);
m_hood.runAngleAdjustPID(m_targetHood);
m_boomBoom.runDrumShooterVelocityPID(m_targetVel);
}
@@ -53,6 +148,7 @@ public class Shoot extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
updateError();
return Math.abs(error) <= tolerance;
}
}