Team4388/2019-Hit-or-Miss
1
0
Fork 0
mirror of https://github.com/Team4388/2019-Hit-or-Miss.git synced 2026-06-09 00:28:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Create new gyro lock code

Browse Source 
Using Joystick_Straight control mode
This commit is contained in:
Keenan D. Buckley
2019-03-07 19:54:33 -07:00
parent 6017618833
commit d33379e1e5
1 changed files with 12 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java
+12 -1
View File
@@ -61,7 +61,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
*/
public class Drive extends Subsystem implements ControlLoopable
{
public static enum DriveControlMode { JOYSTICK, MP_STRAIGHT, MP_TURN, PID_TURN, HOLD, MANUAL, CLIMB, MP_PATH, MP_PATH_VELOCITY, MOTION_MAGIC, ADAPTIVE_PURSUIT, MOVE_POSITION_MAX_SCALE, MOVE_POSITION_LOW_SCALE, MOVE_POSITION_SWITCH, MOVE_POSITION_LOWEST, STOP_MOTORS, RAW};
public static enum DriveControlMode { JOYSTICK, MP_STRAIGHT, MP_TURN, PID_TURN, HOLD, MANUAL, CLIMB, MP_PATH, MP_PATH_VELOCITY, MOTION_MAGIC, ADAPTIVE_PURSUIT, MOVE_POSITION_MAX_SCALE, MOVE_POSITION_LOW_SCALE, MOVE_POSITION_SWITCH, MOVE_POSITION_LOWEST, STOP_MOTORS, RAW, JOYSTICK_STRAIGHT};
public static enum SpeedShiftState { HI, LO };
public static enum ClimberState { DEPLOYED, RETRACTED };
@@ -680,12 +680,23 @@ public class Drive extends Subsystem implements ControlLoopable
m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim,
m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY);
if(m_steerInput == 0 && !gyroNavX.isRotating()){
controlMode = DriveControlMode.JOYSTICK_STRAIGHT;
setGyroLock();
}
}
else if (controlMode == DriveControlMode.CLIMB) {
m_moveOutput = climbSpeed;
}
else if (controlMode == DriveControlMode.JOYSTICK_STRAIGHT) {
double maxYawError = 5; ///TODO: Put this somehwere else
double yawError = gyroLockAngleDeg - getGyroAngleDeg();
m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim,
m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY);
m_steerOutput = kPGyro * yawError;
if(m_steerInput != 0 || yawError > maxYawError){
controlMode = DriveControlMode.JOYSTICK;
}
}
m_drive.arcadeDrive(m_moveOutput, m_steerOutput*.75);