From f3591476b12c5ea20bef54741841018b3e8415e2 Mon Sep 17 00:00:00 2001 From: Dave Staudacher Date: Tue, 6 Mar 2018 03:04:23 -0700 Subject: [PATCH] Set neutral mode to coast for teleop also added explanatory comments around the teleop gyro lock mode code --- src/org/usfirst/frc4388/robot/Robot.java | 1 + .../frc4388/robot/subsystems/Drive.java | 33 +++++++++++-------- 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/src/org/usfirst/frc4388/robot/Robot.java b/src/org/usfirst/frc4388/robot/Robot.java index 2d363a5..a7b1e62 100644 --- a/src/org/usfirst/frc4388/robot/Robot.java +++ b/src/org/usfirst/frc4388/robot/Robot.java @@ -289,6 +289,7 @@ public class Robot extends IterativeRobot if (RLautonomousCommand != null) RLautonomousCommand.cancel(); if (LRautonomousCommand != null) LRautonomousCommand.cancel(); if (LLautonomousCommand != null) LLautonomousCommand.cancel(); + drive.setToBrakeOnNeutral(false); // coast to avoid tipping when driver stops suddenly //MotionProfileCache.getInstance().release(); updateChoosers(); controlLoop.start(); diff --git a/src/org/usfirst/frc4388/robot/subsystems/Drive.java b/src/org/usfirst/frc4388/robot/subsystems/Drive.java index fb03f70..fb612a4 100644 --- a/src/org/usfirst/frc4388/robot/subsystems/Drive.java +++ b/src/org/usfirst/frc4388/robot/subsystems/Drive.java @@ -286,6 +286,20 @@ public class Drive extends Subsystem implements ControlLoopable } } + public void setToBrakeOnNeutral(boolean brakeVsCoast) { + if (brakeVsCoast) { + leftDrive1.setNeutralMode(NeutralMode.Brake); + leftDrive2.setNeutralMode(NeutralMode.Brake); + rightDrive1.setNeutralMode(NeutralMode.Brake); + rightDrive2.setNeutralMode(NeutralMode.Brake); + } else { + leftDrive1.setNeutralMode(NeutralMode.Coast); + leftDrive2.setNeutralMode(NeutralMode.Coast); + rightDrive1.setNeutralMode(NeutralMode.Coast); + rightDrive2.setNeutralMode(NeutralMode.Coast); + } + } + @Override public void initDefaultCommand() { } @@ -623,20 +637,13 @@ public class Drive extends Subsystem implements ControlLoopable m_moveOutput = climbSpeed; } - /* - 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); + if (useGyroLock) { // If currently in gyro lock mode, + if ((m_moveInput == 0.0) || (m_steerInput != 0.0)) { // but stopped driving or started turning, + setGyroLock(false, false); // turn off gyro lock mode } - } else { - if ((m_moveInput != 0.0) && (m_steerInput == 0.0)) { - setGyroLock(true, false); + } else { // If not yet in gyro lock mode, + if ((m_moveInput != 0.0) && (m_steerInput == 0.0)) { // but just started driving without turning, + setGyroLock(true, false); // gyro lock to current heading } }