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 AHRS gyroNavX;
private boolean useGyroLock; private boolean useGyroLock;
private double gyroLockAngleDeg; private double gyroLockAngleDeg;
private double kPGyro = 0.04; //private double kPGyro = 0.04;
private double kPGyro = 0.0625;
private boolean isCalibrating = false; private boolean isCalibrating = false;
private double gyroOffsetDeg = 0; private double gyroOffsetDeg = 0;
@@ -621,12 +622,30 @@ public class Drive extends Subsystem implements ControlLoopable
else if (controlMode == DriveControlMode.CLIMB) { else if (controlMode == DriveControlMode.CLIMB) {
m_moveOutput = climbSpeed; 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) { if (useGyroLock) {
double yawError = gyroLockAngleDeg - getGyroAngleDeg(); double yawError = gyroLockAngleDeg - getGyroAngleDeg();
m_steerOutput = kPGyro * yawError; 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); m_drive.arcadeDrive(m_moveOutput, m_steerOutput*.85);