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

Merge branch 'fix-dead-assist' into add-ramsete-controller-and-trajectories

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-02-17 13:55:29 -07:00
parent c39ec549a9 1aa9037285
commit 3cdeec01eb
2 changed files with 34 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java
+33 -26
View File
@@ -15,10 +15,9 @@ import frc4388.utility.controller.IHandController;
public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
Drive m_drive;
double m_targetGyro;
long lastTime;
double m_targetGyro, m_currentGyro;
long m_lastTime, m_deltaTime;
IHandController m_controller;
boolean isAuxPIDEnabled = false;
/**
* Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller.
@@ -30,7 +29,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
* {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer}
*/
public DriveWithJoystickUsingDeadAssistPID(Drive subsystem, IHandController controller) {
// Use addRequirements() here to declare subsystem dependencies.
// Use addRequirements() here to declare subsystem dependencies.
m_drive = subsystem;
m_controller = controller;
addRequirements(m_drive);
@@ -39,32 +38,19 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
// Called when the command is initially scheduled.
@Override
public void initialize() {
lastTime = System.currentTimeMillis();
m_lastTime = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
double moveInput = m_controller.getLeftYAxis();
m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
double moveInput = -m_controller.getLeftYAxis();
double steerInput = m_controller.getRightXAxis();
double moveOutput = 0;
double steerOutput = 0;
long deltaTime = System.currentTimeMillis() - lastTime;
lastTime = System.currentTimeMillis();
/* If AuxPID is enabled, then update using the steer input */
if (isAuxPIDEnabled) {
m_targetGyro += 2 * steerInput * deltaTime;
m_targetGyro = MathUtil.clamp( m_targetGyro,
currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4),
currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4));
}
/* Otherwise set target angle to current angle (prevents buildup of gyro error) */
else {
m_targetGyro = currentGyro;
}
m_deltaTime = System.currentTimeMillis() - m_lastTime;
m_lastTime = System.currentTimeMillis();
/* If move stick is being used */
if (moveInput != 0) {
@@ -85,22 +71,43 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
} else {
steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
}
resetGyroTarget();
m_drive.driveWithInput(moveOutput, steerOutput);
isAuxPIDEnabled = false;
System.out.println("Driving With Input");
}
/* If only the move stick is being used */
else {
updateGyroTarget(steerInput);
m_drive.driveWithInputAux(moveOutput, m_targetGyro);
isAuxPIDEnabled = true;
System.out.println("Driving with Input Aux with Target: " + m_targetGyro);
}
}
/* If the move stick is not being used */
else {
m_drive.runDriveVelocityPID(0, m_targetGyro);
isAuxPIDEnabled = true;
updateGyroTarget(steerInput);
m_drive.runDriveStraightVelocityPID(0, m_targetGyro);
System.out.println("Driving with Velocity PID with Target: " + m_targetGyro);
}
}
/**
* If AuxPID is enabled, then update using the steer input
*/
private void updateGyroTarget(double steerInput) {
m_targetGyro += 2 * steerInput * m_deltaTime;
m_targetGyro = MathUtil.clamp( m_targetGyro,
m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4),
m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4));
}
/**
* set target angle to current angle (prevents buildup of gyro error).
*/
private void resetGyroTarget() {
m_targetGyro = m_currentGyro;
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {