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();
}
