lemlib / lemlib

An easy to use and feature-rich PROS template for the Vex V5

---

Welcome to LemLib! This open-source PROS template aims to introduce common algorithms like Pure Pursuit and Odometry for new and experienced teams alike.

The creation of this template was inspired by EZ-Template and OkapiLib. We aim to develop a library that strikes a balance between ease-of-use, akin to that of EZ-Template, and comprehensive functionality, similar to that of OkapiLib.

Want a place to chat with the devs and other users? Join our Discord server.

License

This project is licensed under the MIT license. Check LICENSE for more details.

Features

• Generic PID class
• Odometry
• Odom turn to face point
• Boomerang controller
• Pure Pursuit
• Motion Chaining
• Driver Control

Example Project

You can find a fully annotated example project here.

Tutorials

The tutorials provide a detailed walkthrough through all the features of LemLib. It covers everything from installation to Path Following:

FAQ

1. Help! Why is my controller vibrating? If your controller vibrated more than once, your inertial sensor calibration failed. Check if its connected to the right port and try again.

2. What drivetrains are supported? Only tank/differential. This is not going to change until other drivetrains are competitive.

3. Do I need tracking wheels? No, but it is recommended. You should absolutely have a horizontal tracking wheel if you don't have traction wheels, and you have to spend extra effort tuning your movements to prevent any wheel slip.

4. Do I need an inertial sensor? No, but it is highly recommended. The one exception to this would be if you have 2 parallel tracking wheels which are tuned well and are perfectly square. LemLib will work without it, but the accuracy will be compromised.

5. Do I need an SD card? As of v0.5.0, no SD card is necessary.

6. What are the units? The units are inches and degrees. In a future release, Qunits will be used so you can use whatever units you like.

7. Is LemLib V5RC legal? Yes. Per the RECF student-centred policy, in the context of third-party libraries.

Students should be able to understand and explain the code used on their robots

In other words, you need to know how LemLib works. You don't need to know the details like all the math, just more or less how the algorithm works. If you want to learn more about LemLib, you can look through the documentation and ask questions on our Discord server.

Documentation

Check out the Documentation.

Contributing

Want to contribute? Please read CONTRIBUTING.md and join our Discord server.

Code of Conduct

See the Code of Conduct on how to behave like an adult.

lemlib's Issues

[FEATURE] Boomerang Controller

Requested Feature

Implement the boomerang controller used by the ARMS library as an alternative to the current moveTo method of the Chassis. This method of controlling the robot allows the user to specify not only the coordinates they would like the robot to end it's movement at, but also the angle.

[FEATURE] Boomerang controller improvements

Requested Feature

Turn smoothness parameter and secondary carrot lead gain, as well as removing the speed limit at the end of the movement

• Turn smoothness parameter. Value from 0 to 1
• gLead. A second gain for the carrot point.
• remove speed limit when settling

Current Implementation

Currently has dLead, chasePower, and maxSpeed tuneable values, as well as PID gains

Motivation

Greater control over the boomerang controller and the curve it makes.

(Optional) Possible Implementations and Alternatives

Turn Smoothness:

float overturn = fabs(angularPower) + fabs(linearPower) - maxSpeed - maxSpeed * smoothness;
if (overturn > 0) linearPower -= linearPower > 0 ? overturn : -overturn;
prevLinearPower = linearPower;

gLead:

currentCarrot = initialCarrot + (currentCarrot - initialCarrot) * (1 - gLead)

[BUG] Tracking wheels prioritized before IMU for heading

Expected Behavior

If IMU is present, then it should be used.

Observed Behavior

IMU is only used if there are no pairs of tracking wheels.

Steps to Reproduce

Environment

• LemLib Version: Every version with the chassis class
  • From its creation
  • To today
• PROS Kernel Version: N/A
• PROS CLI Version: N/A

Additional Information

The inertial's one purpose is to provide heading data. If it is present in the sensors struct, that means the user wants the imu to be used. Therefore it should be of the highest priority.
I'd be happy to submit a pull request to fix this, although could I create it on the v0.4.7 branch so that a new patch can be released, fixing this bug?

[FEATURE] Kinematic Constraints

