Dominik Diedrich / Mbed 2 deprecated drone_quado_x

code to fly a quadrocopter

Dependencies:   mbed

PWM/degpwm.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 "degpwm.h"
DD1993 0:b0f9c5ac0305 2
DD1993 0:b0f9c5ac0305 3 RawSerial pc_degpwm(USBTX, USBRX);
DD1993 0:b0f9c5ac0305 4
DD1993 0:b0f9c5ac0305 5
DD1993 0:b0f9c5ac0305 6 //////////////////////PITCH FORWARD_DEG_TO_PWM//////////////////////////////////////////////////////////////
DD1993 0:b0f9c5ac0305 7 int Degpwm::pitch_pwm(float pitch_error)
DD1993 0:b0f9c5ac0305 8 {
DD1993 0:b0f9c5ac0305 9 float pitch_e = pitch_error;
DD1993 0:b0f9c5ac0305 10 float pitch_error_pwm = 0.0;
DD1993 0:b0f9c5ac0305 11 int pitch_error_pwm_ready = 0;
DD1993 0:b0f9c5ac0305 12
DD1993 0:b0f9c5ac0305 13 //Umrechnung von Grad zu PWM Wert
DD1993 0:b0f9c5ac0305 14 if (pitch_e < -50 || pitch_e > 50){}
DD1993 0:b0f9c5ac0305 15
DD1993 0:b0f9c5ac0305 16 else{
DD1993 0:b0f9c5ac0305 17
DD1993 0:b0f9c5ac0305 18 pitch_error_pwm = pitch_e *400/50;
DD1993 0:b0f9c5ac0305 19 //pc_degpwm.printf("Output Pitch Errorpwm: %f\n", pitch_e);
DD1993 0:b0f9c5ac0305 20 pitch_error_pwm_ready = pitch_error_pwm;
DD1993 0:b0f9c5ac0305 21 //pc_degpwm.printf("Output Pitch Error: %f| PWM: %d\n", pitch_error, pitch_error_pwm_ready);
DD1993 0:b0f9c5ac0305 22 }
DD1993 0:b0f9c5ac0305 23
DD1993 0:b0f9c5ac0305 24 return pitch_error_pwm_ready; // Rücgabe des Korrekturwerts in PWM
DD1993 0:b0f9c5ac0305 25 }
DD1993 0:b0f9c5ac0305 26
DD1993 0:b0f9c5ac0305 27 //////////////////////ROLL_DEG_TO_PWM//////////////////////////////////////////////////////////////
DD1993 0:b0f9c5ac0305 28 int Degpwm::roll_pwm(float roll_error)
DD1993 0:b0f9c5ac0305 29 {
DD1993 0:b0f9c5ac0305 30 float pitch_e = roll_error;
DD1993 0:b0f9c5ac0305 31 float roll_error_pwm = 0.0;
DD1993 0:b0f9c5ac0305 32 int roll_error_pwm_ready = 0;
DD1993 0:b0f9c5ac0305 33
DD1993 0:b0f9c5ac0305 34 //Umrechnung von Grad zu PWM Wert
DD1993 0:b0f9c5ac0305 35 if (pitch_e < -50 || pitch_e > 50){}
DD1993 0:b0f9c5ac0305 36
DD1993 0:b0f9c5ac0305 37 else{
DD1993 0:b0f9c5ac0305 38
DD1993 0:b0f9c5ac0305 39 roll_error_pwm = pitch_e *400/50;
DD1993 0:b0f9c5ac0305 40 //pc_degpwm.printf("Output Pitch Errorpwm: %f\n", pitch_e);
DD1993 0:b0f9c5ac0305 41 roll_error_pwm_ready = roll_error_pwm;
DD1993 0:b0f9c5ac0305 42 //pc_degpwm.printf("Output Pitch Error: %f| PWM: %d\n", pitch_error, pitch_error_pwm_ready);
DD1993 0:b0f9c5ac0305 43 }
DD1993 0:b0f9c5ac0305 44
DD1993 0:b0f9c5ac0305 45 return roll_error_pwm_ready; // Rücgabe des Korrekturwerts in PWM
DD1993 0:b0f9c5ac0305 46 }
DD1993 0:b0f9c5ac0305 47
DD1993 0:b0f9c5ac0305 48 //////////////////////YAW_ACCEL_TO_PWM//////////////////////////////////////////////////////////////
DD1993 0:b0f9c5ac0305 49 int Degpwm::yaw_pwm(float yaw_error)
DD1993 0:b0f9c5ac0305 50 {
DD1993 0:b0f9c5ac0305 51 float yaw_e = yaw_error;
DD1993 0:b0f9c5ac0305 52 float yaw_error_pwm = 0;
DD1993 0:b0f9c5ac0305 53 int yaw_error_pwm_ready = 0;
DD1993 0:b0f9c5ac0305 54
DD1993 0:b0f9c5ac0305 55 //Umrechnung von Grad zu PWM Wert
DD1993 0:b0f9c5ac0305 56 if (yaw_e < -1200) {
DD1993 0:b0f9c5ac0305 57 yaw_error_pwm = -1200 *370/170;
DD1993 0:b0f9c5ac0305 58 yaw_error_pwm_ready = yaw_error_pwm;
DD1993 0:b0f9c5ac0305 59 }
DD1993 0:b0f9c5ac0305 60
DD1993 0:b0f9c5ac0305 61 else if (yaw_e > 1200) {
DD1993 0:b0f9c5ac0305 62 yaw_error_pwm = 1200 *370/170;
DD1993 0:b0f9c5ac0305 63 yaw_error_pwm_ready = yaw_error_pwm;
DD1993 0:b0f9c5ac0305 64 }
DD1993 0:b0f9c5ac0305 65
DD1993 0:b0f9c5ac0305 66 else{
DD1993 0:b0f9c5ac0305 67 yaw_error_pwm = yaw_e *370/170;
DD1993 0:b0f9c5ac0305 68 yaw_error_pwm_ready = yaw_error_pwm;
DD1993 0:b0f9c5ac0305 69
DD1993 0:b0f9c5ac0305 70 //pc_degpwm.printf("Output yaw Error: %f| PWM: %d\n", yaw_error, yaw_error_pwm_ready);
DD1993 0:b0f9c5ac0305 71 }
DD1993 0:b0f9c5ac0305 72
DD1993 0:b0f9c5ac0305 73 return yaw_error_pwm_ready; // Rückgabe des Korrekturwerts in PWM
DD1993 0:b0f9c5ac0305 74
DD1993 0:b0f9c5ac0305 75 }
DD1993 0:b0f9c5ac0305 76