GitHub repository: OkapiLib/OkapiLib

Issues:
issue_495: state: open
title: The ChassisController DriveToPoint and TurnToPoint methods do not function correctly
body: When using the DriveToPoint or TurnToPoint function the robot will rotate to a seemingly random angle sometimes close to correct and sometimes completely wrong. I tested this using a consistent starting point with no code changes and observed the turn angle wildly change 4/9 runs. We were able to get completely expected and consistent results using turnAngle and moveDistance function calls so we are fairly confident this issue is not hardware related or tuning of the PID values.

```
std::shared_ptr<OdomChassisController> chassis =
ChassisControllerBuilder()
	.withGains(
	{0.003, 0.0001, 0.0001}, //Distance
	{0.0065, 0, 0.0001},     //Turning
	{0.001, 0, 0.0001}       //Straightness
	)
	.withMotors({2,5},{-1,-20}) // Minus sign means motors are reversed
	.withDimensions(AbstractMotor::gearset::green, {{4_in, 11.5_in}, imev5GreenTPR})
	.withSensors(RotationSensor(10), RotationSensor(9, true))
	.withOdometry({{2.75_in, 7.5_in}, 360})
	.buildOdometry();

  	chassis->setMaxVelocity(90);
	chassis->moveDistance(3_in);
	chassis->waitUntilSettled();
	chassis->stop();

	chassis->driveToPoint({70_in, -39_in});
	chassis->waitUntilSettled();
	chassis->stop();

	chassis->turnToPoint({125_in,5_in});
	chassis->stop();
```

**Versions**
4.8.0 OKAPI
3.7.2 PROS Kernel



user: DoctorFogarty
issue_494: state: open
title: fixed rotationSensor reset function
body: ### fixed rotation sensor reset function. Switched function call. It should work properly now to reset to 0. 

<!--
We must be able to understand the design of your change from this description. If we can't get a good idea of what the code will be doing from the description here, the pull request may be closed at the maintainers' discretion. Keep in mind that the maintainer reviewing this PR may not be familiar with or have worked with the code here recently, so please walk us through the concepts.
-->

### Motivation

<!-- Why does this change need to be made? -->

### Possible Drawbacks

<!-- What are the possible side-effects or negative impacts of the code change? -->

### Verification Process

<!--
What process did you follow to verify that your change has the desired effects?

- How did you verify that all new functionality works as expected?
- How did you verify that all changed functionality works as expected?
- How did you verify that the change has not introduced any regressions?
-->

### Applicable Issues

<!-- Enter any applicable Issues here -->

user: mkadi11
issue_493: state: open
title: okapi::RotationSensor::reset() does not reset sensor to zero
body: **Describe the bug**
When the `reset()` method of an `okapi::RotationSensor` is called, `get()` return value does not change to 0.

**Steps To Reproduce**
```c++
okapi::RotationSensor sensor (1); // Or another port
pros::delay(5000); //Spin the sensor to some value
std::cout << "Before reset: " << sensor.get() << "\n"; // Some value
sensor.reset();
std::cout << "After reset: " << sensor.get() << "\n"; // Another value, not zero
```
My output was:
```
Before reset: -220.7
After reset: 139.3
```

**Expected behavior**
Immediately after sensor.reset() is called, the value of sensor.get() should be 0.

**Versions**
Kernel 3.6.2
Okapi 4.8.0

