Diff: Motors/motor_mixer.cpp

Revision:

0:b0f9c5ac0305

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motors/motor_mixer.cpp	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,140 @@
+#include "motor_mixer.h"
+
+RawSerial pc_motor_mixer(USBTX, USBRX);
+PwmOut motor1_out(D5);
+PwmOut motor2_out(D6);
+PwmOut motor3_out(D9);
+PwmOut motor4_out(D10);
+
+//Definition der Ausgänge für die Motoren
+/*
+                FRONT
+
+    CW M3(D9)           CCW M1(D5)
+        \                   /
+         \                 /
+          -----------------      
+         /                 \
+        /                   \
+    CCW M2(D6)          CW M4(D10)
+    
+                BACK
+
+motor1 = rcgas + roll - pitch + yaw
+motor2 = rcgas - roll + pitch + yaw
+motor3 = rcgas − roll - pitch - yaw
+motor4 = rcgas + roll + pitch - yaw
+*/
+
+void motormixer::motor_all_period(int period)
+{
+    motor1_out.period_ms(period);
+    motor2_out.period_ms(period);
+    motor3_out.period_ms(period);
+    motor4_out.period_ms(period);
+}
+
+
+void motormixer::motor_all_off()
+{
+    motor1_out.pulsewidth_us(1100);
+    motor2_out.pulsewidth_us(1100);
+    motor3_out.pulsewidth_us(1100);
+    motor4_out.pulsewidth_us(1100);
+}
+
+/////////////////////////MOTOR1//////////////////////////////////////////////////////////////////////////////////////////
+void motormixer::motor1_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready)
+{
+
+    int motor1_pwm_ready = motor_all_pwm_ready + roll_error_pwm_ready - pitch_error_pwm_ready + yaw_error_pwm_ready;
+
+    //Wert an Motor-Regler senden
+    if (motor1_pwm_ready < 1200) {motor1_out.pulsewidth_us(1200);}
+    else if (motor1_pwm_ready > 1759) {motor1_out.pulsewidth_us(1759);}
+    
+    else {
+
+        motor1_out.pulsewidth_us(motor1_pwm_ready);
+        //pc_motor_mixer.printf("Output Motor1: %d\n", motor1_pwm_ready);
+    }
+}
+
+/////////////////////////MOTOR2//////////////////////////////////////////////////////////////////////////////////////////
+void motormixer::motor2_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready)
+{
+
+    int motor2_pwm_ready = motor_all_pwm_ready - roll_error_pwm_ready + pitch_error_pwm_ready + yaw_error_pwm_ready;
+
+    //Wert an Motor-Regler senden
+    if (motor2_pwm_ready < 1200) {motor2_out.pulsewidth_us(1200);}
+    else if (motor2_pwm_ready > 1759) {motor2_out.pulsewidth_us(1759);}
+    
+    else {
+
+        //pc_motor_mixer.printf("Output Motor2: %d\n", motor2_pwm_ready);
+        motor2_out.pulsewidth_us(motor2_pwm_ready);
+    }
+}
+
+/////////////////////////MOTOR3//////////////////////////////////////////////////////////////////////////////////////////
+void motormixer::motor3_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready)
+{
+
+    int motor3_pwm_ready = motor_all_pwm_ready - roll_error_pwm_ready - pitch_error_pwm_ready - yaw_error_pwm_ready;
+
+    //Wert an Motor-Regler senden
+    if (motor3_pwm_ready < 1200) {motor3_out.pulsewidth_us(1200);}
+    else if (motor3_pwm_ready > 1759) {motor3_out.pulsewidth_us(1759);}
+    
+    else {
+
+        motor3_out.pulsewidth_us(motor3_pwm_ready);
+        //pc_motor_mixer.printf("Output Motor3: %d\n", motor3_pwm_ready);
+    }
+}
+
+/////////////////////////MOTOR4//////////////////////////////////////////////////////////////////////////////////////////
+void motormixer::motor4_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready)
+{
+
+    int motor4_pwm_ready = motor_all_pwm_ready + roll_error_pwm_ready + pitch_error_pwm_ready - yaw_error_pwm_ready;
+
+    //Wert an Motor-Regler senden
+    if (motor4_pwm_ready < 1200) {motor4_out.pulsewidth_us(1200);}
+    else if (motor4_pwm_ready > 1759) {motor4_out.pulsewidth_us(1759);}
+    
+    else {
+
+        //pc_motor_mixer.printf("Output Motor4: %d\n", motor4_pwm_ready);
+        motor4_out.pulsewidth_us(motor4_pwm_ready);
+    }
+}
+
+/////////////////////////MOTOREN KALIBRIEREN//////////////////////////////////////////////////////////////////////////////////////////
+void motormixer::motor_calibration(int period)
+{
+    pc_motor_mixer.printf("Periode einstellen\n");
+    motor1_out.period_ms(period);
+    motor2_out.period_ms(period);
+    motor3_out.period_ms(period);
+    motor4_out.period_ms(period);
+    pc_motor_mixer.printf("Done\n");
+
+    pc_motor_mixer.printf("Vollast...3Sec warten...\n");
+    motor1_out.pulsewidth_us(1759);
+    motor2_out.pulsewidth_us(1759);
+    motor3_out.pulsewidth_us(1759);
+    motor4_out.pulsewidth_us(1759);
+    
+    wait(5);
+    
+    pc_motor_mixer.printf("Keine Last...3Sec warten...\n");
+    motor1_out.pulsewidth_us(1111);
+    motor2_out.pulsewidth_us(1111);
+    motor3_out.pulsewidth_us(1111);
+    motor4_out.pulsewidth_us(1111);
+    pc_motor_mixer.printf("Done\n");
+    
+    wait(5);
+}
\ No newline at end of file