Hi there,
I have bought an MPU6050 chip to measure the angular velocity of a steering wheel. The sensor has a gyroscope for each axis so the idea is to place it anywhere on the wheel and calculate the angular velocity from its values.
My thoughts are based on the Wikipedia article about spheric coordinates.
My idea was, that the angular velocity around the x-axis is being represented by a vector along the x-axis with the length of the value of the velocity. Same for y and z.
From those 3 vectors i am calculating a resulting vector, which length is the resulting angular velocity of the whole Thing:
w_res = sqrt(w_x² + w_y² + w_z²)
I can also find the corresponding angles of the resulting vector to find out in which direction the wheel is turning (back and forth).
So far so good, but now on a testing Setup I have found that the results of the sensor vary, depending if it's turned along an axis of the sensor or if it's tilted. There are differences of up to 70 °/sec, while it is Spinning at 1080 °/sec.
The values in same positions can be reproduced.
So before I am questioning the whole Setup: can someone with a more profound knowledge tell me if there is a math problem?
Cheers,
Nik
#include<math.h>
#include <toneAC.h>
unsigned char Re_buf[11],counter=0;
unsigned char sign=0; //indicator, if data from bluetooth are in buffer
unsigned int frequency; //Zero-frequency/offset
float a[3],w[3],angle[3],T; //Arrays for acceleration, angular speed, angle and temperature
short i=0; //Counter of loops. Used for indication of successful connection
//short axis = 2; //axis of measurement. 0: x across direction of BT-module, 1: y along direction of BT-module, 2 = z;
long double r; //resulting absolute angular speed
double r_ges; //resulting signed angular speed
long double theta; //Angle of rotation axis in spherical coordinates
long double phi; //Angle of rotation axis in spherical coordinates
int mxvlue = 0;
int mnvlue = 0;
void setup() {
pinMode(13,OUTPUT);
digitalWrite(13, LOW); //Indication for connection. Default LOW
Serial.begin(115200); //Initialize serial. Baud rate of 115200 is fixed on the sensor
}
void loop(){
if(sign)
{
i=0;
sign=0;
if(Re_buf[0]==0x55) //检查帧头 -- Check the header
{
switch(Re_buf [1])
{
case 0x51:
a[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*16;
a[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*16;
a[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*16;
T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
break;
case 0x52:
w[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*2000;
w[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*2000;
w[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*2000;
T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
break;
case 0x53:
angle[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*180;
angle[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*180;
angle[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*180;
T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
/* ##### Converting values to spheric coordinates #####
*
* With x, y, z as rotation vectors and values of w[...] as angluar speed. r will be new resulting angular speed, with theta and phi as vector angles.
* There is only ONE degree of freedom: reversing the rotation will cause ONE of the angles to switch from positive to negative or vice versa.
* Multiplication of r value with sign of angles will indicate rotational direction
*
*/
r = sqrt(pow(w[0],2)+pow(w[1],2)+pow(w[2],2));
theta = acos(w[2]/r)/M_PI*180;
phi = atan2(w[1],w[0])/M_PI*180;
r_ges = r * theta/abs(theta) * phi/abs(phi);
Serial.print("r, theta, phi: ");
/*### Formatting data for test output ###*/
if(int(r_ges)>=0){
Serial.print(" ");
}
if(abs(int(r_ges))<10){
Serial.print(" ");
}
else if(abs(int(r_ges))<100){
Serial.print(" ");
}
/*### End of formatting ###*/
Serial.print(int(r_ges)); Serial.print("; ");Serial.print(int(w[1]));
Serial.print('\t');
/*### if angular velocity is 0, angles go crazy ###*/
if(int(r_ges) == 0){
Serial.print("N/A"); Serial.print('\t');
Serial.println("N/A");
}
else{
Serial.print(int (theta)); Serial.print('\t');
Serial.println(int (phi));
}
/*
if(mxvlue<int(r_ges)){
mxvlue = int (r_ges);
}
if(mnvlue>int(r_ges)){
mnvlue = int(r_ges);
}
Serial.print(mxvlue); Serial.print("; ");
Serial.println(mnvlue);
*/
frequency = 2500 + r_ges;
toneAC(frequency);
break;
}
}
}
else{
i++;
if(i>10000){
digitalWrite(13,LOW); //Wait for a high number of loops. If still no connection to BT, turn off LED
}
}
}
void serialEvent() {
while (Serial.available()){
digitalWrite(13,HIGH); //Indicate successful data transfer from BT
//char inChar = (char)Serial.read(); Serial.println(inChar); //Output Original Data, use this code
Re_buf[counter]=(unsigned char)Serial.read();
if(counter==0&&Re_buf[0]!=0x55) return; //第0号数据不是帧头 -- No. 0 data is not a header
counter++;
if(counter==11) //接收到11个数据 -- Received 11 data
{
counter=0; //重新赋值，准备下一帧数据的接收 -- Reassigned, ready to receive the next frame of data
sign=1;
}
}
}