Repository navigation
Expand file tree
/
Copy pathrobot.py
More file actions
65 lines (55 loc) · 2.36 KB
/
Copy pathrobot.py
File metadata and controls
65 lines (55 loc) · 2.36 KB
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
#!/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 wpimath
import wpilib.drive
import wpimath.filter
import wpimath.controller
import DriveTrainSubsystem
class MyRobot(wpilib.TimedRobot):
def robotInit(self) -> None:
"""Robot initialization function"""
self.controller = wpilib.XboxController(0)
self.swerve = DriveTrainSubsystem.DriveTrain()
# Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
self.xspeedLimiter = wpimath.filter.SlewRateLimiter(3)
self.yspeedLimiter = wpimath.filter.SlewRateLimiter(3)
self.rotLimiter = wpimath.filter.SlewRateLimiter(3)
def autonomousPeriodic(self) -> None:
self.driveWithJoystick(False)
self.swerve.updateOdometry()
def teleopPeriodic(self) -> None:
self.driveWithJoystick(True)
def driveWithJoystick(self, fieldRelative: bool) -> None:
# Get the x speed. We are inverting this because Xbox controllers return
# negative values when we push forward.
xSpeed = (
-self.xspeedLimiter.calculate(
wpimath.applyDeadband(self.controller.getLeftY(), 0.02)
)
* DriveTrainSubsystem.kMaxSpeed
)
# Get the y speed or sideways/strafe speed. We are inverting this because
# we want a positive value when we pull to the left. Xbox controllers
# return positive values when you pull to the right by default.
ySpeed = (
-self.yspeedLimiter.calculate(
wpimath.applyDeadband(self.controller.getLeftX(), 0.02)
)
* DriveTrainSubsystem.kMaxSpeed
)
# Get the rate of angular rotation. We are inverting this because we want a
# positive value when we pull to the left (remember, CCW is positive in
# mathematics). Xbox controllers return positive values when you pull to
# the right by default.
rot = (
-self.rotLimiter.calculate(
wpimath.applyDeadband(self.controller.getRightX(), 0.02)
)
* DriveTrainSubsystem.kMaxSpeed
)
self.swerve.drive(xSpeed, ySpeed, rot, fieldRelative, self.getPeriod())