Robots can not always function properly when acceleration, jerk, or velocity is too high. LemLib does not apply such limits to its chassis motion functions. Adhering to these constraints will make motions smoother and be less susceptible to wheel slip, overall increasing their repeatability.

I propose the following global constraints (automatically applied to each movement):

• Max lateral velocity
• Max forwards acceleration
• Max backwards acceleration
• Max forwards jerk
• Max backwards jerk
• Max angular velocity
• Max angular acceleration
• Max angular jerk

It should also be possible to make the chassis use different constraints per function call. Therefore, the user will be able to pass these parameters as part of a struct to the motion functions (if unspecified, use default).

[FEATURE] Correctly handle motor oversaturation.

Requested Feature

When combining two PID outputs together (such as in the case of point-to-point movement, which runs an angular and linear controller), the combined outputs may end up being more than the maximum motor voltage (128, or 12v). This is less than ideal, because the ratio between the left and right wheel speed will not be preserved, and the chassis may turn more or less than expected (depending if the left or right voltage is outside of +-128.

Current Implementation

https://github.com/LemLib/LemLib/blob/master/src/lemlib/chassis/chassis.cpp#L311-L317

Motivation

See above paragraph. Point-to-point movements currently will not execute exactly as expected, and may reach the target slower as a result of underturning.

(Optional) Possible Implementations and Alternatives

Normalize the left and right wheel voltages. If one voltage is over the saturated value (128), then decrease both voltages to preserve the ratio between the two:

Example implementation ripped from some old code:

std::pair<double, double> normalize_motor_power(double left_power, double right_power, double max_power) {
	double largest_speed = std::max(std::abs(left_power), std::abs(power_speed)) / max_speed;

	if (largest_speed > 1.0) {
		left_speed /= largest_speed;
		right_speed /= largest_speed;
	}

	return { left_speed, right_speed };
}

std::pair<double, double> normalized_voltages = math::normalize_speeds(
	linear_power + angular_power,
	linear_power - angular_power,
	12.0
);

left_motors.spin(vex::forward, normalized_voltages.first, vex::volt);
right_motors.spin(vex::forward, normalized_voltages.second, vex::volt);

I think this actual algorithm originates from wpilibc, but it's been used in other places as well (such as okapi).

[BUG] Sudden corrections in chassis.moveTo

Expected Behaviour

When using the chassis.moveTo function, the chassis should smoothly correct its heading to face the target point.

Observed Behaviour

Corrections are very sudden. This reduces the repeatability and increases wheel slippage.

[FEATURE] PTO support

Requested Feature

Power Take Off support in the Chassis class

• Should be able to dynamically switch motors from other subsystems to the chassis
• The extra motors should be included in odometry calculations (if using motor encoder odometry)

Current Implementation

It is not possible to do this.

Motivation

PTOs are becoming increasingly common, and users should be able to make use of them when using LemLib

[FEATURE] Motion chaining

Requested Feature

Motion chaining. Transition from on motion to the other smoothly without stopping

• Robot settles before moving on to next point
• Unnecessary deceleration and acceleration
• Wastes time

Current Implementation

Set timeout too a value too small so the robot does not finish the path and translate the target.

Motivation

Saves time in autos

(Optional) Possible Implementations and Alternatives

Chaining will look different for every motion algorithm:

Boomerang:

Find the imaginary line perpendicular to the heading of the robot. This line intersects the target point.
If the robot can carrot point was on the same side of the line last cycle, but on different the current cycle, exit.
Motion chaining mode enabled whenever a parameter called minimumSpeed is set to a non-zero value

Turning

Exit when the robot crosses the target heading. Does this whenever a parameter called minimumSpeed is set to a non-zero value

Pure Pursuit

Exit whenever the closest point is the last point or the ghost point

[BUG] Chassis.moveTo() tries to turn at the end of movement

Expected Behaviour

The chassis should only go straight in the last 15" of the motion. (This seems a bit high and should be around 5").

Observed Behaviour

The chassis makes a sharp turn at the end of the motion.

[FEATURE] Doxygen workflow

Requested Feature

When a new release has been pushed, the docs should be automatically updated to reflect new changes and version increments. This includes tutorials, doxygen documentation, and javadocs at the start of the source files.

Benefits

• Easier maintenance

[BUG] Tracking Wheels Reporting Incorrect Distance

