VEX Balance Robot PID

[Da_Dude]

note: I have to go so I’m just copying and pasting my question from stack overflow which got deleted. please ask questions if need be.

I’m new here to the VEX website, and it’s only my second year working with vex, and I can’t find a reliable solution that’s already been said/asked. So I was hoping someone else could help me out?

Currently, I’m building a balance robot as part a project and my teacher and I are stumped where to go from where we currently are. Currently we’ve developed a PID with the input being the angle of the robot, which is coming form a VEX robotics inertial sensor. We’ve gotten pretty far just guessing and checking, changing one input at a time, but we’ve reached a standstill where no matter what we change or how little we change it, it won’t get any better(sometimes after changing it back to what worked better than the current iteration it gets worse which is actually really confusing).

So what he’s asked me to do(and what I’ve failed to find) is some sort of equation that may help is get to the final step of balancing(I understand this is highly improbable because all balancing bots use diff motors, inputs, and measurements in the code but any help is better than none)? Or some sort of additional eyes that may be able to give us the edge we need to fix our problem.

I’ll put our code below, and if you have questions about specifics I’ll do my best to answer but tbh I’m not a great coder and my teacher is doing most of the brain work for the PID & coding.

    // Include the V5 Library
    #include <stdio.h>
    #include <stdlib.h>
    #include <stdbool.h>
    #include <math.h>
    #include <string.h>
    #include "vex.h"
  
    // Allows for easier use of the VEX Library
    using namespace vex;

    //robot configuration code

    double err, lasterr = 0, sum = 0;
    double kp = 1.4;
    double kd = .08;
    double ki = 0.04; 
    double speed = 0;
    double target = -2.4;
    double pidout;

    int main() {
    Inertial20.calibrate();
    wait(2, sec);
    // post event registration

    // set default print color to black
    printf("\033[30m");


    while (true){
    err = target -Inertial20.orientation(roll, degrees); //0 is my desired value.
    speed = -Motor11.velocity(rpm);
    lasterr = err;
    sum+=err;
    if (-.3 < err && err < .5) {sum = 0.5 * sum;}
    pidout = err * kp + speed * kd + sum * ki; //I directly set my kp and kd without variables.
    
    target = -2.3 + Motor11.position(turns) * 2;
    Motor1.spin(forward, pidout, volt);
    Motor11.spin(forward, pidout, volt);
    wait(15,msec);
    }
}

Really the only thing we’ve tried is derive this code from a basic pid to be able to move the wheel to keep robot upright. Than once we got this I don’t think we’ve changed the actual code more than 5-10 times or so, and the rest was us just plugging in diff numbers, watching the robot, and changing one of the manual inputs depending on what seemed off the most(jagged corrections, slow to sense something, etc.)

edit 1: Apparently I wasn’t specific enough the first time so I guess I’m trying again.

What’s wrong:
I’m realizing now that I didn’t say exactly how close we got. So unfortunately I’m at home right now and the bot is at school, but the longest we were able to get it to balance without assistance for was ~10 seconds. At which point it overcorrected and promptly fell right into my left hand. So, what we don’t know is how to stop it from over correcting and falling into my hands, and we think the best way to do that is to find the right PID values.
I can try to get a video of the robot tomorrow as well so you guys might have a better idea of what the problem is in case I’m missing something.

What I’m asking for:
What I need is either someone to tell me if there’s some sort of equation we can use to find definite values for our PID, OR If there’s another way to get said values. Like I said before, we’ve already tried manual input followed by testing and we think we’ve done it that way for as long as we could(and we got close), and I’m trying to find some way to solve for the variables.

Also, to Thomas Mathews, thank you for the advice. I’m sure that’s something that looks like it was overlooked but on the platform that I’m using, which is specifically for Vex Robotics called Vex V5 Pro, the things below have to be included otherwise the code produces errors and doesn’t work. I’ll also be adding spaces because I never realized how hard it was to read cause my eyes sort of glaze over when I try to read it.

[jpearman]

A couple of old topics to look at

Tutorial: Segway IQ – Robotsquare

https://vexforum.com/t/working-vex-pendulum-balance-robot/13545/1

[EcstaticPilot]

This document has lots of tips on tuning PIDs
introduction_to_pid_controllers_ed2.pdf (400.2 KB)
The above document contains a good description of a mathematical tuning method called the Ziegler Nichols method

[2775Josh]

Lego, Raspberry and Python Project - Reaction Wheel Inverted Pendulum

I know this kind of deviates from the scope of vex robotics, but the tuning he does in this video is very useful.
The task is essentially the same as yours, just using gyroscopic motion instead of (presumably) wheels on a surface.
He uses some tricks beyond basic PID to tune the balancing and gets it to be particularly effective, and the code is shown (but not really explained) in the video. You might consider taking some of his ideas and adding on to what you have already.

