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)