Dominik Diedrich / Mbed 2 deprecated drone_quado_x

code to fly a quadrocopter

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }