Skip to content

Commit

Permalink
Fix formatting in md code blocks (#2726)
Browse files Browse the repository at this point in the history
  • Loading branch information
Gold856 authored Sep 6, 2024
1 parent 18453a9 commit e53db15
Show file tree
Hide file tree
Showing 97 changed files with 1,938 additions and 1,578 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -121,19 +121,19 @@ The returned setpoint might then be used as in the following example:
from wpilib import Timer
from wpilib.controller import ProfiledPIDController
from wpilib.controller import SimpleMotorFeedforward
def __init__(self):
# Assuming encoder, motor, controller are already defined
def __init__(self):
# Assuming encoder, motor, controller are already defined
self.lastSpeed = 0
self.lastTime = Timer.getFPGATimestamp()
# Assuming feedforward is a SimpleMotorFeedforward object
# Assuming feedforward is a SimpleMotorFeedforward object
self.feedforward = SimpleMotorFeedforward(ks=0.0, kv=0.0, ka=0.0)
def goToPosition(self, goalPosition: float):
pidVal = self.controller.calculate(self.encoder.getDistance(), goalPosition)
def goToPosition(self, goalPosition: float):
pidVal = self.controller.calculate(self.encoder.getDistance(), goalPosition)
acceleration = (self.controller.getSetpoint().velocity - self.lastSpeed) / (Timer.getFPGATimestamp() - self.lastTime)
self.motor.setVoltage(
self.motor.setVoltage(
pidVal
+ self.feedforward.calculate(self.controller.getSetpoint().velocity, acceleration))
self.lastSpeed = controller.getSetpoint().velocity
self.lastSpeed = controller.getSetpoint().velocity
self.lastTime = Timer.getFPGATimestamp()
```

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ In order to create a trapezoidal motion profile, we must first impose some const

```python
from wpimath.trajectory import TrapezoidProfile
# Creates a new set of trapezoidal motion profile constraints
# Creates a new set of trapezoidal motion profile constraints
# Max velocity of 10 meters per second
# Max acceleration of 20 meters per second squared
TrapezoidProfile.Constraints(10, 20)
Expand All @@ -66,7 +66,7 @@ Next, we must specify the desired starting and ending states for our mechanisms

```python
from wpimath.trajectory import TrapezoidProfile
# Creates a new state with a position of 5 meters
# Creates a new state with a position of 5 meters
# and a velocity of 0 meters per second
TrapezoidProfile.State(5, 0)
```
Expand Down Expand Up @@ -96,7 +96,7 @@ Now that we know how to create a set of constraints and the desired start/end st

```python
from wpimath.trajectory import TrapezoidProfile
# Creates a new TrapezoidProfile
# Creates a new TrapezoidProfile
# Profile will have a max vel of 5 meters per second
# Profile will have a max acceleration of 10 meters per second squared
profile = TrapezoidProfile(TrapezoidProfile.Constraints(5, 10))
Expand Down
18 changes: 9 additions & 9 deletions source/docs/software/advanced-controls/filters/debouncer.rst
Original file line number Diff line number Diff line change
Expand Up @@ -19,33 +19,33 @@ The WPILib ``Debouncer`` can be configured in three different modes:
```java
// Initializes a DigitalInput on DIO 0
DigitalInput input = new DigitalInput(0);
// Creates a Debouncer in "both" mode.
// Creates a Debouncer in "both" mode.
Debouncer m_debouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth);
// So if currently false the signal must go true for at least .1 seconds before being read as a True signal.
// So if currently false the signal must go true for at least .1 seconds before being read as a True signal.
if (m_debouncer.calculate(input.get())) {
// Do something now that the DI is True.
// Do something now that the DI is True.
}
```

```c++
// Initializes a DigitalInput on DIO 0
frc::DigitalInput input{0};
// Creates a Debouncer in "both" mode.
// Creates a Debouncer in "both" mode.
frc::Debouncer m_debouncer{100_ms, frc::Debouncer::DebounceType::kBoth};
// So if currently false the signal must go true for at least .1 seconds before being read as a True signal.
// So if currently false the signal must go true for at least .1 seconds before being read as a True signal.
if (m_debouncer.calculate(input.Get())) {
// Do something now that the DI is True.
// Do something now that the DI is True.
}
```

```python
from wpilib import DigitalInput
from wpimath.filter import Debouncer
# Initializes a DigitalInput on DIO 0
# Initializes a DigitalInput on DIO 0
self.input = DigitalInput(0)
# Creates a Debouncer in "both" mode with a debounce time of 0.1 seconds
# Creates a Debouncer in "both" mode with a debounce time of 0.1 seconds
self.debouncer = Debouncer(0.1, Debouncer.DebounceType.kBoth)
# If currently false, the signal must go true for at least 0.1 seconds before being read as a True signal.
# If currently false, the signal must go true for at least 0.1 seconds before being read as a True signal.
if self.debouncer.calculate(self.input.get()):
# Do something now that the DI is True.
pass
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ The ``singlePoleIIR()`` factory method creates a single-pole infinite impulse re

```python
from wpimath.filter import LinearFilter
# Creates a new Single-Pole IIR filter
# Creates a new Single-Pole IIR filter
# Time constant is 0.1 seconds
# Period is 0.02 seconds - this is the standard FRC main loop period
filter = LinearFilter.singlePoleIIR(0.1, 0.02)
Expand Down Expand Up @@ -78,7 +78,7 @@ The ``movingAverage`` factory method creates a simple flat moving average filter

```python
from wpimath.filter import LinearFilter
# Creates a new flat moving average filter
# Creates a new flat moving average filter
# Average will be taken over the last 5 samples
filter = LinearFilter.movingAverage(5)
```
Expand Down Expand Up @@ -110,7 +110,7 @@ The ``highPass`` factory method creates a simple first-order infinite impulse re

```python
from wpimath.filter import LinearFilter
# Creates a new high-pass IIR filter
# Creates a new high-pass IIR filter
# Time constant is 0.1 seconds
# Period is 0.02 seconds - this is the standard FRC main loop period
filter = LinearFilter.highPass(0.1, 0.02)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ Creating a ``MedianFilter`` is simple:

```python
from wpimath.filter import MedianFilter
# Creates a MedianFilter with a window size of 5 samples
# Creates a MedianFilter with a window size of 5 samples
filter = MedianFilter(5)
```

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ Creating a SlewRateLimiter is simple:

```python
from wpimath.filter import SlewRateLimiter
# Creates a SlewRateLimiter that limits the rate of change of the signal to 0.5 units per second
# Creates a SlewRateLimiter that limits the rate of change of the signal to 0.5 units per second
filter = SlewRateLimiter(0.5)
```

Expand Down Expand Up @@ -66,21 +66,21 @@ A typical use of a SlewRateLimiter is to limit the acceleration of a robot's dri
```java
// Ordinary call with no ramping applied
drivetrain.arcadeDrive(forward, turn);
// Slew-rate limits the forward/backward input, limiting forward/backward acceleration
// Slew-rate limits the forward/backward input, limiting forward/backward acceleration
drivetrain.arcadeDrive(filter.calculate(forward), turn);
```

```c++
// Ordinary call with no ramping applied
drivetrain.ArcadeDrive(forward, turn);
// Slew-rate limits the forward/backward input, limiting forward/backward acceleration
// Slew-rate limits the forward/backward input, limiting forward/backward acceleration
drivetrain.ArcadeDrive(filter.Calculate(forward), turn);
```

```python
# Ordinary call with no ramping applied
drivetrain.arcadeDrive(forward, turn)
# Slew-rate limits the forward/backward input, limiting forward/backward acceleration
# Slew-rate limits the forward/backward input, limiting forward/backward acceleration
drivetrain.arcadeDrive(filter.calculate(forward), turn)
```

Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ Reliable data of the :term:`system's <system>` :term:`state`\s, :term:`input`\s

```python
from ntcore import NetworkTableInstance
def robotPeriodic(self):
def robotPeriodic(self):
NetworkTableInstance.getDefault().flush()
```

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -174,21 +174,21 @@ For example, we might use the following Q and R for an elevator system with posi

```C++
// Example system -- must be changed to match your robot.
LinearSystem<2, 1, 1> elevatorSystem = frc::LinearSystemId::IdentifyVelocitySystem(5, 0.5);
LinearQuadraticRegulator<2, 1> controller{
elevatorSystem,
// q's elements
{0.02, 0.4},
// r's elements
{12.0},
// our dt
0.020_s};
LinearSystem<2, 1, 1> elevatorSystem = frc::LinearSystemId::IdentifyVelocitySystem(5, 0.5);
LinearQuadraticRegulator<2, 1> controller{
elevatorSystem,
// q's elements
{0.02, 0.4},
// r's elements
{12.0},
// our dt
0.020_s};
```

```python
from wpimath.controller import LinearQuadraticRegulator_2_1
from wpimath.system.plant import LinearSystemId
# Example system -- must be changed to match your robot.
# Example system -- must be changed to match your robot.
elevatorSystem = LinearSystemId.identifyPositionSystemMeters(5, 0.5)
controller = LinearQuadraticRegulator_2_1(
elevatorSystem,
Expand All @@ -199,7 +199,7 @@ For example, we might use the following Q and R for an elevator system with posi
# our dt
0.020,
)
```
```

### LQR: example application

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,8 @@ To be able to run the tests, SysIdRoutine exposes test "factories", or functions
public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
return routine.quasistatic(direction);
}
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
return routine.dynamic(direction);
}
```
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ Users can create their own constraint by implementing the ``TrajectoryConstraint
units::meters_per_second_t velocity) override {
// code here
}
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) override {
// code here
}
Expand All @@ -52,15 +52,15 @@ Users can create their own constraint by implementing the ``TrajectoryConstraint
from wpimath import units
from wpimath.geometry import Pose2d
from wpimath.trajectory.constraint import TrajectoryConstraint
class MyConstraint(TrajectoryConstraint):
class MyConstraint(TrajectoryConstraint):
def maxVelocity(
self,
pose: Pose2d,
curvature: units.radians_per_meter,
velocity: units.meters_per_second,
) -> units.meters_per_second:
...
def minMaxAcceleration(
def minMaxAcceleration(
self,
pose: Pose2d,
curvature: units.radians_per_meter,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ The final parameter is a ``ProfiledPIDController`` for the rotation of the robot
ProfiledPIDControllerRadians,
)
from wpimath.trajectory import TrapezoidProfileRadians
controller = HolonomicDriveController(
controller = HolonomicDriveController(
PIDController(1, 0, 0),
PIDController(1, 0, 0),
ProfiledPIDControllerRadians(
Expand All @@ -62,7 +62,7 @@ The controller can be updated using the ``Calculate`` (C++) / ``calculate`` (Jav
```java
// Sample the trajectory at 3.4 seconds from the beginning.
Trajectory.State goal = trajectory.sample(3.4);
// Get the adjusted speeds. Here, we want the robot to be facing
// Get the adjusted speeds. Here, we want the robot to be facing
// 70 degrees (in the field-relative coordinate system).
ChassisSpeeds adjustedSpeeds = controller.calculate(
currentRobotPose, goal, Rotation2d.fromDegrees(70.0));
Expand All @@ -71,17 +71,17 @@ The controller can be updated using the ``Calculate`` (C++) / ``calculate`` (Jav
```c++
// Sample the trajectoty at 3.4 seconds from the beginning.
const auto goal = trajectory.Sample(3.4_s);
// Get the adjusted speeds. Here, we want the robot to be facing
// Get the adjusted speeds. Here, we want the robot to be facing
// 70 degrees (in the field-relative coordinate system).
const auto adjustedSpeeds = controller.Calculate(
currentRobotPose, goal, 70_deg);
```

```python
from wpimath.geometry import Rotation2d
# Sample the trajectory at 3.4 seconds from the beginning.
# Sample the trajectory at 3.4 seconds from the beginning.
goal = trajectory.sample(3.4)
# Get the adjusted speeds. Here, we want the robot to be facing
# Get the adjusted speeds. Here, we want the robot to be facing
# 70 degrees (in the field-relative coordinate system).
adjustedSpeeds = controller.calculate(
currentRobotPose, goal, Rotation2d.fromDegrees(70.0)
Expand All @@ -96,7 +96,7 @@ The returned adjusted speeds can be converted into usable speeds using the kinem
.. tab-set-code::
```java
SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(adjustedSpeeds);
SwerveModuleState frontLeft = moduleStates[0];
SwerveModuleState frontLeft = moduleStates[0];
SwerveModuleState frontRight = moduleStates[1];
SwerveModuleState backLeft = moduleStates[2];
SwerveModuleState backRight = moduleStates[3];
Expand Down
12 changes: 6 additions & 6 deletions source/docs/software/advanced-controls/trajectories/ramsete.rst
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ The Ramsete controller should be initialized with two gains, namely ``b`` and ``
// Using the default constructor of RamseteController. Here
// the gains are initialized to 2.0 and 0.7.
RamseteController controller1 = new RamseteController();
// Using the secondary constructor of RamseteController where
// Using the secondary constructor of RamseteController where
// the user can choose any other gains.
RamseteController controller2 = new RamseteController(2.1, 0.8);
```
Expand All @@ -20,20 +20,20 @@ The Ramsete controller should be initialized with two gains, namely ``b`` and ``
// Using the default constructor of RamseteController. Here
// the gains are initialized to 2.0 and 0.7.
frc::RamseteController controller1;
// Using the secondary constructor of RamseteController where
// Using the secondary constructor of RamseteController where
// the user can choose any other gains.
frc::RamseteController controller2{2.1, 0.8};
```

```python
from wpimath.controller import RamseteController
# Using the default constructor of RamseteController. Here
# Using the default constructor of RamseteController. Here
# the gains are initialized to 2.0 and 0.7.
controller1 = RamseteController()
# Using the secondary constructor of RamseteController where
# Using the secondary constructor of RamseteController where
# the user can choose any other gains.
controller2 = RamseteController(2.1, 0.8)
```
```

## Getting Adjusted Velocities
The Ramsete controller returns "adjusted velocities" so that the when the robot tracks these velocities, it accurately reaches the goal point. The controller should be updated periodically with the new goal. The goal comprises of a desired pose, desired linear velocity, and desired angular velocity. Furthermore, the current position of the robot should also be updated periodically. The controller uses these four arguments to return the adjusted linear and angular velocity. Users should command their robot to these linear and angular velocities to achieve optimal trajectory tracking.
Expand All @@ -47,7 +47,7 @@ The controller can be updated using the ``Calculate`` (C++) / ``calculate`` (Jav
```java
Trajectory.State goal = trajectory.sample(3.4); // sample the trajectory at 3.4 seconds from the beginning
ChassisSpeeds adjustedSpeeds = controller.calculate(currentRobotPose, goal);
```
```

```c++
const Trajectory::State goal = trajectory.Sample(3.4_s); // sample the trajectory at 3.4 seconds from the beginning
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -80,37 +80,37 @@ Trajectories in Java can be combined into a single trajectory using the ``concat
List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
new Pose2d(3, 0, Rotation2d.fromDegrees(0)),
new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0)));
var trajectoryTwo =
var trajectoryTwo =
TrajectoryGenerator.generateTrajectory(
new Pose2d(3, 0, Rotation2d.fromDegrees(0)),
List.of(new Translation2d(4, 4), new Translation2d(6, 3)),
new Pose2d(6, 0, Rotation2d.fromDegrees(0)),
new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0)));
var concatTraj = trajectoryOne.concatenate(trajectoryTwo);
var concatTraj = trajectoryOne.concatenate(trajectoryTwo);
```

```c++
auto trajectoryOne = frc::TrajectoryGenerator::GenerateTrajectory(
frc::Pose2d(0_m, 0_m, 0_rad),
{frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
frc::Pose2d(3_m, 0_m, 0_rad), frc::TrajectoryConfig(3_fps, 3_fps_sq));
auto trajectoryTwo = frc::TrajectoryGenerator::GenerateTrajectory(
auto trajectoryTwo = frc::TrajectoryGenerator::GenerateTrajectory(
frc::Pose2d(3_m, 0_m, 0_rad),
{frc::Translation2d(4_m, 4_m), frc::Translation2d(5_m, 3_m)},
frc::Pose2d(6_m, 0_m, 0_rad), frc::TrajectoryConfig(3_fps, 3_fps_sq));
auto concatTraj = m_trajectoryOne + m_trajectoryTwo;
auto concatTraj = m_trajectoryOne + m_trajectoryTwo;
```

```python
from wpimath.geometry import Pose2d, Rotation2d, Translation2d
from wpimath.trajectory import TrajectoryGenerator, TrajectoryConfig
trajectoryOne = TrajectoryGenerator.generateTrajectory(
trajectoryOne = TrajectoryGenerator.generateTrajectory(
Pose2d(0, 0, Rotation2d.fromDegrees(0)),
[Translation2d(1, 1), Translation2d(2, -1)],
Pose2d(3, 0, Rotation2d.fromDegrees(0)),
TrajectoryConfig.fromFps(3.0, 3.0),
)
trajectoryTwo = TrajectoryGenerator.generateTrajectory(
trajectoryTwo = TrajectoryGenerator.generateTrajectory(
Pose2d(3, 0, Rotation2d.fromDegrees(0)),
[Translation2d(4, 4), Translation2d(6, 3)],
Pose2d(6, 0, Rotation2d.fromDegrees(0)),
Expand Down
Loading

0 comments on commit e53db15

Please sign in to comment.