Skocz do zawartości

Kryteria doboru modułu z MPU6050 dla robota balansującego dwu kołowego.


99teki

Pomocna odpowiedź

- chcę zrobić robota balansującego dwu kołowego na MPU6050 oraz esp32 Wroom.

- tylko nie wiem jakie dobrać parametry wyboru modułu dla MPU6050, mam kilka sztuk i one mają różne parametry, przy tej samej kalibracji.

- kalibracja

Acceleration X [g]= -0.00 Acceleration Y [g]= 0.00 Acceleration Z [g]= 1.00
Acceleration X [g]= -0.00 Acceleration Y [g]= 0.00 Acceleration Z [g]= 1.00

- a wartości Roll angle oraz Pitch angle są różne.

	// white
Roll Angle [°] -0.82 Pitch Angle [°] 0.45
Roll Angle [°] -0.82 Pitch Angle [°] 0.45
  
 // green
Roll Angle [°] 0.09 Pitch Angle [°] 0.15
Roll Angle [°] 0.09 Pitch Angle [°] 0.15  

- korzystam z programu filtra Kalmana 

// 2024.12.31, bubu321
//
// Part-XV Kalman filter https://github.com/CarbonAeronautics https://www.youtube.com/watch?v=5HuN9iL-zxU
/*
The contents of this code and instructions are the intellectual property of Carbon Aeronautics. 
The text and figures in this code and instructions are licensed under a Creative Commons Attribution - Noncommercial - ShareAlike 4.0 International Public Licence. 
This license lets you remix, adapt, and build upon your work non-commercially, as long as you credit Carbon Aeronautics 
(but not in any way that suggests that we endorse you or your use of the work) and license your new creations under the identical terms.
This code and instruction is provided "As Is” without any further warranty. Neither Carbon Aeronautics or the author has any liability to any person or entity 
with respect to any loss or damage caused or declared to be caused directly or indirectly by the instructions contained in this code or by 
the software and hardware described in it. As Carbon Aeronautics has no control over the use, setup, assembly, modification or misuse of the hardware, 
software and information described in this manual, no liability shall be assumed nor accepted for any resulting damage or injury. 
By the act of copying, use, setup or assembly, the user accepts all resulting liability.

1.0  29 December 2022 -  initial release
*/

#include <Wire.h>
float RateRoll, RatePitch, RateYaw;
float RateCalibrationRoll, RateCalibrationPitch, RateCalibrationYaw;
int RateCalibrationNumber;
float AccX, AccY, AccZ;
float AngleRoll, AnglePitch;
uint32_t LoopTimer;
bool a = true;
	//
float KalmanAngleRoll=0, KalmanUncertaintyAngleRoll=2*2;
float KalmanAnglePitch=0, KalmanUncertaintyAnglePitch=2*2;
float Kalman1DOutput[]={0,0};

// ----------- subroutine ----------------------
	// -------------------------------------------
void kalman_1d(float KalmanState, float KalmanUncertainty, float KalmanInput, float KalmanMeasurement) 
{
  KalmanState = KalmanState+0.004*KalmanInput;
  KalmanUncertainty = KalmanUncertainty + 0.004 * 0.004 * 4 * 4;
  float KalmanGain = KalmanUncertainty * 1/(1*KalmanUncertainty + 3 * 3);
  KalmanState = KalmanState + KalmanGain * (KalmanMeasurement-KalmanState);
  KalmanUncertainty = (1 - KalmanGain) * KalmanUncertainty;
  Kalman1DOutput[0] = KalmanState; 
  Kalman1DOutput[1] = KalmanUncertainty;
}
	// ------------------------
void gyro_signals(void) 
{
  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x05);
  Wire.endTransmission();
  Wire.beginTransmission(0x68);
  Wire.write(0x1C);				// zakres skali +/- 8g
  Wire.write(0x10);
  Wire.endTransmission();
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);				// rejestr odczytu Acc
  Wire.endTransmission(); 
  Wire.requestFrom(0x68,6);
  int16_t AccXLSB = Wire.read() << 8 | Wire.read();
  int16_t AccYLSB = Wire.read() << 8 | Wire.read();
  int16_t AccZLSB = Wire.read() << 8 | Wire.read();
  Wire.beginTransmission(0x68);
  Wire.write(0x1B); 
  Wire.write(0x8);
  Wire.endTransmission();     
  Wire.beginTransmission(0x68);
  Wire.write(0x43);																	// rejestr odczytu Gyro
  Wire.endTransmission();
  Wire.requestFrom(0x68,6);
  int16_t GyroX = Wire.read()<<8 | Wire.read();
  int16_t GyroY = Wire.read()<<8 | Wire.read();
  int16_t GyroZ = Wire.read()<<8 | Wire.read();
  RateRoll = (float)GyroX / 65.5;
  RatePitch = (float)GyroY / 65.5;
  RateYaw = (float)GyroZ / 65.5;
  AccX = (float)AccXLSB/4096 - 0.02 ;				// korekcja reczna
  AccY = (float)AccYLSB/4096 + 0.02;
  AccZ = (float)AccZLSB/4096 + 0.49;
  AngleRoll=atan(AccY/sqrt(AccX*AccX+AccZ*AccZ)) * 1/(3.142/180);	// wynik w stopniach
  AnglePitch=-atan(AccX/sqrt(AccY*AccY+AccZ*AccZ)) * 1/(3.142/180);
}
	// -------------- setup ----------------
