Dominik Diedrich
/
drone_quado_x
code to fly a quadrocopter
Motors/motor_mixer.cpp@0:b0f9c5ac0305, 2020-05-05 (annotated)
- Committer:
- DD1993
- Date:
- Tue May 05 21:11:38 2020 +0000
- Revision:
- 0:b0f9c5ac0305
initial
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
DD1993 | 0:b0f9c5ac0305 | 1 | #include "motor_mixer.h" |
DD1993 | 0:b0f9c5ac0305 | 2 | |
DD1993 | 0:b0f9c5ac0305 | 3 | RawSerial pc_motor_mixer(USBTX, USBRX); |
DD1993 | 0:b0f9c5ac0305 | 4 | PwmOut motor1_out(D5); |
DD1993 | 0:b0f9c5ac0305 | 5 | PwmOut motor2_out(D6); |
DD1993 | 0:b0f9c5ac0305 | 6 | PwmOut motor3_out(D9); |
DD1993 | 0:b0f9c5ac0305 | 7 | PwmOut motor4_out(D10); |
DD1993 | 0:b0f9c5ac0305 | 8 | |
DD1993 | 0:b0f9c5ac0305 | 9 | //Definition der Ausgänge für die Motoren |
DD1993 | 0:b0f9c5ac0305 | 10 | /* |
DD1993 | 0:b0f9c5ac0305 | 11 | FRONT |
DD1993 | 0:b0f9c5ac0305 | 12 | |
DD1993 | 0:b0f9c5ac0305 | 13 | CW M3(D9) CCW M1(D5) |
DD1993 | 0:b0f9c5ac0305 | 14 | \ / |
DD1993 | 0:b0f9c5ac0305 | 15 | \ / |
DD1993 | 0:b0f9c5ac0305 | 16 | ----------------- |
DD1993 | 0:b0f9c5ac0305 | 17 | / \ |
DD1993 | 0:b0f9c5ac0305 | 18 | / \ |
DD1993 | 0:b0f9c5ac0305 | 19 | CCW M2(D6) CW M4(D10) |
DD1993 | 0:b0f9c5ac0305 | 20 | |
DD1993 | 0:b0f9c5ac0305 | 21 | BACK |
DD1993 | 0:b0f9c5ac0305 | 22 | |
DD1993 | 0:b0f9c5ac0305 | 23 | motor1 = rcgas + roll - pitch + yaw |
DD1993 | 0:b0f9c5ac0305 | 24 | motor2 = rcgas - roll + pitch + yaw |
DD1993 | 0:b0f9c5ac0305 | 25 | motor3 = rcgas − roll - pitch - yaw |
DD1993 | 0:b0f9c5ac0305 | 26 | motor4 = rcgas + roll + pitch - yaw |
DD1993 | 0:b0f9c5ac0305 | 27 | */ |
DD1993 | 0:b0f9c5ac0305 | 28 | |
DD1993 | 0:b0f9c5ac0305 | 29 | void motormixer::motor_all_period(int period) |
DD1993 | 0:b0f9c5ac0305 | 30 | { |
DD1993 | 0:b0f9c5ac0305 | 31 | motor1_out.period_ms(period); |
DD1993 | 0:b0f9c5ac0305 | 32 | motor2_out.period_ms(period); |
DD1993 | 0:b0f9c5ac0305 | 33 | motor3_out.period_ms(period); |
DD1993 | 0:b0f9c5ac0305 | 34 | motor4_out.period_ms(period); |
DD1993 | 0:b0f9c5ac0305 | 35 | } |
DD1993 | 0:b0f9c5ac0305 | 36 | |
DD1993 | 0:b0f9c5ac0305 | 37 | |
DD1993 | 0:b0f9c5ac0305 | 38 | void motormixer::motor_all_off() |
DD1993 | 0:b0f9c5ac0305 | 39 | { |
DD1993 | 0:b0f9c5ac0305 | 40 | motor1_out.pulsewidth_us(1100); |
DD1993 | 0:b0f9c5ac0305 | 41 | motor2_out.pulsewidth_us(1100); |
DD1993 | 0:b0f9c5ac0305 | 42 | motor3_out.pulsewidth_us(1100); |
DD1993 | 0:b0f9c5ac0305 | 43 | motor4_out.pulsewidth_us(1100); |
DD1993 | 0:b0f9c5ac0305 | 44 | } |
DD1993 | 0:b0f9c5ac0305 | 45 | |
DD1993 | 0:b0f9c5ac0305 | 46 | /////////////////////////MOTOR1////////////////////////////////////////////////////////////////////////////////////////// |
DD1993 | 0:b0f9c5ac0305 | 47 | void motormixer::motor1_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready) |
DD1993 | 0:b0f9c5ac0305 | 48 | { |
DD1993 | 0:b0f9c5ac0305 | 49 | |
DD1993 | 0:b0f9c5ac0305 | 50 | int motor1_pwm_ready = motor_all_pwm_ready + roll_error_pwm_ready - pitch_error_pwm_ready + yaw_error_pwm_ready; |
DD1993 | 0:b0f9c5ac0305 | 51 | |
DD1993 | 0:b0f9c5ac0305 | 52 | //Wert an Motor-Regler senden |
DD1993 | 0:b0f9c5ac0305 | 53 | if (motor1_pwm_ready < 1200) {motor1_out.pulsewidth_us(1200);} |
DD1993 | 0:b0f9c5ac0305 | 54 | else if (motor1_pwm_ready > 1759) {motor1_out.pulsewidth_us(1759);} |
DD1993 | 0:b0f9c5ac0305 | 55 | |
DD1993 | 0:b0f9c5ac0305 | 56 | else { |
DD1993 | 0:b0f9c5ac0305 | 57 | |
DD1993 | 0:b0f9c5ac0305 | 58 | motor1_out.pulsewidth_us(motor1_pwm_ready); |
DD1993 | 0:b0f9c5ac0305 | 59 | //pc_motor_mixer.printf("Output Motor1: %d\n", motor1_pwm_ready); |
DD1993 | 0:b0f9c5ac0305 | 60 | } |
DD1993 | 0:b0f9c5ac0305 | 61 | } |
DD1993 | 0:b0f9c5ac0305 | 62 | |
DD1993 | 0:b0f9c5ac0305 | 63 | /////////////////////////MOTOR2////////////////////////////////////////////////////////////////////////////////////////// |
DD1993 | 0:b0f9c5ac0305 | 64 | void motormixer::motor2_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready) |
DD1993 | 0:b0f9c5ac0305 | 65 | { |
DD1993 | 0:b0f9c5ac0305 | 66 | |
DD1993 | 0:b0f9c5ac0305 | 67 | int motor2_pwm_ready = motor_all_pwm_ready - roll_error_pwm_ready + pitch_error_pwm_ready + yaw_error_pwm_ready; |
DD1993 | 0:b0f9c5ac0305 | 68 | |
DD1993 | 0:b0f9c5ac0305 | 69 | //Wert an Motor-Regler senden |
DD1993 | 0:b0f9c5ac0305 | 70 | if (motor2_pwm_ready < 1200) {motor2_out.pulsewidth_us(1200);} |
DD1993 | 0:b0f9c5ac0305 | 71 | else if (motor2_pwm_ready > 1759) {motor2_out.pulsewidth_us(1759);} |
DD1993 | 0:b0f9c5ac0305 | 72 | |
DD1993 | 0:b0f9c5ac0305 | 73 | else { |
DD1993 | 0:b0f9c5ac0305 | 74 | |
DD1993 | 0:b0f9c5ac0305 | 75 | //pc_motor_mixer.printf("Output Motor2: %d\n", motor2_pwm_ready); |
DD1993 | 0:b0f9c5ac0305 | 76 | motor2_out.pulsewidth_us(motor2_pwm_ready); |
DD1993 | 0:b0f9c5ac0305 | 77 | } |
DD1993 | 0:b0f9c5ac0305 | 78 | } |
DD1993 | 0:b0f9c5ac0305 | 79 | |
DD1993 | 0:b0f9c5ac0305 | 80 | /////////////////////////MOTOR3////////////////////////////////////////////////////////////////////////////////////////// |
DD1993 | 0:b0f9c5ac0305 | 81 | void motormixer::motor3_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready) |
DD1993 | 0:b0f9c5ac0305 | 82 | { |
DD1993 | 0:b0f9c5ac0305 | 83 | |
DD1993 | 0:b0f9c5ac0305 | 84 | int motor3_pwm_ready = motor_all_pwm_ready - roll_error_pwm_ready - pitch_error_pwm_ready - yaw_error_pwm_ready; |
DD1993 | 0:b0f9c5ac0305 | 85 | |
DD1993 | 0:b0f9c5ac0305 | 86 | //Wert an Motor-Regler senden |
DD1993 | 0:b0f9c5ac0305 | 87 | if (motor3_pwm_ready < 1200) {motor3_out.pulsewidth_us(1200);} |
DD1993 | 0:b0f9c5ac0305 | 88 | else if (motor3_pwm_ready > 1759) {motor3_out.pulsewidth_us(1759);} |
DD1993 | 0:b0f9c5ac0305 | 89 | |
DD1993 | 0:b0f9c5ac0305 | 90 | else { |
DD1993 | 0:b0f9c5ac0305 | 91 | |
DD1993 | 0:b0f9c5ac0305 | 92 | motor3_out.pulsewidth_us(motor3_pwm_ready); |
DD1993 | 0:b0f9c5ac0305 | 93 | //pc_motor_mixer.printf("Output Motor3: %d\n", motor3_pwm_ready); |
DD1993 | 0:b0f9c5ac0305 | 94 | } |
DD1993 | 0:b0f9c5ac0305 | 95 | } |
DD1993 | 0:b0f9c5ac0305 | 96 | |
DD1993 | 0:b0f9c5ac0305 | 97 | /////////////////////////MOTOR4////////////////////////////////////////////////////////////////////////////////////////// |
DD1993 | 0:b0f9c5ac0305 | 98 | void motormixer::motor4_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready) |
DD1993 | 0:b0f9c5ac0305 | 99 | { |
DD1993 | 0:b0f9c5ac0305 | 100 | |
DD1993 | 0:b0f9c5ac0305 | 101 | int motor4_pwm_ready = motor_all_pwm_ready + roll_error_pwm_ready + pitch_error_pwm_ready - yaw_error_pwm_ready; |
DD1993 | 0:b0f9c5ac0305 | 102 | |
DD1993 | 0:b0f9c5ac0305 | 103 | //Wert an Motor-Regler senden |
DD1993 | 0:b0f9c5ac0305 | 104 | if (motor4_pwm_ready < 1200) {motor4_out.pulsewidth_us(1200);} |
DD1993 | 0:b0f9c5ac0305 | 105 | else if (motor4_pwm_ready > 1759) {motor4_out.pulsewidth_us(1759);} |
DD1993 | 0:b0f9c5ac0305 | 106 | |
DD1993 | 0:b0f9c5ac0305 | 107 | else { |
DD1993 | 0:b0f9c5ac0305 | 108 | |
DD1993 | 0:b0f9c5ac0305 | 109 | //pc_motor_mixer.printf("Output Motor4: %d\n", motor4_pwm_ready); |
DD1993 | 0:b0f9c5ac0305 | 110 | motor4_out.pulsewidth_us(motor4_pwm_ready); |
DD1993 | 0:b0f9c5ac0305 | 111 | } |
DD1993 | 0:b0f9c5ac0305 | 112 | } |
DD1993 | 0:b0f9c5ac0305 | 113 | |
DD1993 | 0:b0f9c5ac0305 | 114 | /////////////////////////MOTOREN KALIBRIEREN////////////////////////////////////////////////////////////////////////////////////////// |
DD1993 | 0:b0f9c5ac0305 | 115 | void motormixer::motor_calibration(int period) |
DD1993 | 0:b0f9c5ac0305 | 116 | { |
DD1993 | 0:b0f9c5ac0305 | 117 | pc_motor_mixer.printf("Periode einstellen\n"); |
DD1993 | 0:b0f9c5ac0305 | 118 | motor1_out.period_ms(period); |
DD1993 | 0:b0f9c5ac0305 | 119 | motor2_out.period_ms(period); |
DD1993 | 0:b0f9c5ac0305 | 120 | motor3_out.period_ms(period); |
DD1993 | 0:b0f9c5ac0305 | 121 | motor4_out.period_ms(period); |
DD1993 | 0:b0f9c5ac0305 | 122 | pc_motor_mixer.printf("Done\n"); |
DD1993 | 0:b0f9c5ac0305 | 123 | |
DD1993 | 0:b0f9c5ac0305 | 124 | pc_motor_mixer.printf("Vollast...3Sec warten...\n"); |
DD1993 | 0:b0f9c5ac0305 | 125 | motor1_out.pulsewidth_us(1759); |
DD1993 | 0:b0f9c5ac0305 | 126 | motor2_out.pulsewidth_us(1759); |
DD1993 | 0:b0f9c5ac0305 | 127 | motor3_out.pulsewidth_us(1759); |
DD1993 | 0:b0f9c5ac0305 | 128 | motor4_out.pulsewidth_us(1759); |
DD1993 | 0:b0f9c5ac0305 | 129 | |
DD1993 | 0:b0f9c5ac0305 | 130 | wait(5); |
DD1993 | 0:b0f9c5ac0305 | 131 | |
DD1993 | 0:b0f9c5ac0305 | 132 | pc_motor_mixer.printf("Keine Last...3Sec warten...\n"); |
DD1993 | 0:b0f9c5ac0305 | 133 | motor1_out.pulsewidth_us(1111); |
DD1993 | 0:b0f9c5ac0305 | 134 | motor2_out.pulsewidth_us(1111); |
DD1993 | 0:b0f9c5ac0305 | 135 | motor3_out.pulsewidth_us(1111); |
DD1993 | 0:b0f9c5ac0305 | 136 | motor4_out.pulsewidth_us(1111); |
DD1993 | 0:b0f9c5ac0305 | 137 | pc_motor_mixer.printf("Done\n"); |
DD1993 | 0:b0f9c5ac0305 | 138 | |
DD1993 | 0:b0f9c5ac0305 | 139 | wait(5); |
DD1993 | 0:b0f9c5ac0305 | 140 | } |