mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
44 lines
1.4 KiB
Python
44 lines
1.4 KiB
Python
|
#!/usr/bin/env python3
|
||
|
|
#
|
||
|
|
# Copyright (c) FIRST and other WPILib contributors.
|
||
|
|
# Open Source Software; you can modify and/or share it under the terms of
|
||
|
|
# the WPILib BSD license file in the root directory of this project.
|
||
|
|
#
|
||
|
|
|
||
|
|
import wpilib
|
||
|
|
|
||
|
|
import constants
|
||
|
|
from subsystems.elevator import Elevator
|
||
|
|
|
||
|
|
|
||
|
|
class MyRobot(wpilib.TimedRobot):
|
||
|
|
"""This is a sample program to demonstrate the use of elevator simulation."""
|
||
|
|
|
||
|
|
def __init__(self) -> None:
|
||
|
|
super().__init__(0.020)
|
||
|
|
self.joystick = wpilib.Joystick(constants.kJoystickPort)
|
||
|
|
self.elevator = Elevator()
|
||
|
|
|
||
|
|
def robotPeriodic(self) -> None:
|
||
|
|
# Update the telemetry, including mechanism visualization, regardless of mode.
|
||
|
|
self.elevator.updateTelemetry()
|
||
|
|
|
||
|
|
def simulationPeriodic(self) -> None:
|
||
|
|
# Update the simulation model.
|
||
|
|
self.elevator.simulationPeriodic()
|
||
|
|
|
||
|
|
def teleopInit(self) -> None:
|
||
|
|
self.elevator.reset()
|
||
|
|
|
||
|
|
def teleopPeriodic(self) -> None:
|
||
|
|
if self.joystick.getTrigger():
|
||
|
|
# Here, we set the constant setpoint of 10 meters.
|
||
|
|
self.elevator.reachGoal(constants.kSetpoint)
|
||
|
|
else:
|
||
|
|
# Otherwise, we update the setpoint to 1 meter.
|
||
|
|
self.elevator.reachGoal(constants.kLowerkSetpoint)
|
||
|
|
|
||
|
|
def disabledInit(self) -> None:
|
||
|
|
# This just makes sure that our simulation code knows that the motor's off.
|
||
|
|
self.elevator.stop()
|