![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
code to fly a quadrocopter
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