I know one accelerometer won't cut it since raising my arm in front of me (perpendicular to my chest) and beside me (parallel to my chest) both have acceleration acting on the z-direction, so the robot can't distinguish that at all.
How can I do exactly what that guy in the video did? Is it through a Kalman filter? Quaternions? I'm a bit lost...
Exactly. Use his exact equipment and write his exact code. It might help to know if he learned it all in a forum in a week or over a six years of core work for his dissertation.
Assuming "a bit lost" means you have firmware/software and hardware experience, but lost on the path toward this goal; create projects involving the 6050 in increasing complexity; First one axis, then two, then three, then attach physical parts, then make them move, then design the parts to work together without destroying the other. I believe the MPU6050 is either near the end of its life, or at the end. Try the MPU9250.
I guess to be more specific, how could I specify an angle (something the servo would understand) where if I raise my arm forward, it would be stay at a constant value even though I twist my arm?
With the code I have right now, the pitch goes to 90 degrees with my arm extended in front, but twisting my arm makes the pitch drop to zero:
#include <Wire.h>
//Servo codes
#include <Servo.h>
int servoPin1 = 3; // servo signal attached to digital pin 9
Servo myServo1;
int servoPin2 = 4; // servo signal attached to digital pin 9
Servo myServo2;
//
const int MPU = 0x68; // MPU6050 I2C address
float AX, AY, AZ, Tp, GX, GY, GZ; // Raw accel and gyro data
float Sa = 1670.7; // Sensitivity of accelerometers in s^2/m
float Sg = 7509.87; // Sensitivity of gyroscopes in s/rad
// Pitch and roll variables
float pitch;
float roll;
// Calibrated error values:
int16_t AXe = -894.333;
int16_t AYe = -165.667;
int16_t AZe = 934;
int16_t GXe = -25;
int16_t GYe = 188.6667;
int16_t GZe = 142.6667;
void setup() {
delay(1000);
Serial.begin(115200);
// Set up the code to read the acceleration and rotation rates
Wire.begin();
Wire.beginTransmission(MPU);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
//Servo codes
myServo1.attach(servoPin1);
myServo2.attach(servoPin2);
}
void loop() {
getData();
// Calculate values of pitch and roll
pitch = (180/PI)*atan(AY/(sqrt(pow(AX,2)+pow(AZ,2))));
roll = (180/PI)*atan(-AX/(sqrt(pow(AY,2)+pow(AZ,2))));
Serial.println("Pitch:" + String(pitch,2) + ", Roll:" + String(roll,2) + ", max:" + String(90) + ", min:" + String(-90));
// Print the accelerometer data
// Serial.println("Ax:" + String(AX,2) + ", Ay:" + String(AY,2) + ", Az:" + String(AZ,2)+ ", max:" + String(15) + ", min:" + String(-15));
//// Print the gyroscope data
//Serial.println("Gx:" + String(GX,2) + ", Gy:" + String(GY,2) + ", Gz:" + String(GZ,2) + ", max:" + String(4.363) + ", min:" + String(-4.363));
// //Servo codes
// if (roll<0){
// myServo1.write(roll+90);
// //Serial.println(roll+90);
// } else {
// myServo1.write(roll+90);
// }
// if (pitch<0){
// myServo2.write(pitch+90);
// //Serial.println(pitch+90);
// } else {
// myServo2.write(pitch+90);
// }
}
//Function to update accelerometer and gyro data
void getData() {
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 14, true);
AX = ((Wire.read() << 8 | Wire.read()) - AXe) / Sa;
AY = ((Wire.read() << 8 | Wire.read()) - AYe) / Sa;
AZ = ((Wire.read() << 8 | Wire.read()) - AZe) / Sa;
Tp = Wire.read() << 8 | Wire.read();
GX = ((Wire.read() << 8 | Wire.read()) - GXe) / Sg;
GY = ((Wire.read() << 8 | Wire.read()) - GYe) / Sg;
GZ = ((Wire.read() << 8 | Wire.read()) - GZe) / Sg;
}
It is extremely important to understand that Euler angles are NOT independent variables, like Cartesian coordinates (X, Y, Z), and that the order of applying rotation angles matters a great deal.
There are 12 different definitions of Euler angles in common use.
When it's flat on the table, rotation around the z-axis doesn't work, that's why we can't reliably get yaw. When it's flipped up, it's now rotating around the MPU6050's y-axis (which was initially the z-axis), so what I still consider as "roll" doesn't work anymore since it became the "yaw" (rotating around the z-axis).
I need to find a way to get the same angles from the same tilts regardless of the orientation...
I'm not sure what question to ask to get help with this, but I'm slowly putting the pieces together.
I guess that is my question. Is there a way for, say, "roll" to remain as "roll" regardless if the MPU6050's flat on the table, tilted up, sideways, etc.?
Yes, if you use Euler angles properly, but that is guaranteed to fail in the special cases that pitch is close to +/- 90 degrees. Look up "gimbal lock" to see why.