Expected Behavior

TrackingWheel::getDistanceTravelled() should return the distance it has travelled.

Observed Behavior

When using a rotation sensor, it does not return the correct distance. It resets with every rotation of the tracking wheel.

[BUG] Calibrating Imu doesn't check reset() return value

Expected Behavior

The chassis should only have to calibrate the imu once.

Observed Behavior

The chassis will forever calibrate the imu.

Steps to Reproduce

1. Initialize a chassis with an imu
2. Set errno to PROS_ERR, ENODEV, or ENXIO
3. Run chassis.calibrate()

Environment

• LemLib Version: 0.4.7
• PROS Kernel Version: 3.8.0
• PROS CLI Version: 3.4.1
• Additional Templates: OkapiLib-4.8.0

Additional Information

In my code, errno is set to ENODEV, because I check for the presence of my odometry sensors to find what odometry sensor configuration to use. (The robot has two different possible sensor configurations).

Fix

Change the code from here:

        odomSensors.imu->reset(true);
        // keep on calibrating until it calibrates successfully
        while (errno == PROS_ERR || errno == ENODEV || errno == ENXIO) {
            pros::c::controller_rumble(pros::E_CONTROLLER_MASTER, "---");
            odomSensors.imu->reset(true);
            pros::delay(10);
        }

to

        // calibrate inertial, and if calbration fails, then repeat until successful 
        while (odomSensors.imu->reset(true) != 1 && (errno == PROS_ERR || errno == ENODEV || errno == ENXIO)) {
            pros::c::controller_rumble(pros::E_CONTROLLER_MASTER, "---");
            pros::delay(10);
        }

Without this fix, I believe that if the inertial attempts to calibrate and fails, then it will cause this same bug, and will never exit the loop.
I think there should also be a maximum number of attempts, after which it gives up and sets the inertial to nullptr, because the inertial sensor may be invalid/disconnected.

Please let me know if I should submit a pull request to resolve this issue, and whether I should implement the maximum number of attempts, and I will do so. Also, what branch should I fork from?

[IMPROVEMENT] Switching all refactor Raw Pointers to Smart Pointers.

Currently, the significant majority of the refactor's classes depend on raw pointers to function. This leads to the risk of memory leaks. Switching the library over to smart pointers will guarantee memory safety for the library, barring segfaults, along with setting a new standard for contributing to the library. I'm assigning myself this improvement.

[BUG] Inertial Sensor Calibration Fails

Expected Behavior

If the inertial sensor calibration fails, it should automatically be attempted again

Observed Behavior

No errors are thrown or attempts to recalibrate are made if the inertial sensor calibration fails

Steps to Reproduce

Force inertial sensor calibration to fail (e.g shaking it while calibrating)

Environment

• LemLib Version: 0.4.6
• PROS Kernel Version: 3.8.0
• PROS CLI Version: 3.4.1
• Additional Templates: None

Additional Information

If the inertial sensor calibration fails, the odometry will not be able to update its position and the chassis will not move if requested.

[BUG] Logger not correctly logging Integral term in FAPID

Expected Behavior

When logging PID it should show P term, I term, D term as they should be calculated

Observed Behavior

The logger shows an I term regardless of whether or not kI = 0

Steps to Reproduce

Log PID with a kD not equal to 0

Environment

• LemLib Version: movement_beta
• PROS Kernel Version: 3.8.0
• PROS CLI Version: 3.4.1
• Additional Templates: None

Additional Information

Found the issue in pid.cpp under lemlib::FAPID::update(), where the line to log is using kD for the Integral term instead of kI

if (log) { Logger::logPid(name, output, error, kP * error, kD * totalError, kD * deltaError); }

Swing turn radius parameter

Requested Feature

The swingToPoint() and swingToHeading() functions should have radius parameter to change the swing arc.

• .radius will be a new parameter in the params struct and have an default value of 0
• increasing .radius will increase the swing radius in inches
• a value greater than 0 of .radius will necessitate the use of the locked side of the drivetrain to achieve the specified swing radius

Current Implementation

Currently you can do janky stuff with boomerang to achieve a similar result.

Motivation

Using moveToPose to simulate a swing turn with a larger radius is janky and not optimal. A .radius parameter would increase the versatility of the swing turn functions while not noticeably increasing the complexity of the function call and usage.