[Da_Dude]

Hey jpearman, thanks for all the help. Your suggestion to read that Vex Pendulum comment section was really helpful and we found his code he used in his Vex IQ. So after adjusting the code to the V5 system, but the problem is my teacher and I have reached a point again where we’re just trying to change values and are trouble shooting to reduce oscillations via changing constants and they way @vamfum calculated his output. Here’s a a copy of our code below of what we’ve been able to derive from @vamfum’s post.

// Include the V5 Library
#include "vex.h"
  
using namespace vex;
int main() {
  float motorAngleRaw,                      // The angle of the "motor", measured in degrees. We will take the average of both motor positions, wich is essentially how far the middle of the robot has traveled.
        motorAngle,                         // The angle of the motor, converted to radians (2*pi radians equals 360 degrees).
        motorAngleReference = 0,            // The reference angle of the motor. The robot will attempt to drive forward or backward, such that its measured position equals this reference (or close enough).
        motorAngleError,                    // The error: the deviation of the measured motor angle from the reference. The robot attempts to make this zero, by driving toward the reference.
        motorAngleErrorAccumulated = 0,     // We add up all of the motor angle error in time. If this value gets out of hand, we can use it to drive the robot back to the reference position a bit quicker.
        motorAngularSpeed,                  // The motor speed, estimated by how far the motor has turned in a given amount of time
        motorAngularSpeedReference = 0,     // The reference speed during manouvers: how fast we would like to drive, measured in radians per second.
        motorAngularSpeedError,             // The error: the deviation of the motor speed from the reference speed.
        motorDutyCycle,                     // The 'voltage' signal we send to the motor. We calulate a new value each time, just right to keep the robot upright.
        gyroRateRaw,                        // The raw value from the gyro sensor in rate mode.
        gyroRate,                           // The angular rate of the robot (how fast it is falling forward), measured in radians per second.
        gyroEstimatedAngle = 0,             // The gyro doesnt measure the angle of the robot, but we can estimate this angle by keeping track of the gyroRate value in time.
        gyroOffset = 0;                     // Over time, the gyro rate value can drift. This causes the sensor to think it is moving even when it is perfectly still. We keep track of this offset.

 
Inertial20.calibrate();
wait(2,seconds);
  //Calculating the (initial) avrage gyro sensor value
  const int gyroRateCalibrateCount = 100;
  for (int i = 0; i < gyroRateCalibrateCount; i++){
    gyroOffset = gyroOffset + Inertial20.gyroRate(zaxis,dps);
     //getGyroRate(gyro);
    wait(10,msec);
  }
  //The offset is equal to the long term average value of the sensor.
  gyroOffset = gyroOffset/gyroRateCalibrateCount;

  //Timing settings for the program
  const int loopTimeMiliSec = 20,        // Time of each loop, measured in miliseconds.
            motorAngleHistoryLength = 4; // Number of previous motor angles we keep track of.

  const float loopTimeSec = loopTimeMiliSec/1000.0,              // Time of each loop, measured in seconds.
              radiansPerDegree =3.1415/180.0,                       // The number of radians in a degree.
              radiansPerSecondPerRawGyroUnit = radiansPerDegree, // For the VEX IQ, 1 unit = 1 deg/s
              radiansPerRawMotorUnit = radiansPerDegree,         // For the VEX IQ, 1 unit = 1 deg
              gyroDriftCompensationRate = 0.1*loopTimeSec,       // The rate at which well update the gyro offset
              degPerSecPerPercentSpeed = 7.1,                    // On the VEX IQ, "1% speed" corresponds to 7.1 deg/s (if speed control were enabled)
              radPerSecPerPercentSpeed = degPerSecPerPercentSpeed * radiansPerDegree; //Convert this number to the speed in rad/s per "percent speed"

  //A (fifo) array which well use to keep track of previous motor positions, which we can use to calculate the rate of change (speed)
  float motorAngleHistory[motorAngleHistoryLength];
  memset(motorAngleHistory,0,4*motorAngleHistoryLength);
  int motorAngleIndex = 0;

  const float gainGyroAngle                  = 900*.12,  //For every radian (57 degrees) we lean forward,            apply this amount of duty cycle.
              gainGyroRate                   = 36*.12 ,   //For every radian/s we fall forward,                       apply this amount of duty cycle.
              gainMotorAngle                 = 15*.12 ,   //For every radian we are ahead of the reference,           apply this amount of duty cycle
              gainMotorAngularSpeed          = 9.6*.12,  //For every radian/s drive faster than the reference value, apply this amount of duty cycle
              gainMotorAngleErrorAccumulated = 3*.12;    //For every radian x s of accumulated motor angle,          apply this amount of duty cycle

   // Variables to control the reference speed and steering
   float speed    = 0,
        steering = 0;

   //Joystick positions
   int joyStickLeft = 0,
       joyStickRight = 0;

   //Maximum speed and steering when remote joysticks are fully moved forward
   const int kMaxRemoteSpeed = 20;
   const int kMaxRemoteSteering = 10;

   //Initialization complete
   //setTouchLEDRGB(touchLed, 0, 255, 0);

   //Run the main loop until the touch LED is touched.
   while(true)
   {

      ///////////////////////////////////////////////////////////////
      //
      //  Driving and Steering. Modify the <<speed>> and <<steering>>
      //  variables as you like to make your segway go anywhere!
      //
      //  (If you don't have the controller, just delete the four lines
      //  below. Then <<speed>> and <<steering>> remain zero.)
      //
      ///////////////////////////////////////////////////////////////


      speed    = (Controller1.Axis3.position()) * (kMaxRemoteSpeed   /100.0);
      steering = (Controller1.Axis1.position()) * (kMaxRemoteSteering/100.0);

      ///////////////////////////////////////////////////////////////
      //  (You don't need to modify anything in the sections below.)
      ///////////////////////////////////////////////////////////////

      ///////////////////////////////////////////////////////////////
      //  Reading the Gyro.
      ///////////////////////////////////////////////////////////////

      //gyroRateRaw = getGyroRateFloat(gyro);
      //gyroRate = (gyroRateRaw - gyroOffset)*radiansPerSecondPerRawGyroUnit;
      gyroRate= Inertial20.gyroRate(zaxis,dps)*radiansPerDegree;

      ///////////////////////////////////////////////////////////////
      //  Reading the Motor Position
      ///////////////////////////////////////////////////////////////
      motorAngleRaw = Motor1.position(degrees);
      //motorAngleRaw = (getMotorEncoder(right) + getMotorEncoder(left))/2.0;
      motorAngle = motorAngleRaw*radiansPerDegree;

      motorAngularSpeedReference = speed*radPerSecPerPercentSpeed;//***********
      motorAngleReference = motorAngleReference + motorAngularSpeedReference*loopTimeSec;

      motorAngleError = motorAngle - motorAngleReference;

      ///////////////////////////////////////////////////////////////
      //  Computing Motor Speed
      ///////////////////////////////////////////////////////////////

      motorAngularSpeed = (motorAngle - motorAngleHistory[motorAngleIndex])/(motorAngleHistoryLength*loopTimeSec);
      motorAngleHistory[motorAngleIndex] = motorAngle;
      motorAngularSpeedError = motorAngularSpeed;

      ///////////////////////////////////////////////////////////////
      //  Computing the motor duty cycle value
      ///////////////////////////////////////////////////////////////

      motorDutyCycle =   gainGyroAngle  * gyroEstimatedAngle //less than like 1, at the least, .05
                       + gainGyroRate   * gyroRate //around 1 for medium speed manuvers
                       + gainMotorAngle * motorAngleError // massive number that climbs as you hold the controller, quickly in the hundreds
                       + gainMotorAngularSpeed * motorAngularSpeedError
                       + gainMotorAngleErrorAccumulated * motorAngleErrorAccumulated;
      Brain.Screen.clearScreen();
      Brain.Screen.setCursor(1,1);
      Brain.Screen.print(motorAngleErrorAccumulated);
      ///////////////////////////////////////////////////////////////
      //  Apply the signal to the motor, and add steering
      ///////////////////////////////////////////////////////////////

      Motor1.spin(forward, motorDutyCycle + steering,volt);
      Motor11.spin(forward, motorDutyCycle + steering,volt);

      ///////////////////////////////////////////////////////////////
      //  Update angle estimate and Gyro Offset Estimate
      ///////////////////////////////////////////////////////////////

      gyroEstimatedAngle = gyroEstimatedAngle + gyroRate*loopTimeSec;
      gyroOffset = (1-gyroDriftCompensationRate)*gyroOffset+gyroDriftCompensationRate;

      ///////////////////////////////////////////////////////////////
      //  Update Accumulated Motor Error
      ///////////////////////////////////////////////////////////////

      motorAngleErrorAccumulated = motorAngleErrorAccumulated + motorAngleError*loopTimeSec;
      motorAngleIndex = (motorAngleIndex + 1) % motorAngleHistoryLength;

      ///////////////////////////////////////////////////////////////
      //  Wait for the loop to complete
      ///////////////////////////////////////////////////////////////

      wait(loopTimeMiliSec,msec);

   }
}

wanted to know if you had any other suggestions?
I also looked into it a little more myself and found this article and wanted to see your opinion on if ‘what they say about fixing oscillations’ is viable or not.
link btw, its about 12 paragraphs in case you didn’t want to read into it that much.