void setup() {
  Serial.begin(115200);
  pinMode(2, OUTPUT);				// esp32 Wroom GPIO2 LED
  digitalWrite(2, HIGH);
  Wire.setClock(400000);
  Wire.begin();
  delay(250);
  Wire.beginTransmission(0x68); 
  Wire.write(0x6B);
  Wire.write(0x00);				// zasilanie
  Wire.endTransmission();
  for (RateCalibrationNumber = 0; RateCalibrationNumber < 2000; RateCalibrationNumber ++) 
	{
    gyro_signals();
    RateCalibrationRoll += RateRoll;
    RateCalibrationPitch += RatePitch;
    RateCalibrationYaw += RateYaw;
    delay(1);
  }
  RateCalibrationRoll /= 2000;
  RateCalibrationPitch /= 2000;
  RateCalibrationYaw /= 2000;
  delay(1000);
  digitalWrite(2, LOW);
	LoopTimer = micros();
}
	// ------------------- lopp -----
void loop() {
  gyro_signals();
  RateRoll -= RateCalibrationRoll;
  RatePitch -= RateCalibrationPitch;
  RateYaw -= RateCalibrationYaw;
// - start the iteration for the Kalman filter with the roll and pitch
		kalman_1d(KalmanAngleRoll, 
	KalmanUncertaintyAngleRoll, RateRoll, AngleRoll);
  //
	KalmanAngleRoll = Kalman1DOutput[0]; 
  KalmanUncertaintyAngleRoll = Kalman1DOutput[1];	// kat roll
		//
		kalman_1d(KalmanAnglePitch, 
	KalmanUncertaintyAnglePitch, RatePitch, AnglePitch);
		//
	KalmanAnglePitch = Kalman1DOutput[0]; 
  KalmanUncertaintyAnglePitch=Kalman1DOutput[1];	// kat pitch
    // ---- przewidywane wartosci katow ------
	if (a)	
  {
		Serial.print("Roll Angle [°] ");
		Serial.print(KalmanAngleRoll);
		Serial.print(" Pitch Angle [°] ");
		Serial.println(KalmanAnglePitch);
	}
	if(!a)
	{
		Serial.print("Acceleration X [g]= ");
		Serial.print(AccX);
		Serial.print(" Acceleration Y [g]= ");
		Serial.print(AccY);
		Serial.print(" Acceleration Z [g]= ");
		Serial.println(AccZ);
	}	
		//
  while (micros() - LoopTimer < 4000);	// 250 Hz
  LoopTimer = micros();
	// delay(100);
}
	// --- end ---

- zrób robota i zobacz jak będzie działał.

Link do komentarza
Share on other sites

54 minuty temu, 99teki napisał:

- tylko nie wiem jakie dobrać parametry wyboru modułu dla MPU6050, mam kilka sztuk i one mają różne parametry, przy tej samej kalibracji.

Dla mnie to jest tyle samo. Spróbowałbym zacząć od analizy czy obracając moduł z dwoma akcelerometrami różnica między nimi jest identyczna (lub akceptowalnie identyczna), bo wtedy oznacza to, że program nie kompensuje różnic w mocowaniu mechanicznym i wystarczy dodać korekcję kąta dla każdego akcelerometru w oczkiwanej pozycji "zero".

  • Lubię! 1
Link do komentarza
Share on other sites

Bądź aktywny - zaloguj się lub utwórz konto!

Tylko zarejestrowani użytkownicy mogą komentować zawartość tej strony

Utwórz konto w ~20 sekund!

Zarejestruj nowe konto, to proste!

Zarejestruj się »

Zaloguj się

Posiadasz własne konto? Użyj go!

Zaloguj się »
×
×
  • Utwórz nowe...

Ważne informacje

Ta strona używa ciasteczek (cookies), dzięki którym może działać lepiej. Więcej na ten temat znajdziesz w Polityce Prywatności.