int main(void) { int i = 100000000; int j = 10000000; int cnt = 1000; delay_init(); led_Configuration(); MPU6050_Configuration(); // PWM_Configuration(); TIM3_PWM_Init(); TIM1_PWM_Init(); TIM2_PWM_Init(); // SysTick_Config(SystemCoreClock / 1000); // ms CtrData = CTRL_UP; while (i-- > 0) { while (j-- > 0) { ; } } while (1) { if(schedulercnt_2ms >= 2) { if (cnt > 0) { //读加速度计和陀螺仪数据 READ_MPU6050(); //飞控函数 Control(); //PWM波输出函数 PWMControl(PWM); cnt--; } else { CtrData = CTRL_STOP; Control(); PWMControl(PWM); } schedulercnt_2ms = 0; } } }
//------------------------------------------------------------------------------------------------------------------------------------------------------------------ // Interrupt-Handler des Systick timers //------------------------------------------------------------------------------------------------------------------------------------------------------------------ void SysTick_Handler(void) { GPIOC->ODR |= (1UL << 5); // Pin5 an Port C oszilloskopieren um dauer für den Systick Handler zu beobachten (optional) // Beschleunigungswerte und Drehraten vom MPU-6050 lesen i2cData[0] = 0x3B; // Register 0x3B stösst das Auslesen an I2C_Write(I2C1, i2cData, 1, I2C_MPUAddress); I2C_Read(I2C1, i2cDataRead, 14, I2C_MPUAddress); // Daten ordnen und abspeichern accY = ((i2cDataRead[2] << 8) | i2cDataRead[3]); // gelesene Daten in Variable speichern accZ = ((i2cDataRead[4] << 8) | i2cDataRead[5]); // gelesene Daten in Variable speichern gyroX = ((i2cDataRead[8] << 8) | i2cData[9]); // gelesene Daten in Variable speichern //Kalibrierung von accY, accZ und gyroX - Werte wurden in einem Versuch ermittelt accY = accY + 150; accZ = accZ - 451.63; gyroX = gyroX + 273.25; accYangle = (atan2(accY, accZ))*RAD_TO_DEG; // Winkel aus den Beschleunigungswerten errechnen accYangle = accYangle + angleOffset; // Winkeloffset addieren gyroXrate = gyroX / GYRO_SCALE_FACTOR; // Winkelgeschwindigkeit nach Skalierung // erneute Initialisierung der Winkel bei Vorzeichenwechsel if((accYangle < -90 && kalAngle > 90) || (accYangle > 90 && kalAngle < -90)) { setAngle(accYangle); compAngle = accYangle; kalAngle = accYangle; gyroAngle = accYangle; } // Kalman-Winkel mit Kalman-Funktion erhalten else kalAngle = getAngle(accYangle, gyroXrate, sys_8); gyroAngle = gyroAngle + gyroXrate * sys_8; // Winkel aus den Winkelgeschwindigkeitswerten errechnen compAngle = 0.93 * (compAngle + gyroXrate * sys_8) + 0.07 * accYangle; // Winkel über Komplementärfilter berechnen // Drift des Drehratensensors abfangen if((gyroAngle < -180) || (gyroAngle > 180)) gyroAngle = kalAngle; if(abs(kalAngle) < 45) { // erkennen ob Roboter umgefallen -> wenn < 45, dann nicht umgefallen //Relegung e = kalAngle; // Sollwinkelvorgabe eSum = eSum + e * sys_8; // Integration -> I-Anteil dE = (e - last_e) / sys_8; // Differentiation -> D-Anteil Output = kp * e + ki * eSum + kd * dE; // Reglerausgang berechnen LOutput = Output; // Ausgangswert für linken Motor ROutput = Output; // Ausgangswert für rechten Motor last_e = e; // Aktualisierung des Eingangsparameters PWMControl(); // PWM-Signal ausgeben } //if else { // erkennen ob Roboter umgefallen -> wenn >= 45, dann umgefallen TIM2->CCR1 = 0; // linken Motor abschalten TIM2->CCR2 = 0; // rechten Motor abschalten while(1) {} // Endlosschleife -> Reset notwendig } //else GPIOC->ODR &= 0xDF; // Pin5 an Port C oszilloskopieren um dauer für den Systick Handler zu beobachten (optional) } //Interrupt-Handler des Systick timers
void BLDC::Control() { PWMControl(); }