Przeszukaj forum

- 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ł.