**Additional Information**
[The implementation in rotationSensor.cpp](https://github.com/OkapiLib/OkapiLib/blob/master/src/impl/device/rotarysensor/rotationSensor.cpp) calls `pros::c::rotation_reset(port);`.
To achieve the [function described in the docs](https://okapilib.github.io/OkapiLib/classokapi_1_1RotationSensor.html#af2898478ce400a3494d3e280eabce7c1), `pros::c::rotation_reset_position(port);` should be used instead.

This issue also makes calling `resetSensors()` on a Two or ThreeEncoderSkidSteerModel which uses rotation sensors rather than ADI encoders ineffective.



user: bnhrkn
issue_492: state: open
title: Allow AsyncVelControllerBuilder to support different velocity controllers
body: **Is your feature request related to a problem? Please describe.**
Okapi offers an excellent framework for teams to build their code upon. However, the velocity controller builder only supports building pid or integrated controllers. If a user one day inherits the AsyncVelocityController class and make a new type of velocity controller (take back half for example), there is no functionality within the builder to accept the new controller type.

Relevant code is as follows:
```cpp
std::shared_ptr<AsyncVelocityController<double, double>> AsyncVelControllerBuilder::build() {
  if (!hasMotors) {
    std::string msg("AsyncVelControllerBuilder: No motors given.");
    LOG_ERROR(msg);
    throw std::runtime_error(msg);
  }

  if (hasGains) {
    if (!hasVelMath) {
      std::string msg("AsyncVelControllerBuilder: No VelMath given.");
      LOG_ERROR(msg);
      throw std::runtime_error(msg);
    }

    if (!gearsetSetByUser) {
      LOG_WARN_S(
        "AsyncVelControllerBuilder: The default gearset is selected. This could be a bug.");
    }

    return buildAVPC();
  } else {
    return buildAVIC();
  }
}
```

**Describe the solution you'd like**
Add a method which allows the user to pass a shared pointer to the iterative velocity controller desired, similar to the withOdometry(std::shared_ptr\<Odometry\>) method in ChassisControllerBuilder if a user wants to supply their own variation of odometry. The user can then just inject the controller directly without needing to mess with the builder itself.

The builder can be modified to check if a controller is directly given by the user:
```cpp
AsyncVelControllerBuilder& AsyncVelControllerBuilder::withController(const std::shared_ptr<IterativeVelController>& ivelController){
  internalController = std::move(ivelController);
  hasCustomController = true;
}

std::shared_ptr<AsyncVelocityController<double, double>> AsyncVelControllerBuilder::build() {
  if (!hasMotors) {
    std::string msg("AsyncVelControllerBuilder: No motors given.");
    LOG_ERROR(msg);
    throw std::runtime_error(msg);
  }

  if(hasController){
    if (!hasVelMath) {
      std::string msg("AsyncVelControllerBuilder: No VelMath given.");
      LOG_ERROR(msg);
      throw std::runtime_error(msg);
    }

    if (!gearsetSetByUser) {
      LOG_WARN_S(
        "AsyncVelControllerBuilder: The default gearset is selected. This could be a bug.");
    }

    return buildCustomController();
  }

  if (hasGains) {
    if (!hasVelMath) {
      std::string msg("AsyncVelControllerBuilder: No VelMath given.");
      LOG_ERROR(msg);
      throw std::runtime_error(msg);
    }

    if (!gearsetSetByUser) {
      LOG_WARN_S(
        "AsyncVelControllerBuilder: The default gearset is selected. This could be a bug.");
    }

    return buildAVPC();
  } else {
    return buildAVIC();
  }
}
```

**Describe alternatives you've considered**
The controller constructors are extremely long, and therefore won't be as simply as using the builder and breaking the process down.

**Versions**
okapilib v4.8.0

**Additional context**
I can probably work implementing this. Should be doable by just adding the necessary variables and methods, and adding another condition check if a controller is already supplied within the build() method.

Also, this can be extended to position controller builders as well most likely.
user: Ryan4253
labels: enhancement: New feature or request
issue_491: state: open
title: Add build script
body: ### Description of the Change

<!--
We must be able to understand the design of your change from this description. If we can't get a good idea of what the code will be doing from the description here, the pull request may be closed at the maintainers' discretion. Keep in mind that the maintainer reviewing this PR may not be familiar with or have worked with the code here recently, so please walk us through the concepts.
-->

### Motivation

<!-- Why does this change need to be made? -->

### Possible Drawbacks

<!-- What are the possible side-effects or negative impacts of the code change? -->

### Verification Process

<!--
What process did you follow to verify that your change has the desired effects?

- How did you verify that all new functionality works as expected?
- How did you verify that all changed functionality works as expected?
- How did you verify that the change has not introduced any regressions?
-->

### Applicable Issues

<!-- Enter any applicable Issues here -->

user: Octogonapus
issue_490: state: open
title: Create clearer documentation on controller and button usage
body: I've seen a few users miss the `operator[]` overload and similar functionality. We need to expand the documentation in this area.

Created from #489.
user: Octogonapus
issue_449: state: open
title: Inertial Sensor Odometry
body: **Is your feature request related to a problem? Please describe.**
In smaller drivetrains, it can be difficult to place multiple tracking encoder wheels in such a way to allow use of the 2 or 3 encoder odometry methods. This limits these drivetrains to internal encoders, which increases the potential for error.

**Describe the solution you'd like**
Two new derived odometry classes. The first class would use one centrally mounted encoder wheel to measure forward translation and an inertial sensor to measure heading. This class would be similar in function to the existing two encoder odometry class. A second derived class would add a strafe encoder in a similar manner to the existing three encoder class.
 

**Describe alternatives you've considered**
Existing odometry models.

**Versions**
kernel 3.2.1 pros-mainline
okapilib 4.0.3 pros-mainline

**Additional context**
This feature requires the resolution of issue #444 .

user: invalidflaw
labels: enhancement: New feature or request
issue_433: state: open
title: AsyncMotionProfileController not respecting gear ratios when based off of a ChassisController
body: **Describe the bug**
The AsyncMotionProfileController will turn motors as if the chassis is direct drive green, ignoring any gear ratios you have set in the ChassisController definition.

**Steps To Reproduce**
1. Create a chassis controller with a gear ration (ex: blue cartridge with a 3:5 reduction)

```
auto Chassis = ChassisControllerBuilder()
    .withMotors (
      DriveLeft,
      DriveRight
    )
    .withDimensions(
	  AbstractMotor::gearset::blue,
      {{3.25_in, 26.5_cm}, imev5BlueTPR * (5.0 / 3.0)}
    ).build()
  ;
```

2. Using this ChassisController as a base, create an AsyncMotionProfileController
```
auto ChassisMotionController = AsyncMotionProfileControllerBuilder()
    .withOutput(Chassis)
    .withLimits({1, 2, 10})
    .buildMotionProfileController()
  ;

```

3. Generate and run a path
**Expected behavior**
I expect the chassis to move based on the gear ratios specified in the ChassisControllerBuilder. Instead, it seems to assume green cartridges.

**Versions**
PROS Kernel: 3.2.1
Okapilib: 4.0.3

**Additional Information**
This was tested on a skid-steer chassis with 2 motors driving each side. Each motor turns a 36 tooth gear, which meshes with a 60 tooth gear that is connected to the wheel by quarter inch shaft.
I also don't believe this to be a duplicate of #243, since that issue seemed to be with AsyncMotionProfileController s not based on an existing ChassisController.

user: drdeaton
labels: bug: Something isn't working
workaround available: This bug has a workaround
issue_413: state: open
title: Movement-based calls should be stopped when their task ends
body: **Describe the bug**
Calling `moveDistance` or similar will start that movement. Normally, the movement (and with it, the call) will finish before its task is deleted. However, if the task is deleted prematurely, the movement continues, which can lead to a loss of control over the robot.

As a safety measure, users can call the relevant `stop` method.

This should be fixed by tracking the task the movement was started from; if that task is deleted, then the movement should be interrupted.

**Steps To Reproduce**
```cpp
void opcontrol() {

  chassis = ChassisControllerBuilder()
                .withMotors({1, -3}, {-2, 4})
                .withGains({0.001, 0, 0}, {0, 0, 0})
                .withDimensions(AbstractMotor::gearset::green,
                                {{4_in, 11.5_in}, imev5GreenTPR})
                .build();

  pros::Task task([](void *) {
  std::cout << "Starting" << std::endl;
    chassis->moveDistance(20_ft);
  }, nullptr, "task");
  pros::delay(500);
  task.remove();
  std::cout << "Killed" << std::endl;

  pros::delay(500);
}
```

**Expected behavior**
The robot should have been stopped when the task was killed. Instead, it kept driving.

**Versions**
v4.0.1
user: Octogonapus
labels: bug: Something isn't working
workaround available: This bug has a workaround
issue_370: state: open
title: Multidimensional EKF
body: OkapiLib already features a one-dimensional EKF, so it should be feasible to have a 
multidimensional EKF. There's an example at https://github.com/OkapiLib/TinyEKF, which creates a Kalman filter and shows an example of sensor fusion, although the code is written to be very lightweight, which may not be necessary under V5. 
user: Heasummn
labels: enhancement: New feature or request
help wanted: Extra attention is needed
issue_328: state: open
title: Move assignment operators
body: **Is your feature request related to a problem? Please describe.**
It's not possible to swap two instances of CCPID, because std::swap requires move assignment.

**Describe the solution you'd like**
I'd like CCPID and other OkapiLib classes where move semantics are desirable to use the "Rule of Five"~~, by `= default`ing both the move assignment and move constructor, as the current move constructor just does a member-wise move.~~

**Versions**
I am on OkapiLib version 3.3.8.

**Additional context**
https://en.cppreference.com/w/cpp/language/rule_of_three 

user: vexcat
labels: enhancement: New feature or request
issue_303: state: open
title: Iterative Chassis PID - Give the chassis some error and it will do 1 step of PID
body: Currently, there are two pairs of movement functions.
`moveDistance`/`moveDistanceAsync` and `turnAngle`/`turnAngleAsync`.
These each set the target of the chassis relative to current position then execute the motion.
However, I want to be able to define my own error calculation for the chassis to do PID on.
What I mean is to be able to use the chassis in a way similar to an `IterativePIDController` where you can specify the target and then step. However, the implementation might be different for a chassis.

This will be useful for

- Active Brake
- Alignment (Such as Vision Sensor Alignment)
- Angle/Point Seeking

Lets say for example we have Odometry. We are able to read the absolute chassis angle at any time.
Using the above functions, if we wanted to turn to face 0 degrees, we would have to do something along the lines of
`robotChassis.turnAngle(0_deg - chassisOdom.theta);`
If chassis theta is -90 degrees, this will then set the target of the chassis to +90 degrees.
The robot will then turn to face 0 degrees.
However, if the robot slips, or something goes wrong with the turn, the target will become messed up and the turn will be inaccurate. To attempt to hone in to the 0 degrees, you would have to run the function again and hope that it will reach the target.

However, all this time the chassisTheta was kept accurate.
This is the request - to be able to specify the value to calculate the error from.

This is then what you could do to hone in to 0 degrees.
```cpp
do
{
    robotChassis.turnAngleIterative(0_deg - chassisOdom.theta); // Does 1 step of PID from specified error
    pros::delay(20);
}
while (!robotChassis.isSettled);
```
This way it does PID on the calculation we used to determine the wanted angle instead of calculating once and then doing PID on that static target.
Another way to do this would be
```cpp
while(true)
{
    if(myButton)
    {
        //Does 1 step of PID given error
        // could be any arbitrary calculation giving turning error
        myChassis->turnAngleIterative(targetAngle - currentAngle); 
        pros::delay(20);
    }
}
```

Then it will only do PID when the button is pressed.
This might be useful for vision alignment or anything where you want the chassis to use PID to react to external values. 
You would also be able to specify a dynamic variable as the target and move the target around.
So as a recap, instead of calculating and executing the whole movement as one action, it instead does one step of PID towards the target and then re-calculates the target.
I think the best way of doing this is by manually calculating the error, but there may be other ways such as setting the target with an input.

user: theol0403
labels: enhancement: New feature or request
issue_297: state: open
title: Improve ChassisController constructor with Gyro Support
body: **Is your feature request related to a problem? Please describe.**
Yaw-rate gyro support is lacking in OkapiLib, and for robots with unreliable encoders, [turnAngle()](https://pros.cs.purdue.edu/v5/okapi/api/chassis/controller/abstract-chassis-controller.html#turnangle) is entirely unreliable.

**Describe the solution you'd like**
Overload the ChassisController constructor with a gyro.
Utilize the gyro when [turnAngle()](https://pros.cs.purdue.edu/v5/okapi/api/chassis/controller/abstract-chassis-controller.html#turnangle) is called, potentially fused with the existing encoder method.

**Describe alternatives you've considered**
Of course, this could be done separately, but if the point is to enable high level programming, then it is better integrated into OkapiLib.

**Versions**
```
PROS Project for v5...
Name      Version    Origin
--------  ---------  -------------
kernel    3.1.3      pros-mainline
okapilib  3.3.5      pros-mainline
```

**Additional context**
This should have precedence over any odometry work. What is the point of odometry if you do not know where you are in the first place?

user: EricHallahan
labels: question: Further information is requested
issue_128: state: open
title: Better localization
body: If someone has other sensors than encoders on their robot, we should be able to fuse them for a better state estimate. There is a promising looking SR-UKF implementation here meant for embedded systems: https://github.com/sfwa/ukf. We should look into whether it's feasible to run that filter with 6 state variables and 5 (I think?) measurement variables.
user: Octogonapus
labels: enhancement: New feature or request
help wanted: Extra attention is needed
issue_127: state: open
title: Pure Pursuit
body: I would really like to have pure pursuit available for the initial release. It's a flashy and useful feature that should attract attention to the project. There is an excellent paper here which describes the algorithm: https://www.ri.cmu.edu/pub_files/pub3/coulter_r_craig_1992_1/coulter_r_craig_1992_1.pdf

Additionally, this introduction to differential drive kinematics will be helpful: http://rossum.sourceforge.net/papers/DiffSteer/DiffSteer.html

I am not sure if the appropriate controller architecture is in place for this, but I think it's time to start trying to implement it.
user: Octogonapus
labels: enhancement: New feature or request
help wanted: Extra attention is needed
issue_111: state: open
title: Rosserial Integration
body: **Is your feature request related to a problem? Please describe.**

Problem: Integrating a ROS serial connection with VEX hardware is possible with [rosserial for V5](https://github.com/CanyonTurtle/rosserial/tree/more-examples) alone, but requires user configuration for lots of hardware-specific setup, such as setting up publishers for sensor data, moving the robot based on commands, etc...

**Describe the solution you'd like**

The OkapiLib API can act as a schema for mapping ROS objects into abstractions, e.g. chassis, that ROS can control via standardized ROS messages, e.g. Twist command velocity messages for that chassis.

**Describe alternatives you've considered**

Using rosserial with vex hardware is still possible ([example](https://github.com/CanyonTurtle/rosserial/blob/more-examples/rosserial_vex_cortex/src/ros_lib/examples/twistdrive.cpp)), but the OkapiLib abstractions can provide control systems built-in, which will allow the ROS messages to be a more intelligent control interface, with minimal user requirement.

**Versions**

PROS: 3.0.6
OkapiLib: recent

**Additional context**
 
This issue is intended to be a running discussion about integrating rosserial and OkapiLib.

user: CanyonTurtle
issue_106: state: open
title: Integration testing on a V5
body: We need some test infrastructure to run integration tests on a real V5. The workflow I am looking for is,
- Jenkins runs a task to build and run integration tests:
  - Run `make`
  - From a Python program, use the PROS CLI to upload and open the terminal
  - The tests will start running automatically if the download succeeded, so expect some output. We can have a header that gets printed after a second or so (to wait for the terminal to connect) before tests start. If the Python program does not see the header after a few seconds, something is wrong. After the tests finish, results are printed in a standard format the Python program can look for and parse to see if any failed.
  - Fail the build if uploading failed, if starting tests failed, if the tests never finished (we can have some sort of timeout), or if any tests failed.
user: Octogonapus
labels: enhancement: New feature or request
issue_49: state: open
title: WPIlib Commands
body: WPIlib has optional command-based programming: https://github.com/wpilibsuite/allwpilib/blob/master/wpilibc/src/main/native/include/Commands/Command.h

Commands are useful for segmenting robot logic and make running actions in parallel easier (you can run commands in parallel and wait for certain commands to finish). OkapiLib should have some variant of this.

## Command Lifecycle
- Start
- Initialize
- Loop
  - Step
  - IsFinished
- End

### Interruption
Interruptible commands can be interrupted during Loop.

## Problems with WPIlib's implementation
The user has to implement a command subclass. This is basically necessary for correctly stopping the command if it needs to be interrupted. However, this would be difficult for people that don't understand C++'s class syntax.

I'm not sure how to solve having to implement a Command class. The command arbiter needs a way to properly interrupt and clean up after a command. There would have to be some clever lambda usage to accomplish that. Just accepting a lambda for the step implementation won't cut it.
user: Octogonapus
labels: enhancement: New feature or request
help wanted: Extra attention is needed
issue_2: state: open
title: Add an XDriveModel ctor that takes a gyro
body: This is Brad's suggestion ([WPIlib code here](https://github.com/wpilibsuite/allwpilib/blob/master/wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp#L60)) so you can drive around independent of heading. This has been tried in the past on the Cortex and it didn't work well enough to be used for a match. The gyro drifts too much. V5 would have to perform substantially better for this to work.

Alternatively, instead of taking a gyro object, we could do this the WPIlib way and just take the value of the heading so we aren't locked in to using a gyro. Then the heading measurement could come from a combination of quads & a gyro.

Gyro is waiting on Vex fixing the ADI port analog in sampling rate.
user: Octogonapus
labels: enhancement: New feature or request