Skip to content

Commit b52eed6

Browse files
henrymwestfallhenrymwestfall
authored
Merge pull request fairviewrobotics#34 from henrymwestfall/master
Better mode changing controls
2 parents 1f159ea + 19663ac commit b52eed6

File tree

6 files changed

+189
-20
lines changed

6 files changed

+189
-20
lines changed

‎src/main/kotlin/frc/robot/Constants.kt‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -117,7 +117,12 @@ class Constants {
117117
val kTrackwidthMeters = 0.558
118118
val kDriveKinematics = DifferentialDriveKinematics(kTrackwidthMeters)
119119

120-
val ballHeightForAutoIntake = 100.0
120+
val kballHeightForAutoIntake = 0.33 // a third of the screen
121+
val kBallAreaThreshold = 0.1
122+
123+
val kDriveToggleHoldTime = 5 // seconds to hold button to change drive mode
124+
val kDriveModeChangeRumbleTime = 1
125+
val kDriveModeChangingOn = true // turn this off to prevent changing
121126

122127
val constants = mutableMapOf(
123128
"DrivetrainPID_P" to 0.035,

‎src/main/kotlin/frc/robot/RobotContainer.kt‎

Lines changed: 31 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -77,7 +77,10 @@ class RobotContainer {
7777
/*** --- commands --- ***/
7878
//drive by a joystick (controller1)
7979
val XboxDriveCommand = XboxDrive(drivetrain, controller1)
80-
val XboxDriveSpecial = XboxDrive(drivetrain, controller1)
80+
val XboxDriveSpecial = XboxDriveSpecial(drivetrain, controller1)
81+
val driveModeToggle = DriveModeToggleSubsystem(drivetrain, controller1,
82+
listOf(XboxDriveCommand, XboxDriveSpecial), 0
83+
)
8184

8285
/** -- 0 point autos -- **/
8386
val noAuto = DriveDoubleSupplier(drivetrain, { 0.0 }, { 0.0 })
@@ -227,7 +230,11 @@ class RobotContainer {
227230
/* setup default commands */
228231
drivetrain.defaultCommand = XboxDriveCommand
229232
//drivetrain.defaultCommand = XboxDriveSpecial
230-
233+
driveModeToggle.defaultCommand = ChangeDriveMode(driveModeToggle) {
234+
Constants.kDriveModeChangingOn && controller1.aButton
235+
}
236+
237+
231238
/* default gate - run forward on X, backwards on A
232239
* If left bumper held, run until a ball is seen by the sensor
233240
*/
@@ -268,23 +275,32 @@ class RobotContainer {
268275

269276
lights.defaultCommand = setAlliance
270277

271-
/* toggle vision mode when start is pressed on controller0 */
272-
visionToggle.defaultCommand = VisionModeChange(visionToggle, {
273-
val visionMode = {if (controller0.getStartButtonPressed()) {
274-
when (visionToggle.visionMode) {
275-
VisionModes.BALL -> VisionModes.HIGHGOAL
276-
VisionModes.HIGHGOAL -> VisionModes.BALL
278+
/* toggle vision mode with D-Pad (pov) and toggle
279+
auto intake with start and back on controller0.
280+
controller1 can also change vision mode */
281+
visionToggle.defaultCommand = VisionModeChange(visionToggle) {
282+
val visionMode = {
283+
when (controller1.pov) {
284+
90 -> VisionModes.HIGHGOAL // right
285+
270 -> VisionModes.BALL // left
286+
else -> when (controller0.pov) {
287+
90 -> VisionModes.HIGHGOAL // right
288+
270 -> VisionModes.BALL // left
289+
else -> visionToggle.visionMode
290+
}
277291
}
278-
} else {
279-
visionToggle.visionMode
280-
} }
292+
}
281293

282-
val autoIntakeOn = {if (controller0.getBackButtonPressed()) {
283-
!visionToggle.visionIntakeOn
284-
} else visionToggle.visionIntakeOn }
294+
val autoIntakeOn = {
295+
when {
296+
controller0.startButtonPressed -> true
297+
controller0.backButtonPressed -> false
298+
else -> visionToggle.visionIntakeOn
299+
}
300+
}
285301

286302
Pair(visionMode(), autoIntakeOn())
287-
})
303+
}
288304

289305
/* set options for autonomous */
290306
m_autoCommandChooser.setDefaultOption("Power Port Vision Autonomous", visionAuto())

‎src/main/kotlin/frc/robot/commands/ChangeDriveMode.kt‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
/*----------------------------------------------------------------------------*/
2+
/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */
3+
/* Open Source Software - may be modified and shared by FRC teams. The code */
4+
/* must be accompanied by the FIRST BSD license file in the root directory of */
5+
/* the project. */
6+
/*----------------------------------------------------------------------------*/
7+
8+
package frc.robot.commands
9+
10+
import edu.wpi.first.wpilibj2.command.CommandBase
11+
import frc.robot.Constants
12+
import frc.robot.subsystems.*
13+
import java.time.LocalTime
14+
15+
class ChangeDriveMode(val driveToggle: DriveModeToggleSubsystem,
16+
val activating: () -> Boolean) : CommandBase() {
17+
/**
18+
* Creates a new ChangeDriveMode command.
19+
*
20+
* @param driveToggle The subsystem used by this command.
21+
* @param activating The function to call to determine if the driver
22+
* is trying to change the mode.
23+
*/
24+
25+
init {
26+
addRequirements(driveToggle)
27+
}
28+
29+
// Called when the command is initially scheduled.
30+
override fun initialize() {
31+
}
32+
33+
// Called every time the scheduler runs while the command is scheduled.
34+
override fun execute() {
35+
val now = LocalTime.now()
36+
37+
// handle input
38+
if (activating()) {
39+
if (driveToggle.holdStartTime == null) {
40+
driveToggle.holdStartTime = now
41+
} else if (now.minusSeconds(Constants.kDriveToggleHoldTime.toLong()).isAfter(driveToggle.holdStartTime)) {
42+
driveToggle.incrementMode()
43+
driveToggle.holdStartTime = null
44+
}
45+
} else driveToggle.holdStartTime = null
46+
47+
// update the drive mode
48+
driveToggle.applyDriveMode()
49+
50+
// stop rumble if time is up
51+
if (driveToggle.rumbleStartTime != null
52+
&& now.minusSeconds(Constants.kDriveModeChangeRumbleTime.toLong())
53+
.isAfter(driveToggle.rumbleStartTime)) {
54+
driveToggle.stopRumble()
55+
}
56+
}
57+
58+
// Called once the command ends or is interrupted.
59+
override fun end(interrupted: Boolean) {
60+
}
61+
62+
// Returns true when the command should end.
63+
override fun isFinished(): Boolean {
64+
return false
65+
}
66+
}

‎src/main/kotlin/frc/robot/commands/VisionModeChange.kt‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -15,9 +15,10 @@ import frc.robot.subsystems.VisionToggleSubsystem
1515
class VisionModeChange(val visionToggle: VisionToggleSubsystem,
1616
val mode: () -> Pair<VisionModes, Boolean>) : CommandBase() {
1717
/**
18-
* Creates a new ExampleCommand.
18+
* Creates a new VisionModeChange command.
1919
*
20-
* @param m_subsystem The subsystem used by this command.
20+
* @param visionToggle The subsystem used by this command.
21+
* @param mode The function to get the vision mode and automatic intake state as a pair
2122
*/
2223
init {
2324
addRequirements(visionToggle)

‎src/main/kotlin/frc/robot/subsystems/DriveModeToggleSubsystem.kt‎

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
package frc.robot.subsystems
2+
3+
import edu.wpi.first.wpilibj.GenericHID
4+
import edu.wpi.first.wpilibj.XboxController
5+
import edu.wpi.first.wpilibj2.command.CommandBase
6+
import java.time.LocalTime
7+
import edu.wpi.first.wpilibj2.command.SubsystemBase
8+
import frc.robot.Constants
9+
import java.lang.IndexOutOfBoundsException
10+
import java.util.*
11+
import kotlin.concurrent.schedule
12+
13+
/**
14+
* Subsystem to handle changing the command used to drive the robot.
15+
* This is intended to test the commands, not for use in real matches.
16+
*/
17+
18+
class DriveModeToggleSubsystem(private val drivetrain: DrivetrainSubsystem,
19+
private val controller1: XboxController,
20+
private val driveModes: List<CommandBase>,
21+
var currentMode: Int) : SubsystemBase() {
22+
23+
var holdStartTime: LocalTime? = null
24+
var rumbleStartTime: LocalTime? = null
25+
26+
override fun periodic() {
27+
}
28+
29+
fun incrementMode() {
30+
currentMode = (currentMode + 1) % driveModes.size
31+
32+
// rumble the controller
33+
controller1.setRumble(GenericHID.RumbleType.kLeftRumble, 1.0)
34+
controller1.setRumble(GenericHID.RumbleType.kRightRumble, 1.0)
35+
rumbleStartTime = LocalTime.now()
36+
}
37+
38+
fun stopRumble() {
39+
rumbleStartTime = null
40+
controller1.setRumble(GenericHID.RumbleType.kLeftRumble, 0.0)
41+
controller1.setRumble(GenericHID.RumbleType.kRightRumble, 0.0)
42+
}
43+
44+
fun applyDriveMode() {
45+
drivetrain.defaultCommand = try {
46+
driveModes[currentMode]
47+
} catch (e: IndexOutOfBoundsException) { // this shouldn't happen, but just in case
48+
driveModes[0]
49+
}
50+
}
51+
}

‎src/main/kotlin/frc/robot/subsystems/IntakeSubsystem.kt‎

Lines changed: 32 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
package frc.robot.subsystems
22

3+
import java.time.LocalTime
34
import edu.wpi.first.networktables.NetworkTable
45
import edu.wpi.first.networktables.NetworkTableInstance
56
import edu.wpi.first.wpilibj.SpeedController
@@ -29,6 +30,10 @@ class IntakeSubsystem(val motor0: SpeedController,
2930

3031
var internalSpeed = 0.0
3132

33+
var intakeShutoffDelayStartTime: LocalTime? = null
34+
val autoShutoffDelay: Long = 1 // seconds after not seeing a ball to keep running automatic intake
35+
var runningIntakeAutomatically = false
36+
3237
companion object {
3338
val ntInst = NetworkTableInstance.getDefault()
3439
val table = ntInst.getTable("ball-vision")
@@ -72,8 +77,33 @@ class IntakeSubsystem(val motor0: SpeedController,
7277
}
7378

7479
fun getRunningAutomatic(): Boolean {
75-
return (visionToggle.visionIntakeOn
80+
val runningBefore = runningIntakeAutomatically
81+
val shouldRunIntake = (visionToggle.visionIntakeOn
7682
&& ballFound.getBoolean(false)
77-
&& ballHeight.getDouble(0.0) >= Constants.ballHeightForAutoIntake)
83+
&& ballHeight.getDouble(0.0) >= Constants.kballHeightForAutoIntake)
84+
85+
// unless the system is turned off, wait for some time before turning off the intake
86+
if (!visionToggle.visionIntakeOn) runningIntakeAutomatically = false
87+
else if (shouldRunIntake) {
88+
// cancel any delays and start the intake
89+
intakeShutoffDelayStartTime = null
90+
runningIntakeAutomatically = true
91+
} else {
92+
val now = LocalTime.now()
93+
94+
// handle losing sight of the ball
95+
if (runningBefore && !shouldRunIntake) {
96+
if (intakeShutoffDelayStartTime == null) {
97+
// start delay if we haven't already
98+
intakeShutoffDelayStartTime = now
99+
} else if (now.minusSeconds(autoShutoffDelay).isAfter(intakeShutoffDelayStartTime)) {
100+
// the delay has passed and we don't see the ball, turn off the automatic intake
101+
intakeShutoffDelayStartTime = null
102+
runningIntakeAutomatically = false
103+
}
104+
}
105+
}
106+
107+
return runningIntakeAutomatically
78108
}
79109
}

0 commit comments

Comments
 (0)