(Optional) Possible Implementations and Alternatives

[FEATURE] Motion in Teleop

[BUG] Logger System Unused

Observed Behavior

The debugger system is unused.

Expected Behavior

Making mistakes such as trying to move the chassis prior to calibrating should be logged appropriately.

How to Reproduce

1. Configure chassis
2. Move chassis without calibrating it

Project Info

Pros Kernel: 3.8.0
LemLib version: 0.4.5
Additional Libraries: None

Errors / Screenshots / Code

None

Additional Notes

Some examples of when the logger should be used:

1. Turning to face a point very close to the robot [Warning]
2. V5 device command failed to run successfully [Error]
3. Chassis sensor calibration failed [Error]
4. Odom configuration error (e.g 2 tracking wheels in same position) [Error]
5. Failed to read SD card [Error]
6. Improperly formatted file [Error]
7. Same motors on left and right side drivetrain [Error]

and many more situations that should be addressed in a PR

[FEATURE] GPS sensor support

Requested Feature

V5 GPS sensor support for odometry

Motivation

The V5 GPS sensor allows users to get the robot's current position relative to the field. Unlike typical odometry configurations with a gyroscope and tracking wheels, it measures the robot's location relative to the field border, not relative to the robot's starting position. This means it does not drift over time. It will be useful for complex autonomous routines, such as those in Autonomous Skills.

Implementation

The GPS sensor can be passed as a sensor in the OdomSensors_t struct. Users can then manually update the position of the robot using the sensor by calling a Chassis::updateGPS() function.

[FEATURE] Motion Profiling

Requested Feature

Motion Profiling should be added as a chassis motion algorithm. Motion Profiling is an open-loop control algorithm that can move the chassis to follow a path while respecting kinematic constraints such as maximum velocity, acceleration, and jerk.

Benefits

• Very smooth, minimal wheel slippage and tipping
• Ideal for movements early in autonomous

[Docs] Add doxygen

[QUESTION] LemLib Setup

What makes a tracking wheel "reversed"?
For the left/right I'm assuming that moving forward should increase the encoder values, and for the middle, I'm not sure.

Here's a quick video that details the issue I'm running into and also the setup of my robot:
https://derock.media/63c5sI.mp4

Code: https://github.com/ItzDerock/95993A-spin-up/
You're probably interested in src/config/config.cpp

[FEATURE] Custom moveVelocity Function

Requested Feature

Create a custom moveVelocity function to replace the flawed VEX one

Current Implementation

Currently LemLib just uses voltage for moving motors

Motivation

Easier and more intuitive for movement algorithms, chaining off of issue #4

(Optional) Possible Implementations and Alternatives

Could do something uses move voltage and a PID loop, with get_velocity as the input for the loop
(In conjunction with above) Could also create some sort of function to predict a starting point for the voltage to minimized time to reach target

[FEATURE] Drive Motor Velocity Controller

Problem

Currently, drivetrain motors are controlled directly by the motion algorithms simply by controlling their voltage. This reduces the effectiveness of the motion algorithms, and makes velocity units confusing

Proposed Solution

The chassis class should have velocity controllers for drivetrain motors. This way, motion algorithms can simply request the drivetrain to move at a specified speed (e.g 6 in/s)

[FEATURE]Add this pointers to everything(this pointer police)

Requested Feature

Add this pointer to everything.

[BUG] example.txt included in cold package

Expected Behavior

example.txt should not be included in the cold package

Observed Behavior

example.txt is in the cold package

Steps to Reproduce

compile the program, use your favorite decompiler to view example.txt in the LemLib.a binary

Environment

• LemLib Version: 0.5.0-rc1 and newer
• PROS Kernel Version: 3.8.0
• PROS CLI Version: 3.4.3
• Additional Templates: None

Additional Information

New users are confused when their program does not compile if they have example.txt in the static folder.

Potential Fix

Modify asset.mk so it will add assets from the cold_static folder to the cold package, and add assets from the static folder into the hot package

Hello, as the university team would like to use your library, but if the automatic code cannot be run in manual mode, it will be impossible to debug, so I would like you to modify the driver function to remove its status detection, thank you.

[FEATURE] VSCode Extension

