Skip to content
This repository has been archived by the owner on Feb 3, 2023. It is now read-only.

Iterative Chassis PID - Give the chassis some error and it will do 1 step of PID #303

Open
theol0403 opened this issue Jan 7, 2019 · 5 comments
Assignees
Labels
enhancement New feature or request
Milestone

Comments

@theol0403
Copy link
Member

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.

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

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.

@Octogonapus Octogonapus added enhancement New feature or request needs docs This issue needs documentation in the pros-docs repo. labels Jan 7, 2019
@Octogonapus Octogonapus added this to the Version 4.1.0 milestone Jan 7, 2019
@Octogonapus Octogonapus self-assigned this Jan 7, 2019
@Octogonapus Octogonapus removed the needs docs This issue needs documentation in the pros-docs repo. label Jan 13, 2020
@dcieslak19973
Copy link
Contributor

I"d like to help on this; do you have strong opinions on implementation details?

@Octogonapus
Copy link
Member

I was hoping @theol0403 could comment but I don"t have any opinions on implementation details at this time. I haven"t touched this code in a while. Why don"t you start by designing the API first?

@dcieslak19973
Copy link
Contributor

dcieslak19973 commented Nov 15, 2020

Similar to @theol0403 "s original but in looking into the details a bit, what I"d suggest is:

  /**
   * Sets the target distance for the robot to drive straight using PID.
   *
   * Meant to be called similar to:
   * 
   * ```cpp
   * QLength distToMove = QLength(36_in);
   * QLength relativeDistMoved = QLength(0_in);
   * QAngle curHeading = getHeading(chassis, otherParameters);
   * bool bFirstPass = true;
   * do {
   *   // It is possible to change the value of distToMove in this loop if desired
   *   relativeDistMoved = calculateDistMoved(distToMove, relativeDistMoved, chassis, otherParameter);
   *   QAngle headingChange = getHeadingChange(curHeading, chassis, otherParameter);
   *   curHeading = curHeading + headingChange;
   *   chassis->moveDistanceIterative(distToMove, relativeDistMoved, headingChange, bFirstPass);
   *   bFirstPass = false;
   *   // No need for pros::delay(20); here as moveDistanceIterative already delays
   * } while(!chassis->isSettled());
   * ```
   * 
   * @param itarget distance to travel
   * @param idistMoved how far along `itarget` the current iteration is
   * @param iangle current heading error 
   * @param firstIter indicates whether this is the first iteration which resets the PID error accumulation
   * 
   */
  void moveDistanceIterative(QLength itarget, QLength idistMoved, QAngle iangle, bool firstIter);

Something similar for turnAngleIterative, which is a simpler case.

@theol0403
Copy link
Member Author

theol0403 commented Nov 18, 2020

Hey, sorry for the late reply. Throwback to my first github issue ever 😅

I"m not actually sure how this can be cleanly implemented. Since the chassis control runs in a asynchronous task, there would essentially need a statemachine to keep track of the user command, which might get messy, especially when considering concurrency issues and the iterative nature of the command.

In reality, the implementation of that could almost be identical to just repeatedly calling moveDistanceAsync with the error instead of desired distance, except for some of the one time things like resetting the PIDs.

I also don"t really like that API, I think it should be something as simple as void moveDistanceIterative(QLength ierror); and void turnAngleIterative(QAngle ierror);. That error then gets repeated to the internal PID"s step function.

Here is one proposed functionality:

void moveDistanceAsync(iangle) {
 // reset PID
 // set mode to distance
 // set target distance
}

void turnAngleAsync(iangle) {
 // reset PID
 // set mode to angle
 // set target angle
}

void moveDistanceIterative(ierror) {
 // if previous mode != distanceIterative
 // 	then reset PID
 // set mode to distanceIterative
 // set pid target to 0
 // set distance error
}

void turnAngleIterative(ierror) {
 // if previous mode != angleIterative
 // 	then reset PID
 // set mode to angleIterative
 // set pid target to 0
 // set angle error
}

// main task

case distance:
  distancePid->step(currentDistance);
  anglePid->step(currentAngle);
case angle:
  turnPid->step(currentAngle);
case distanceIterative:
  distancePid->step(-distanceError); // target is at 0
  anglePid->step(currentAngle);
case angleIterative:
  turnPid->step(-angleError);

here is another proposal, which I favor for reliability reasons:

void moveDistanceIterative(ierror) {
 // if previous mode != distanceIterative
 // 	then reset PID
 // set mode to distanceIterative
 // pause internal task
 // do all the PID calculations right here, then apply to motors
}

void turnAngleIterative(ierror) {
 // if previous mode != angleIterative
 // 	then reset PID
 // set mode to angleIterative
 // pause internal task
 // do all the PID calculations right here, then apply to motors
}

Hopefully that makes sense.

Either way example usage would be:

do {
  chassis->turnAngleIterative(targetAngle - odom.theta);
  pros::delay(20);
} while(!chassis->isSettled());

@dcieslak19973
Copy link
Contributor

I can see where you"re going with that thought. I understand allowing the user to specify only the error is simpler, but I"m not sure how it would work if the user is not allowed to set the target in this iterative mode. Nor, too, how the user would run consecutive iterative movements where they would want to reset the PIDs.

Taking a step back, I think a goal here that we both recognize is a desire for users to use something other than the encoders (either motor or tracking wheels) to control the movement of the robot.

Within void ChassisControllerPID::loop() one could imagine generalizing the call to

auto encStartVals = chassisModel->getSensorVals();
double distanceElapsed = 0, angleChange = 0;

as:

pidLoopSensorDataAcquire->initialize();

(terrible name, admittedly), and then the call to:

encVals = chassisModel->getSensorVals() - encStartVals;
distanceElapsed = static_cast<double>((encVals[0] + encVals[1])) / 2.0;
angleChange = static_cast<double>(encVals[0] - encVals[1]);

as:

distanceElapsed = pidLoopSensorDataAcquire->getDistanceElapsed(mode);
angleChange = pidLoopSensorDataAcquire->getAngleChange(mode);

Sign up for free to subscribe to this conversation on GitHub. Already have an account? Sign in.
Labels
enhancement New feature or request
Projects
None yet
Development

No branches or pull requests

3 participants