$ Balancing Tray Project Troubleshooting $

Hello, I have posted on this forum before about my project but I seem to get no where with my own programming skills, so I am looking for a programmer who would be able to troubleshoot my code/ alter it in order to fix the two problems I am having, I will of course pay for such help.

My project is fairly simple in comparison to the stuff some other people are doing, but basically it is a tray that has to stay in a stable upright position, regardless of the frames that are held.

The self stabilizing in my case is achieved by having Arduino along with a MPU6050 Gyroscope connected directly to the bottom of the Tray. (It is upside down due to the fact that there was no other way to attach it)

The two frames have 20 kg*cm servo motors that work in reverse of the motion registered from the MPU6050. Meaning for example if the Tray along with the Gyroscope is rotated clockwise 30 degrees. The servo motor will rotate counter clockwise 30 degrees. Thus, although the frame itself will be tilted 30 degrees clockwise, the tray itself will remain in the upright position.

The project has two frames since each one is responsible for the motion in each axis.

The code was taken online and altered for my implementation, although unfortunately I don't quite remember the source now.

So here are the two problems I am facing, (I will post a video on Youtube to show the prototype)

First problem: The servo motors seem to oscillate, meaning that if you are tilt the frame 30 degrees clockwise, the servo motor that holds the tray, will first rotate approximately 30 degrees counter clockwise and then rotate back. This is hard to explain so please refer to the Youtube video to see what I am talking about. I only connected one of the motors for the presentation, but same goes for the second motor on the other frame.

Second problem: The servo motors don't seem to quite match the opposite angle of Gyroscope reading, meaning for example if we tilt the farm 90 degrees, we'd expect servo motor to rotate 90 degrees in opposite direction so that the tray will be exactly perpendicular to the frame. However as once again you'd be able to see in the Youtube video, when the frame is tilted 90 degrees the servo motor (when not oscillating from first problem) it will rotate approximately 75 degree.

Also just on a side note I am looking to find out what does MPU offsets are, and how they effect the project. Since I'm assuming they are needed to calibrate the gyroscope.

Please note that in the "Loop" portion of the code which is responsible for the function of the Gyroscope

Void Loop Code:

void loop() {
  if (mpuInterrupt ) { // wait for MPU interrupt or extra packet(s) available
    GetDMP(); // Gets the MPU Data and canculates angles
  }
//*****************************************************************************************************************************************************************************
//************************************              Put any code you want to use the values that come from your MPU6050 here               ************************************     
//*****************************************************************************************************************************************************************************

  static long QTimer = millis();
  if ((long)( millis() - QTimer ) >= 100) {
    QTimer = millis();
    

                 // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            // Serial.print(ypr[0] * 180/M_PI); // Z Coordinate
            
              if (ypr[1] > 0)
            {
            ypr[1]=ypr[1]-M_PI;
            }
            else if (ypr[1] < 0)
            {
            ypr[1]=ypr[1]+M_PI;
            }
            myservoX.write(int(ypr[1] * 180/M_PI)+90);   // Rotation around X
              delay(20);
            
              if (ypr[2] > 0)
            {
            ypr[2]=ypr[2]-M_PI;
            }
            else if (ypr[2] < 0)
            {
            ypr[2]=ypr[2]+M_PI;
            }
            Serial.print("\t");
            Serial.print(ypr[2] * 180/M_PI);
            
            Serial.print("\t");
            Serial.println(ypr[1] * 180/M_PI);
            myservoY.write(int(ypr[2] * 180/M_PI)+90);   // Rotation around Y
              delay(20);
  }
}

