Skip to content

Commit 54af5be

Browse files
henrymwestfallhenrymwestfall
authored
Merge pull request fairviewrobotics#31 from henrymwestfall/master
New robot controls
2 parents fb7c914 + fa735bd commit 54af5be

File tree

9 files changed

+196
-50
lines changed

9 files changed

+196
-50
lines changed

‎.java-version‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
11

‎README.md‎

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,16 @@
22

33
Code for FRC 2036 Black Knights' 2020 Infinite Recharge robot.
44

5+
## Environment
6+
This project is most compatible with Java 11, which is
7+
not the most recent version of Java.
8+
9+
It is
10+
recommended to use [jEnv](https://github.com/jenv/jenv)
11+
to switch between Java versions on Mac OS and Linux.
12+
I followed [these](https://chamikakasun.medium.com/how-to-manage-multiple-java-version-in-macos-e5421345f6d0)
13+
instructions to set up jEnv on macOS Big Sur.
14+
515
## Building
616

717
This project uses gradle as its build system. The GradleRIO plugin provides a wide number of FRC specific gradle commands (see [here](https://github.com/wpilibsuite/GradleRIO) for details).
@@ -15,7 +25,7 @@ Deploying and displaying the log require your computer to be connected to the ro
1525

1626
Passing the `--offline` flag to gradle will prevent it from trying to update and/or download dependencies, which can be useful at competition.
1727

18-
It is **highly recommenced** to use intellij when working on the code. VSCode, though it is the officially supported editor, provides *very* limited code completion. Intellij provides excellent code completion, especially for kotlin.
28+
It is **highly recommended** to use intellij when working on the code. VSCode, though it is the officially supported editor, provides *very* limited code completion. Intellij provides excellent code completion, especially for kotlin.
1929

2030
## FRC Documentation
2131
The documentation for the FRC Control System can be found [here](https://docs.wpilib.org/en/latest/). These are very useful to refer to.

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

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -117,6 +117,8 @@ class Constants {
117117
val kTrackwidthMeters = 0.558
118118
val kDriveKinematics = DifferentialDriveKinematics(kTrackwidthMeters)
119119

120+
val ballHeightForAutoIntake = 100.0
121+
120122
val constants = mutableMapOf(
121123
"DrivetrainPID_P" to 0.035,
122124
"DrivetrainPID_I" to 0.0,

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

Lines changed: 62 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,8 @@ import frc.robot.triggers.EndgameTrigger
3232
* periodic methods (other than the scheduler calls). Instead, the structure of the robot
3333
* (including subsystems, commands, and button mappings) should be declared here.
3434
*/
35+
36+
3537
class RobotContainer {
3638
val sch = CommandScheduler.getInstance()
3739
var m_autoCommandChooser: SendableChooser<Command> = SendableChooser()
@@ -56,14 +58,15 @@ class RobotContainer {
5658
val motorsLeft = SpeedControllerGroup(motorFrontLeft, motorBackLeft)
5759
val motorsRight = SpeedControllerGroup(motorFrontRight, motorBackRight)
5860

61+
val visionToggle = VisionToggleSubsystem()
5962

6063
val gyro = AHRS()
6164
val leftDrivetrainEncoder = Encoder(Constants.leftDrivetrainEncoderPortA, Constants.leftDrivetrainEncoderPortB, Constants.kDrivetrainEncoderAReversed)
6265
val rightDrivetrainencoder = Encoder(Constants.rightDrivetrainEncoderPortA, Constants.rightDrivetrainEncoderPortB, Constants.kDrivetrainEncoderBReversed)
6366

6467
val drivetrain = DrivetrainSubsystem(motorsLeft, motorsRight, gyro, leftDrivetrainEncoder, rightDrivetrainencoder)
6568
val shooter = ShooterSubsystem(CANSparkMax(Constants.kShooterPort, MotorType.kBrushless))
66-
val intake = IntakeSubsystem(WPI_TalonSRX(Constants.kIntakePort), WPI_VictorSPX(Constants.kIntake2Port))
69+
val intake = IntakeSubsystem(WPI_TalonSRX(Constants.kIntakePort), WPI_VictorSPX(Constants.kIntake2Port), visionToggle)
6770

6871
val indexer = IndexerSubsystem(WPI_TalonSRX(Constants.kIndexerPort))
6972
val gate = GateSubsystem(WPI_TalonSRX(Constants.kGatePort), ColorSensorV3(I2C.Port.kOnboard))
@@ -131,6 +134,20 @@ class RobotContainer {
131134
)
132135
}
133136

137+
val visionBallLineup = {
138+
SequentialCommandGroup(
139+
BallVision(drivetrain, 0.0),
140+
DriveDoubleSupplier(drivetrain, { 0.3 }, { 0.0 }).withTimeout(0.5)
141+
)
142+
}
143+
144+
val visionLineup: () -> SequentialCommandGroup = {
145+
when (visionToggle.visionMode) {
146+
VisionModes.HIGHGOAL -> visionHighGoalLineUp()
147+
VisionModes.BALL -> visionBallLineup()
148+
}
149+
}
150+
134151
val forwardShootAutoNoIntake = SequentialCommandGroup(
135152
ParallelCommandGroup(
136153
SequentialCommandGroup(
@@ -175,40 +192,48 @@ class RobotContainer {
175192

176193
fun configureButtonBindings() {
177194
Constants.loadConstants()
195+
178196
/* Shooting and vision lining up */
179197
JoystickButton(controller1, kBumperRight.value).whenHeld(
180198
CompositeShoot(intake, indexer, gate, shooter, 0.0)
181199
)
182200

183201
JoystickButton(controller1, kBumperLeft.value).whenHeld(
184-
visionHighGoalLineUp()
202+
visionLineup()
185203
)
186204

187-
EndgameTrigger().and(JoystickButton(controller1, kA.value)).whileActiveOnce(
205+
/* Climbing, including winch */
206+
// winch forwards on B. This lowers the robot
207+
EndgameTrigger().and(JoystickButton(controller1, kB.value)).whileActiveOnce(
188208
FixedWinchSpeed(winch, { Constants.kWinchSpeed })
189209
)
190210

211+
// winch backwards on Y. This raises the robot
191212
EndgameTrigger().and(JoystickButton(controller1, kY.value)).whileActiveOnce(
192213
FixedWinchSpeed(winch, { -Constants.kWinchSpeed })
193214
)
194215

195-
EndgameTrigger().and(Trigger({ controller1.getTriggerAxis(kLeft) > Constants.kClimberTriggerThresh })).whileActiveOnce(
196-
FixedClimbSpeed(climber, { Constants.kClimberDownSpeed * controller1.getTriggerAxis(kLeft) })
216+
// raise climber with right trigger
217+
EndgameTrigger().and(Trigger({ controller1.getTriggerAxis(kRight) > Constants.kClimberTriggerThresh })).whileActiveOnce(
218+
FixedClimbSpeed(climber, { Constants.kClimberSpeed * controller1.getTriggerAxis(kRight) })
197219
)
198220

199-
EndgameTrigger().and(Trigger({ controller1.getTriggerAxis(kRight) > Constants.kClimberTriggerThresh })).whileActiveOnce(
200-
FixedClimbSpeed(climber, { -Constants.kClimberSpeed * controller1.getTriggerAxis(kRight) })
221+
// lower climber with left trigger
222+
EndgameTrigger().and(Trigger({ controller1.getTriggerAxis(kLeft) > Constants.kClimberTriggerThresh })).whileActiveOnce(
223+
FixedClimbSpeed(climber, { -Constants.kClimberDownSpeed * controller1.getTriggerAxis(kLeft) })
201224
)
202225

226+
203227
/* setup default commands */
204-
//drivetrain.defaultCommand = XboxDriveCommand
205-
drivetrain.defaultCommand = XboxDriveSpecial
206-
/* default gate - run forward on X, backwards on B
228+
drivetrain.defaultCommand = XboxDriveCommand
229+
//drivetrain.defaultCommand = XboxDriveSpecial
230+
231+
/* default gate - run forward on X, backwards on A
207232
* If left bumper held, run until a ball is seen by the sensor
208233
*/
209234
gate.defaultCommand = SensoredFixedGateSpeed(gate, {
210235
if (controller0.xButton) Constants.kGateSpeed else (
211-
if (controller0.bButton) -Constants.kGateSpeed else (
236+
if (controller0.aButton) -Constants.kGateSpeed else (
212237
if (controller0.getBumper(kLeft)) Constants.kGateLoadSpeed else (
213238
if (controller0.getTriggerAxis(kLeft) >= Constants.kTriggerThresh) -Constants.kGateLoadSpeed else 0.0
214239
)
@@ -218,10 +243,10 @@ class RobotContainer {
218243
controller0.getBumper(kLeft) || controller1.xButton
219244
})
220245

221-
/* default shooter - run forward on y, slowly backwards on a */
246+
/* default shooter - run forward on Y, slowly backwards on B */
222247
shooter.defaultCommand = FixedShooterSpeed(shooter, {
223248
if (controller0.yButton) Constants.kShooterSpeed else (
224-
if (controller0.aButton) Constants.kShooterReverseSpeed else 0.0)
249+
if (controller0.bButton) Constants.kShooterReverseSpeed else 0.0)
225250
})
226251

227252
/* default indexer - run forward on left bumper, backwards on left trigger */
@@ -233,12 +258,34 @@ class RobotContainer {
233258

234259
/* default intake - run forward on right bumper, backwards on right trigger */
235260
intake.defaultCommand = FixedIntakeSpeed(intake, {
236-
if (controller0.getBumper(kRight) || controller1.xButton) Constants.kIntakeSpeed else (
261+
if (controller0.getBumper(kRight) || controller1.xButton || intake.getRunningAutomatic()) {
262+
Constants.kIntakeSpeed
263+
} else (
237264
if (controller0.getTriggerAxis(kRight) >= Constants.kTriggerThresh)
238-
controller0.getTriggerAxis(kRight) * -Constants.kIntakeDir else 0.0)
265+
controller0.getTriggerAxis(kRight) * -Constants.kIntakeDir
266+
else 0.0)
239267
})
268+
240269
lights.defaultCommand = setAlliance
241270

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
277+
}
278+
} else {
279+
visionToggle.visionMode
280+
} }
281+
282+
val autoIntakeOn = {if (controller0.getBackButtonPressed()) {
283+
!visionToggle.visionIntakeOn
284+
} else visionToggle.visionIntakeOn }
285+
286+
Pair(visionMode(), autoIntakeOn())
287+
})
288+
242289
/* set options for autonomous */
243290
m_autoCommandChooser.setDefaultOption("Power Port Vision Autonomous", visionAuto())
244291
m_autoCommandChooser.addOption("Backup 2s Autonomous", backupAuto)

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

Lines changed: 41 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -7,36 +7,41 @@ import frc.robot.Constants
77
import frc.robot.subsystems.DrivetrainSubsystem
88

99
/**
10-
* The command for the high goal vision targeting algorithm
11-
* Right now it just drives straight at the target
10+
* The command for the ball vision targeting algorithm
11+
* Right now it just drives straight at the balls.
12+
* This code is almost entirely copied from VisionHighGoal.kt
1213
*/
1314

1415
class BallVision(val driveSubsystem: DrivetrainSubsystem, forwardSpeed: Double) : PIDCommand(
15-
PIDController(
16-
Constants.constants["DrivetrainPID_P"] ?: 0.035,
17-
Constants.constants["DrivetrainPID_I"] ?: 0.0,
18-
Constants.constants["DrivetrainPID_D"] ?: 0.005
19-
),
20-
driveSubsystem::getAngle,
21-
{
22-
/* dampen turning */
23-
/* because the offsets sum over multiple iterations, dampening doesn't make us not hit the target, it just slows turning towards it */
24-
var yawOff = yaw.getDouble(0.0) / 1.5
25-
/* dampen even more if we are making more than a small turn. */
26-
if (yawOff >= 5.0) {
27-
yawOff /= 1.5
28-
}
16+
PIDController(
17+
Constants.constants["DrivetrainPID_P"] ?: 0.035,
18+
Constants.constants["DrivetrainPID_I"] ?: 0.0,
19+
Constants.constants["DrivetrainPID_D"] ?: 0.005
20+
),
21+
driveSubsystem::getAngle,
22+
{
23+
/* dampen turning */
24+
/* because the offsets sum over multiple iterations, dampening doesn't make us not hit the target, it just slows turning towards it */
25+
var yawOff = yaw.getDouble(0.0) / 1.3
26+
/* dampen even more if we are making more than a small turn. */
27+
if (yawOff >= 5.0) {
28+
yawOff /= 1.5
29+
}
2930

30-
driveSubsystem.getAngle() + yawOff
31-
},
32-
{ output: Double -> driveSubsystem.driveArcade(forwardSpeed, output) },
33-
arrayOf(driveSubsystem)) {
31+
driveSubsystem.getAngle() + yawOff
32+
},
33+
{ output: Double -> driveSubsystem.driveArcade(forwardSpeed, output) },
34+
arrayOf(driveSubsystem)) {
35+
36+
var pTarget = true
37+
var ppTarget = true
3438

3539
companion object {
3640
val ntInst = NetworkTableInstance.getDefault()
3741
val table = ntInst.getTable("ball-vision")
38-
val yaw = table.getEntry("yaw")
39-
val isTarget = table.getEntry("isTarget")
42+
val yaw = table.getEntry("yaw") // ideal yaw to collect most balls
43+
val ballFound = table.getEntry("ballFound") // whether a ball is visible
44+
val ballHeight = table.getEntry("ballHeight") // the height of the ball
4045
}
4146

4247
init {
@@ -47,24 +52,29 @@ class BallVision(val driveSubsystem: DrivetrainSubsystem, forwardSpeed: Double)
4752

4853
fun setPIDParams() {
4954
controller.setTolerance(
50-
Constants.constants["DrivetrainPID_AngleToleranceDeg"] ?: 2.0,
51-
Constants.constants["DrivetrainPID_AngleRateToleranceDegPerS"] ?: 1.0
55+
Constants.constants["DrivetrainPID_AngleToleranceDeg"] ?: 2.0,
56+
Constants.constants["DrivetrainPID_AngleRateToleranceDegPerS"] ?: 1.0
5257
)
5358
controller.setPID(
54-
Constants.constants["DrivetrainPID_P"] ?: 0.035,
55-
Constants.constants["DrivetrainPID_I"] ?: 0.0,
56-
Constants.constants["DrivetrainPID_D"] ?: 0.005
59+
Constants.constants["DrivetrainPID_P"] ?: 0.035,
60+
Constants.constants["DrivetrainPID_I"] ?: 0.0,
61+
Constants.constants["DrivetrainPID_D"] ?: 0.005
5762
)
5863
}
5964

6065
override fun isFinished(): Boolean {
61-
6266
/* if no gyro, fail */
6367
if (!driveSubsystem.gyroUp()) return true
64-
//return getController().atSetpoint()
68+
69+
6570
/* if we can't see a vision target, stop */
66-
/* TODO: require no vision target for two or three frames before stopping */
67-
if (!isTarget.getBoolean(false)) return true
71+
val targetFound = ballFound.getBoolean(false)
72+
if (!targetFound && !pTarget && !ppTarget) return true
73+
74+
val tmp = pTarget
75+
pTarget = targetFound
76+
ppTarget = tmp
77+
6878
return false
6979
}
7080

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

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
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.subsystems.ExampleSubsystem
12+
import frc.robot.subsystems.VisionModes
13+
import frc.robot.subsystems.VisionToggleSubsystem
14+
15+
class VisionModeChange(val visionToggle: VisionToggleSubsystem,
16+
val mode: () -> Pair<VisionModes, Boolean>) : CommandBase() {
17+
/**
18+
* Creates a new ExampleCommand.
19+
*
20+
* @param m_subsystem The subsystem used by this command.
21+
*/
22+
init {
23+
addRequirements(visionToggle)
24+
}
25+
26+
// Called when the command is initially scheduled.
27+
override fun initialize() {
28+
}
29+
30+
// Called every time the scheduler runs while the command is scheduled.
31+
override fun execute() {
32+
val currentMode = mode()
33+
visionToggle.visionMode = currentMode.first
34+
visionToggle.visionIntakeOn = currentMode.second
35+
}
36+
37+
// Called once the command ends or is interrupted.
38+
override fun end(interrupted: Boolean) {
39+
}
40+
41+
// Returns true when the command should end.
42+
override fun isFinished(): Boolean {
43+
return false
44+
}
45+
}

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@ import kotlin.math.abs
1717
class GateSubsystem(val motor: SpeedController, val colorSensor: ColorSensorV3) : SubsystemBase() {
1818

1919
override fun periodic() {
20-
debugMeasurment()
20+
debugMeasurement()
2121
}
2222

2323
/* set a motor speed, no matter if a ball is in the gate or not */
@@ -46,7 +46,7 @@ class GateSubsystem(val motor: SpeedController, val colorSensor: ColorSensorV3)
4646
}
4747

4848
/* print status of sensor + its reading */
49-
fun debugMeasurment() {
49+
fun debugMeasurement() {
5050
SmartDashboard.putBoolean("Gate Ball Sensor Status", colorSensor.getIR() != 0)
5151
SmartDashboard.putBoolean("Ball Present in Gate", isBallTriggered())
5252
SmartDashboard.putNumber("Gate Ball Sensor Reading", colorSensor.getIR().toDouble())

0 commit comments

Comments
 (0)