Visual Studio Code Extension

This LemLib VSCode extension would provide support for many different functions:

• Path creator (migrate from https://github.com/sizzinseal/path-gen)
• Logging
• Setup - setting up LemLib
• [Maybe] Gui Creator - Create a teleop and autonomous through a GUI, without any programming

It would also include a few commands:

• Install - installs LemLib using the PROS CLI
• Upload path - uploads the generated path to the V5 Brain using a serial port
• Save log - saves the log of the ran program to the output

[BUG] Dangling pointers with LemLib constructors that take pointers or references

I noticed when reading through LemLib that there are cases which can lead to dangling pointers/memory corruption with constructors that take and store pointers to objects.
eg.

// I use new here but the problem would still apply if I used a smart pointer
lemlib::Chassis* createChassis() {
    lemlib::TrackingWheel horizontal(&horizontalEnc, lemlib::Omniwheel::NEW_275, -3.7);
    lemlib::OdomSensors sensors(nullptr, 
                                nullptr, 
                                &horizontal, 
                                nullptr, 
                                &imu
    );

    auto chassis = new lemlib::Chassis(drivetrain, linearController, angularController, sensors);
    return chassis;

    // `horizontal` is invalidated here leaving a dangling pointer in the chassis object!!
}

This isn't very common because most people use global variables to store all their objects, so their references are never invalidated.
The example here is simplistic but it is pretty easy to run into this issue if you aren't storing everything in global variables.
It affects (as far as I can tell), every LemLib constructor that takes and stores a reference or pointer.
This issue is present in the current release and in all previous releases of LemLib.

Fixing this would require using smart pointers (and it would force users to use them too), or using std::move to pass the objects.
I don't really know what a good solution is if we don't want to require users to know how to construct unique_ptr/shared_ptrs or how to use std::move
Maybe a static create method for things like tracking wheels/motor groups that returns a smart pointer?
or taking std::optional<MotorGroup> instead of MotorGroup* (this would avoid the need for smart pointers but require that the MotorGroups are either built in the constructor call or passed in with std::move)

[FEATURE] Doxygen Improvements

Requested Feature

Reformatted side-tree in the doxygen documentation.

• Update doxygen-awesome (should be a submodule, or be updated automatically)
• Reformat the side tree
• The index page should not just be the readme of the project. It should have nice links as to where to go, with a small about section

Current Implementation

Sections of the README, under are the individual tutorials, then classes and files in the README.

Motivation

The current implementation is not super intuitive for inexperienced users. This should not be the case. By simply reformatting the side-tree, this can be resolved.

[FEATURE] Gain Scheduling

Requested Feature

The ability to put in control cases for PID constant(s) (I.e. kP = 2, kD = 3 at 90 degrees and kP = 2, kD = 4 at 160 degrees) and interpolate between the constant(s).

• Once the movement starts the constants should be calculated, they don't need to change during the movement.

Current Implementation

There currently is no implementation.

Motivation

Due to the non-linear nature of the robot's movement, one set of PID constants is not as effective in one case to the other, so it would be nice to have a way of automatically making adjustments to the constants to increase consistent performance.

(Optional) Possible Implementations and Alternatives

Having an alternative constructor for FAPID, where you can insert control cases as list of lists or other method (I.e. {kF = 1, kA = 0, kP = 3, kI = 1, kD = 4, case = 90}) and from lowest case to highest case, interpolate linearly or with a spline the constants

[FEATURE] Holonomic Drivetrain Support

Requested Feature
Holonomic drivetrain support

Motivation
Currently, only differential drivetrains are supported. This limits who can use LemLib. These drivetrains are still very popular, and it makes sense to support them.

Implementation
This can be implemented by creating an abstract Chassis class, and then having classes such as Differential or Holonomic

[DOCS] Chassis Config PID is yet to be updated

the chassis set up tutorial does not include the kI and windup range properties in the lemlib::ControllerSettings struct example

[BUG] Tutorial Docs Image Paths are Wrong

Expected Behavior

Images to be displayed

Observed Behavior

Picture icon is displayed, its looking for the pictures in tutorials/assets/[filename], when in reality the assets folder is at the same level as the tutorials folder.

Steps to Reproduce

Open the tutorials (at least number 2) on github

Environment

Master branch