Quote:
Originally Posted by jakemochas
We did not use a magnetometer. We only used a gyro.
In the end, though, we were not satisfied with any of the gyros. The gyros had significant drift, asymmetrical behavior, and were extremely sensitive to collisions on the field. In about 45 seconds, the gyro would drift by as much as 60 degrees and the advantages of the gyro would be lost.
In order to solve this problem....
One of our goals for next year is to find a better gyro solution for our swerve drive system.
|
Ok I'd like to throw an idea to you, and others feel free to shoot holes through this (couEthergh cough cough).
I'm going to start out with two assumptions that I hope people can help me verify:
1. A gyro... has quick repsonse, but is subject to drift
2. A Magnetometer does not drift but has a slow response (lag)
If these assumptions are correct (to the way I'm thinking they work), then I'd propose the idea of having them work together to yield a quick reponse heading that remains fairly accurate.
I've solved this kind of problem before at work and since you are a c++ programmer... I'll show you the code where I do this. right here:
Code:
size_t GetPlayPos()
{
DWORD playpos;
//size_t SampleOffset=0;
size_t SampleOffset=(size_t)(0.038 * m_SampleRate);
m_lpdsb->GetCurrentPosition(&playpos,NULL);
playpos/=m_BlockAlign; //convert to samples
//Note: This design assumes the buffer size (of our secondary buffer) is the same size as the sample rate. This is fine for now, but at some
//point I may need to make a distinction if I have to change the buffer size.
time_type CPUClock=((__int64)time_type::get_current_time()+m_ClockPhaseOffset) % 10000000;
double CalibratePlayPos=(double)CPUClock*m_SampleRate;
int PhaseOffset=(int)playpos-(int)CalibratePlayPos;
int iSampleRate=(int)m_SampleRate;
//Check wrap-around case
if (PhaseOffset > iSampleRate>>1)
PhaseOffset= ((int)playpos-iSampleRate) - (int)CalibratePlayPos;
else if (PhaseOffset < -(iSampleRate>>1))
PhaseOffset= playpos - ((int)CalibratePlayPos-iSampleRate);
double Current_ClockPhaseOffset=(double)m_ClockPhaseOffset + (((double)PhaseOffset / m_SampleRate) * 10000000.0);
//check math
//CPUClock=((__int64)time_type::get_current_time() + (__int64)Current_ClockPhaseOffset) % 10000000;
//CalibratePlayPos=(double)CPUClock*m_SampleRate;
//printf("Test-> playpos %d, %d \n",playpos,(size_t)(CalibratePlayPos));
const double dSmoothingValue=0.1;
//blend the current phase error to the current phase offset
m_ClockPhaseOffset=(__int64) (((1.0-dSmoothingValue) * (double)m_ClockPhaseOffset) + (dSmoothingValue * Current_ClockPhaseOffset));
//debug_output(p_debug_category,L"playpos %d, %d, %d\n",playpos,(size_t)(CalibratePlayPos),PhaseOffset);
size_t Error=abs(PhaseOffset);
//printf("\r%d ",Error);
//if (Error>1000)
// debug_output(p_debug_category,L"%d\n",Error);
return AdvancePosition((size_t)(CalibratePlayPos),SampleOffset);
}
In this example we want to determine the play position of the audio cursor inside a sound card (using direct sound). The reality is the GetCurrentPosition() method is similar to using (what my assumption is of) the magnetometer where it has lag (about 20 30ms of repeated results). This is not acceptable! So to solve the problem I use the PC clock since it has a quick response (e.g. like the gyro). The way this code works is it applies a continuous error correction to whatever the current clocktime is read... so it would be like using the gyro reading as your base answer and applying the magentometer error correction on it. The design here is similar to using P in PID (in the PhaseOffset variable)... and doing a blend function on the error computed. The blend allows the gyro to have more influence on quick movement and less influence on slower movements while over time it is always recalibrating itself to the correct point of reference. I hope this idea is useful and the assumptions about the sensors are correct... I may have a run with these.