The if/ else statements are there because My Gyroscope is attached upside down, so in order for the Servo motors to operate properly, I needed to convert the degrees so that the orientation of the gyroscope would not matter. (Perhaps there was another way of doing this, but technically this works so I'm not sure if this has to be changed, after all it might be one of the causes for my problems.)
The delay(20) is pehaps unnecessary, I added it in the hopes that the program will slow down and prevent the oscillation.

Also please note, although it might seem I know what I'm talking about, I am not that knowledgable with the coding, so you will perhaps have to dumb it down it for me =D. The code seems to work in the sense of what I am trying to achieve however do to those problems causing my project to be useless as of right now.

Please reply here or message me for clarification as well as an agreement on the payment etc.

Thank You!

Attached are the Code, my best attempt at connection of the Project on Fritzing, and the real connection just for the visual of my project.

Fixing_Code_Final.ino (10.8 KB)

What's the budget?

If in regards to the budget for the programming, I would like to know the scope of the work that has to be done and then go from there.
I understand that the first problem will most likely involve adding some sort of room for error in the gyroscope reading before servo motor response, I simply don't have enough C knowledge to implement it, and the second problem would most likely involve calibration of Gyroscope which could be either as simple as adding calibration code in the beginning of the whole code or something much more complex.
If anyone who understands what it will take, I would like to discuss the scope of work, to pay most likely hourly. I'm simple lost in this case.

You have mounted the MPU6050 to the moving part of the platform? In combination with RC-Servos this adds "some" problems (there's a reason why gimbals don't do it this way).

Yea that seemed like the easiest way to do it, with the limited knowledge my hopes were to simple counter the readings that gyroscope gives by rotating the servo in opposite direction, I saw similar projects that have gyroscope attached to the frame itself however I had no clue how to implement this. I still hope that by introducing some room for error let's say 5 or so degrees, that the project would still be able to operate, it does not have to perfect but simply achieve my goal for the time being.

Hm ... I've tried it myself some years ago, but never could stop the oszillation. When oszillation was dampened enough, the setport was reached by creeping. Reason was that the servos have an internal control loop (no big surprise there) - which behaved quite non-deterministic.

Oh, so by creeping you mean making the servo motor slow down before reaching the designated degree? I was hoping adding a delay would have a similar effect but since the servo motors mirror the gyroscope reading it doesn't seem to help.

The strange thing is the code seemed to work better initially, it would rotate correctly without oscillation, however once I added the second frame it started to oscillate, this was the reason I didn't change my project implementation, because initially it was working, at least better. Didn't realize this would give me trouble once the full framework was completed.

My brainstorming: also thanks for replying, forces me into much better critical thinking.

But here's what I am thinking.

As it stands right now, if the tray is standing on the most even surface on earth in the most ideal condition. the reading from Gyroscope is 0 degrees X and 0 degrees Y... World is not perfect and most certainly my annoying a** project. so I give it a room for error of let's say 10 degrees. Since it will be jump from positive to negative that means from -10 degrees to +10 degrees.

I take the reading from the Gyroscope. If it's for example at an instance is giving me a reading of let's say +20 degrees. I make the program take that 20 (x variable). subtract a 1 (x-1). and move the servo motor a so called 1 degree. then I make the program recheck the reading, it checks it, now it's 19 degrees, once again it does the same operation now going to 18. This repeats until it reaches my room for error. That being those 10 degrees. although now I understand it will most likely should be more like 5 degrees room for error. This will also be copied and altered to work for the negative numbers, and then be copied and mirrored for the other axis (Y).

I was just wondering, would the program be fast enough to do this? Because the most I hope to achieve is let's say 90 degrees. which means it has to run a loop of almost 85 times.

Basicly correct, but you cannot rely on gyro readings alone. You need sensor fusion of ACC+GYRO to get sensible readings. You can implement that on the arduino (which gives asmall but significant delay) or you can use the built-in DMP of the MPU6050 to do the fusion for you. That's step one. Now that you have the correct angle, just move the servos accordingly. Watch out, there is that beast called "gimbal lock", see if you need to take care of it in your application. And you do not need a deadband any more, you can use the servos to correct in realtime - that is, if your servos are of reasonable quality, 'cause they will move quite a lot.

Hi,
Have you any hardware to try and code with?
To test your musings?

Now its get down and dirty and experiment.
Can you tell us your electronics, programming, arduino, hardware experience?

Tom... :slight_smile:

IMO the hardware is shown in the video in the first posting.

Oh, when looking at your video again ... if you use ACC (IFR or FIR with t ~ 0.5s) instead of GYRO, you should be able to get a reasonable result even in your setup. It will creep, but that's probably not the big issue.

If in regards to the Hardware, I use MPU6050, which I believe is both Gyroscope and Accelerometer. Unfortunately I can only understand the concept of what an accelerometer does, I don't really know how to implement it in my project. knowledge is very limited as I simply my previous knowledge I had from classes like C++ and Microcontrollers. So Arduino isn't quite coming easy for me =D.

My hopes were to simply use the idea shown in this video

Except by reversing the controls to make motors rotate in the opposite direction of the MPU6050.
I found a code that would simply control the Servo using MPU6050 as seen in video (Not the link in the video, thank sketch was broken) and then I altered it to reverse controls and to change orientation to upside down.

Unfortunately didn't realize there would be so much other problems :slightly_frowning_face:

I don't know how you'd do it without mounting the MPU-6050 on the moving platform: you need to know its orientation.
I agree that a servo is a bad idea: now you have to manage two feedback loops, one of which you don't control.

I did a proof of concept for a 1-axis stabilizer (the platform only needs one degree of freedom). It works very well: StableTable - YouTube

We haven't built a full-size version yet, but the PoC is very promising.

cedarlakeinstruments:
I don't know how you'd do it without mounting the MPU-6050 on the moving platform: you need to know its orientation.

You need to know the (absolute) orientation of one part in the system. All oter parts are connected voia cinematics. When you place the sensor on the body which tends to move first, your system is more responsive. If you have backlash in the system ... oh my.

cedarlakeinstruments:
I don't know how you'd do it without mounting the MPU-6050 on the moving platform: you need to know its orientation.
I agree that a servo is a bad idea: now you have to manage two feedback loops, one of which you don't control.

I did a proof of concept for a 1-axis stabilizer (the platform only needs one degree of freedom). It works very well: https://www.youtube.com/watch?v=Ezwf3kFpk7c

We haven't built a full-size version yet, but the PoC is very promising.

Oh that's very Nice!

I was wondering what's the logic behind making the motor move? I have tried to rework my code

   if (ypr[1] > (5*M_PI)/180) // 5 degree room for movement before servo is active
            {
            temp[1]=ypr[1]-M_PI; // if the reading is greater than 5 degrees
            }
            else if (ypr[1] < -(5*M_PI)/180) // 5 degree room for movement before servo is active
            {
            temp[1]=ypr[1]+M_PI; // if the reading is less than -5 degrees
            }
            else
            {
              temp[1]=ypr[1]; // if the reading is between -5 and 5, simply output 
            }
            myservoX.write(int(temp[1] * 180/M_PI)+90);   // Input to the SErvo

I added the comments, just wondering if you also had to use if/ else statements depending on which way you are tilting your project.

I'm not using a (RC) servo. I'm using a DC gear motor and in essence, that and the MPU6050 and code form my own servomechanism.

The difference between desired angle and current angle is interpreted as an error. If the error is inside a certain deadband (5 degrees in your case), then the PWM generated to control the motor is zero, so it won't move. Otherwise, depending on the sign of the error, the motor direction is set clockwise or counterclockwise and the PWM value is sent to the motor.

    double deadband = 2.0;
    double error = -1.0 * ((ypr[2] * 180/M_PI) - _setpoint);
    int absError = abs(error);
    int pwm = (absError < 45.0 && absError > deadband) ? (int)(absError * 7.0) & 255 : 0;
    bool dir = error > 0 ? true : false;
    digitalWrite(A1, dir);
    digitalWrite(A2, !dir);            
    analogWrite(SPD_PIN, pwm);

cedarlakeinstruments:
I'm not using a (RC) servo. I'm using a DC gear motor and in essence, that and the MPU6050 and code form my own servomechanism.

The difference between desired angle and current angle is interpreted as an error. If the error is inside a certain deadband (5 degrees in your case), then the PWM generated to control the motor is zero, so it won't move. Otherwise, depending on the sign of the error, the motor direction is set clockwise or counterclockwise and the PWM value is sent to the motor.

    double deadband = 2.0;

double error = -1.0 * ((ypr[2] * 180/M_PI) - _setpoint);
    int absError = abs(error);
    int pwm = (absError < 45.0 && absError > deadband) ? (int)(absError * 7.0) & 255 : 0;
    bool dir = error > 0 ? true : false;
    digitalWrite(A1, dir);
    digitalWrite(A2, !dir);           
    analogWrite(SPD_PIN, pwm);

Oh ok Thank You!

Technically this is the same concept I am trying to achieve, I check the MPU6050 reading and then depending on if it's greater than 5 or -5 make the servo motor move accordingly, however I believe I have it wrong in the sense that I am hard coding the numbers in instead of making the servo motor rotate clockwise/ counter clockwise until they reach the condition of between -5 to 5. Will try to change that up with some commands I have found online for this.

I was also wondering, I noticed that many if not all of these balancing projects use MPU offsets. I used the following tutorial to find mine so called offsets to input into my program.

Do you happen to know what's the purpose of these offsets? They seem important but I can't quite figure out what exactly they change in my program.

I haven't looked into the specifics of calibration and my program is still using the default values. However, based on my previous experience with accelerometers & gyros, it's probably compensating for the difference in ideal placement of the sensor, versus the at-rest position in your mechanism.

One thing I have found is that my system really wants to have the z-axis of the '6050 vertical at power-on. This could be because I haven't calibrated it yet.