-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathrobot.py
107 lines (79 loc) · 3.08 KB
/
robot.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
import wpilib
from ctre import WPI_TalonSRX
from magicbot import MagicRobot
from networktables import NetworkTables, NetworkTable
from wpilib import DoubleSolenoid
from components.drivetrain import DriveTrain
from controllers.gyro import gyro
import wpilib.drive
from robotpy_ext.autonomous import AutonomousModeSelector
from ctre import NeutralMode
import navx
# Download and install stuff on the RoboRIO after imaging
'''
py -3 -m robotpy_installer download-python
py -3 -m robotpy_installer install-python
py -3 -m robotpy_installer download robotpy
py -3 -m robotpy_installer install robotpy
py -3 -m robotpy_installer download robotpy[ctre]
py -3 -m robotpy_installer install robotpy[ctre]
py -3 -m robotpy_installer download robotpy[rev]
py -3 -m robotpy_installer install robotpy[rev]
py -3 -m robotpy_installer download pynetworktables
py -3 -m robotpy_installer install pynetworktables
py -3 -m pip install -U robotpy[ctre]
py -3 -m pip install robotpy[ctre]
'''
# Push code to RoboRIO (only after imaging)
'''
python robot/robot.py deploy --skip-tests
py robot/robot.py deploy --skip-tests --no-version-check
'''
INPUT_SENSITIVITY = 0.05
PNEUMATICS_MODULE_TYPE = wpilib.PneumaticsModuleType.CTREPCM
MagicRobot.control_loop_wait_time = 0.05
SPEED_MULTIPLIER = 1
ANGLE_MULTIPLIER = 1
WINDING_SPEED = .5
NEUTRAL_MODE = NeutralMode(2)
class SpartaBot(MagicRobot):
# a DriveTrain instance is automatically created by MagicRobot
drivetrain: DriveTrain
gyro: gyro
def createObjects(self):
'''Create motors and stuff here'''
NetworkTables.initialize(server='roborio-5045-frc.local')
self.sd: NetworkTable = NetworkTables.getTable('SmartDashboard')
self.drive_controller: wpilib.XboxController = wpilib.XboxController(0) # 0 works for sim?
self.talon_L_1 = WPI_TalonSRX(1)
self.talon_L_2 = WPI_TalonSRX(5)
self.talon_R_1 = WPI_TalonSRX(6)
self.talon_R_2 = WPI_TalonSRX(9)
def disabledPeriodic(self):
self.sd.putValue("Mode", "Disabled")
def teleopInit(self):
self.sd.putValue("Mode", "Teleop")
# self.limelight = NetworkTables.getTable("limelight")
# self.limelight.LEDState(3)
# print("limelight on")
'''Called when teleop starts; optional'''
def teleopPeriodic(self):
'''
Called on each iteration of the control loop\n
NOTE: all components' execute() methods will be called automatically
'''
# drive controls
# print("tele")
angle = self.drive_controller.getRightX()
speed = self.drive_controller.getLeftY()
if (abs(angle) > INPUT_SENSITIVITY or abs(speed) > INPUT_SENSITIVITY):
self.drivetrain.set_motors(speed, -angle)
self.sd.putValue('Drivetrain: ', 'moving')
else:
# reset value to make robot stop moving
self.drivetrain.set_motors(0.0, 0.0)
self.sd.putValue('Drivetrain: ', 'static')
if self.drive_controller.getAButton():
self.gyro.balancing(self)
if __name__ == '__main__':
wpilib.run(SpartaBot)