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

Add Teleop Gyro Lock

Browse Source
This commit is contained in:
HFocus
2018-03-05 20:52:31 -07:00
parent 6efa60ec6b
commit 9a896e2021
1 changed files with 22 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/org/usfirst/frc4388/robot/subsystems/Drive.java
+22 -3
View File
@@ -193,7 +193,8 @@ public class Drive extends Subsystem implements ControlLoopable
private AHRS gyroNavX;
private boolean useGyroLock;
private double gyroLockAngleDeg;
private double kPGyro = 0.04;
//private double kPGyro = 0.04;
private double kPGyro = 0.0625;
private boolean isCalibrating = false;
private double gyroOffsetDeg = 0;
@@ -621,12 +622,30 @@ public class Drive extends Subsystem implements ControlLoopable
else if (controlMode == DriveControlMode.CLIMB) {
m_moveOutput = climbSpeed;
}
m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
/*
if (m_steerInput == 0.0 && m_moveInput != 0.0 && !useGyroLock) { // If just stopped turning, turn on gyro lock
setGyroLock(true, false);
} else if (m_steerInput != 0.0 && useGyroLock){ // If just started turning, turn off gyro lock
setGyroLock(false, false);
} */
if (useGyroLock) {
if ((m_moveInput == 0.0) || (m_steerInput != 0.0)) {
setGyroLock(false, false);
}
} else {
if ((m_moveInput != 0.0) && (m_steerInput == 0.0)) {
setGyroLock(true, false);
}
}
if (useGyroLock) {
double yawError = gyroLockAngleDeg - getGyroAngleDeg();
m_steerOutput = kPGyro * yawError;
} else {
m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
}
m_drive.arcadeDrive(m_moveOutput, m_steerOutput*.85);