Diff: main.cpp

Revision:

0:b0f9c5ac0305

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,290 @@
+#include "mbed.h"
+#include "pwmdeg.h"
+#include "degpwm.h"
+#include "PID_roll.h"
+#include "PID_pitch.h"
+#include "PID_yaw.h"
+#include "mpu_output.h"
+#include "motor_mixer.h"
+
+#define pitch_output
+
+RawSerial pc_main(USBTX, USBRX);
+mpu_output main_mpu_output;
+pwmdeg pwmdeg_main;
+Degpwm Degpwm_main;
+motormixer motormixer_main;
+Timer test;
+
+//Interrupt Handler
+InterruptIn roll_int(A0);       //Channel 1
+InterruptIn pitch_int(A1);      //Channel 2
+InterruptIn throttle_int(D11);  //Channel 3
+InterruptIn yaw_int(D12);       //Channel 4
+
+//Variablen ChannelIn
+volatile uint16_t _pulsewidth_ch1;
+volatile uint16_t _pulsewidth_ch2;
+volatile uint16_t _pulsewidth_ch3;
+volatile uint16_t _pulsewidth_ch4;
+
+//Timer ChannelIn
+Timer _t_ch1;
+Timer _t_ch2;
+Timer _t_ch3;
+Timer _t_ch4;
+
+
+//Init von PID Regler
+//Beispiel:    PIDClass pid_roll(AngleKp, AngleKi, RateKp, RateKd);
+
+//Roll
+PID_ROLL_Class pid_roll(4, 0.0007, 0.020, 0);
+
+//Pitch
+PID_PITCH_Class pid_pitch(4, 0.0007, 0.020, 0);  //PID_PITCH_Class pid_pitch(4, 0.0011, 0.06, 12.35);  //PID_PITCH_Class pid_pitch(4, 0.0011, 0.06, 12.35);  //PID_PITCH_Class pid_pitch(4, 0.0011, 0.06, 12.35);
+
+//Yaw
+PID_YAW_Class pid_yaw(4, 0.000, 0.000, 0.0); //PID_YAW_Class pid_yaw(4, 0.0002, 0.015, 0.0); Stärker einstellen!!!!
+
+float Rate = 0, dt = 10;
+
+//Main Funktionen
+void pwm_in_main();
+void pitch_rc_main();
+void roll_rc_main();
+void yaw_rc_main();
+void motors_ready_main();
+void pwm_read_main();
+void rise_ch1();
+void fall_ch1();
+void rise_ch2();
+void fall_ch2();
+void rise_ch3();
+void fall_ch3();
+void rise_ch4();
+void fall_ch4();
+
+//PwmIn Variablen
+int ele_in_main = 0; //Channel 1
+int ail_in_main = 0; //Channel 2
+int thr_in_main = 0; //Channel 3
+int rud_in_main = 0; //Channel 4
+
+
+//Übergabevariablen
+//Channel1
+float roll_error = 0;
+int roll_error_pwm_ready = 0;
+
+//Channel2
+float pitch_error = 0;
+int pitch_error_pwm_ready = 0;
+
+//Channel3
+float yaw_error = 0;
+int yaw_error_pwm_ready = 0;
+
+//Channel4
+int motor_all_pwm_ready = 0;
+
+
+
+////////////////////////////////////MAIN///////////////////////////////////////////////////////////////////////
+int main()
+{
+    NVIC_SetPriority(EXTI3_IRQn, 0x01); //Priorität von Interrupt des Sensors dekrementieren!!!
+
+    main_mpu_output.Init();             //Sensor initalisieren
+    main_mpu_output.dmpDataUpdate();
+    
+    //MBED_ASSERT(main_mpu_output.Init() == true);
+    
+    motormixer_main.motor_all_period(2.5);        //Einstellen Periode Motoren
+    //motormixer_main.motor_calibration(2.5); //Kalibrieren der Motoren
+
+    //ISR Funktionen RC ChannelIn
+    roll_int.rise(&rise_ch1);
+    pitch_int.rise(&rise_ch2);
+    throttle_int.rise(&rise_ch3);
+    yaw_int.rise(&rise_ch4);
+
+    while(1) {
+        test.start();
+
+        //Alle Channel einlesen
+        pwm_in_main();
+
+        //PWM Signale des RC in Grad umrechnen, Roll-Error abspeichern (danach in PWM)
+        roll_rc_main();
+
+        //PWM Signale des RC in Grad umrechnen, Pitch-Error abspeichern (danach in PWM)
+        pitch_rc_main();
+
+        //PWM Signale des RC in Grad umrechnen, Yaw-Error abspeichern (danach in PWM)
+        yaw_rc_main();
+
+        //Werte in Mototrmixer schreiben --> ansteuern der Motoren
+        motors_ready_main();
+
+        //pc_main.printf("hier\n");
+        test.stop();
+        int zeit = test.read_ms();
+        //pc_main.printf("zeit: %d\n", zeit);
+        test.reset();
+
+    }
+}
+
+///////////////////ISR von Rise Channel 1 Roll/////////////////////////////////////
+void rise_ch1()
+{
+    _t_ch1.start();
+    roll_int.fall(&fall_ch1);//Interrupt Fall
+}
+
+void fall_ch1()
+{
+    _t_ch1.stop();
+    if(_t_ch1.read_us()>=1000&&_t_ch1.read_us()<=2000) {
+        _pulsewidth_ch1 = _t_ch1.read_us();
+        //pc_main.printf("ch1: %d\n", _pulsewidth_ch1);
+    }
+    _t_ch1.reset();
+
+}
+
+///////////////////ISR von Rise Channel 2 Pitch/////////////////////////////////////
+void rise_ch2()
+{
+    _t_ch2.start();
+    pitch_int.fall(&fall_ch2);//Interrupt Fall
+
+}
+
+void fall_ch2()
+{
+    _t_ch2.stop();
+    if(_t_ch2.read_us()>=1000&&_t_ch2.read_us()<=2000) {
+        _pulsewidth_ch2 = _t_ch2.read_us();
+        //pc_main.printf("ch1: %d\n", _pulsewidth_ch2);
+    }
+    _t_ch2.reset();
+
+}
+
+///////////////////ISR von Rise Channel 3 Throttle/////////////////////////////////////
+void rise_ch3()
+{
+    _t_ch3.start();
+    throttle_int.fall(&fall_ch3);//Interrupt Fall
+}
+
+void fall_ch3()
+{
+    _t_ch3.stop();
+    if(_t_ch3.read_us()>=1000&&_t_ch3.read_us()<=2000) {
+        _pulsewidth_ch3 = _t_ch3.read_us();
+        //pc_main.printf("ch1: %d\n", _pulsewidth_ch1);
+    }
+    _t_ch3.reset();
+
+}
+
+///////////////////ISR von Rise Channel 4 YAW/////////////////////////////////////
+void rise_ch4()
+{
+    _t_ch4.start();
+    yaw_int.fall(&fall_ch4);//Interrupt Fall
+
+}
+
+void fall_ch4()
+{
+    _t_ch4.stop();
+    if(_t_ch4.read_us()>=1000&&_t_ch4.read_us()<=2000) {
+        _pulsewidth_ch4 = _t_ch4.read_us();
+        //pc_main.printf("ch4: %d\n", _pulsewidth_ch4);
+    }
+    _t_ch4.reset();
+
+}
+
+
+///////PWM In abspeichern///////////////////////////////
+
+void pwm_in_main()
+{
+    ail_in_main = _pulsewidth_ch1;
+    ele_in_main = _pulsewidth_ch2;
+    thr_in_main = _pulsewidth_ch3;
+    rud_in_main = _pulsewidth_ch4;
+
+    //pc_main.printf("throttle: %d|pitch: %d|roll: %d|rud: %d \n", thr_in_main, ele_in_main, ail_in_main, rud_in_main);
+
+}
+
+///////////Roll Rechnen und Abspeichern///////////////////////////////////////////////////////////////////////////////////
+void roll_rc_main()
+{
+    //Roll-Error abspeichern (in PWM)
+    float Setpoint_Roll = pwmdeg_main.roll(ail_in_main), CurrentPosition_Roll = main_mpu_output.sensor_roll();
+    roll_error = pid_roll.update_roll(Setpoint_Roll, CurrentPosition_Roll, Rate, dt);
+    //pc_main.printf("Output pitch error: %d\n", pitch_error);
+    roll_error_pwm_ready = Degpwm_main.roll_pwm(roll_error);
+    //pc_main.printf("Output roll error pwm: %d\n", roll_error_pwm_ready);
+}
+
+///////////Pitch Rechnen und Abspeichern///////////////////////////////////////////////////////////////////////////////////
+void pitch_rc_main()
+{
+    float Setpoint_Pitch = pwmdeg_main.pitch_forward(ele_in_main), CurrentPosition_Pitch = main_mpu_output.sensor_pitch();
+    pitch_error = pid_pitch.update_pitch(Setpoint_Pitch, CurrentPosition_Pitch, Rate, dt);
+    //pc_main.printf("Output pitch error: %f\n", pitch_error);
+    pitch_error_pwm_ready = Degpwm_main.pitch_pwm(pitch_error);
+    //pc_main.printf("Output pitch error pwm: %d\n", pitch_error_pwm_ready);
+    #ifdef pitch_output
+        pc_main.printf("SetAccel: %f| CurrentAccel: %f| Output pitch error pwm: %d|pitch error: %f\n", Setpoint_Pitch, CurrentPosition_Pitch, pitch_error_pwm_ready, pitch_error);
+    #endif
+
+}
+
+
+///////////Yaw Rechnen und Abspeichern///////////////////////////////////////////////////////////////////////////////////
+void yaw_rc_main()
+{
+    //Yaw Error abspeichern (in PWM)
+    float SetAccel_Yaw = pwmdeg_main.yaw(rud_in_main), CurrentAccel_Yaw = main_mpu_output.sensor_yaw();
+    yaw_error = pid_yaw.update_yaw(SetAccel_Yaw, CurrentAccel_Yaw, Rate, dt);
+    yaw_error_pwm_ready = Degpwm_main.yaw_pwm(yaw_error);
+    //pc_main.printf("Output yaw error pwm: %d\n", yaw_error_pwm_ready);
+    //pc_main.printf("SetAccel: %f| CurrentAccel: %f| Output yaw error pwm: %d|yaw error: %f\n", SetAccel_Yaw, CurrentAccel_Yaw, yaw_error_pwm_ready, yaw_error);
+
+}
+
+///////////Übergabe an Motormixer///////////////////////////////////////////////////////////////////////////////////
+void motors_ready_main()
+{
+    //yaw_error_pwm_ready = 0;
+    //Throttle abspeichern (in PWM)
+    if(thr_in_main > 1111) {
+
+        motor_all_pwm_ready = thr_in_main;
+
+        //PWM Werte in Motormixer und in fertige Motoren in Variablen speichern
+        //Motor1
+        motormixer_main.motor1_ready(motor_all_pwm_ready, roll_error_pwm_ready, pitch_error_pwm_ready, yaw_error_pwm_ready);
+        //Motor2
+        motormixer_main.motor2_ready(motor_all_pwm_ready, roll_error_pwm_ready, pitch_error_pwm_ready, yaw_error_pwm_ready);
+        //Motor3
+        motormixer_main.motor3_ready(motor_all_pwm_ready, roll_error_pwm_ready, pitch_error_pwm_ready, yaw_error_pwm_ready);
+        //Motor4
+        motormixer_main.motor4_ready(motor_all_pwm_ready, roll_error_pwm_ready, pitch_error_pwm_ready, yaw_error_pwm_ready);
+    }
+
+    else {
+        //pc_main.printf("throttle: %d\n", throttle_in.pulsewidth());
+        motormixer_main.motor_all_off();
+    }
+
+}
\ No newline at end of file