Rodrigo Gikas / CrazyflieController

Crazyflie 2.0 Controller

Dependents:   Drone_Controlador_Atitude

Mixer/Mixer.cpp@2:9e07bed8e8ed, 2018-11-21 (annotated)

Committer:
IgneousGuikas
Date:
Wed Nov 21 10:46:29 2018 +0000
Revision:
2:9e07bed8e8ed
Parent:
1:2abfa02e99d5 
Teste;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IgneousGuikas 0:332926683ee3 1 #include "mbed.h"
IgneousGuikas 1:2abfa02e99d5 2 #include "Parameters.h"
IgneousGuikas 0:332926683ee3 3 #include "Mixer.h"
IgneousGuikas 0:332926683ee3 4
IgneousGuikas 0:332926683ee3 5 // Class constructor
IgneousGuikas 0:332926683ee3 6 Mixer::Mixer() : motor0(PA_1), motor1(PB_11), motor2(PA_15), motor3(PB_9_ALT1)
IgneousGuikas 0:332926683ee3 7 {
IgneousGuikas 0:332926683ee3 8 }
IgneousGuikas 0:332926683ee3 9
IgneousGuikas 0:332926683ee3 10 // Actuate motors with the desired force (N) and torques (N.m)
IgneousGuikas 0:332926683ee3 11 void Mixer::actuate( float ft , float tau_phi , float tau_theta , float tau_psi )
IgneousGuikas 0:332926683ee3 12 {
IgneousGuikas 0:332926683ee3 13 ft_to_omega(ft,tau_phi,tau_theta,tau_psi);
IgneousGuikas 0:332926683ee3 14 motor0 = omega_to_pwm(omega0);
IgneousGuikas 0:332926683ee3 15 motor1 = omega_to_pwm(omega1);
IgneousGuikas 0:332926683ee3 16 motor2 = omega_to_pwm(omega2);
IgneousGuikas 0:332926683ee3 17 motor3 = omega_to_pwm(omega3);
IgneousGuikas 0:332926683ee3 18 }
IgneousGuikas 0:332926683ee3 19
IgneousGuikas 0:332926683ee3 20 // Converts desired force (N) and torques (N.m) to angular velocities ( rad /s)
IgneousGuikas 0:332926683ee3 21 void Mixer::ft_to_omega( float ft , float tau_phi , float tau_theta , float tau_psi )
IgneousGuikas 0:332926683ee3 22 {
IgneousGuikas 2:9e07bed8e8ed 23 float a1 = (1.0f/(4.0f*kl))*ft;
IgneousGuikas 2:9e07bed8e8ed 24 float a2 = (1.0f/(4.0f*kl*l))*tau_phi;
IgneousGuikas 2:9e07bed8e8ed 25 float a3 = (1.0f/(4.0f*kl*l))*tau_theta;
IgneousGuikas 2:9e07bed8e8ed 26 float a4 = (1.0f/(4.0f*kd))*tau_psi;
IgneousGuikas 0:332926683ee3 27
IgneousGuikas 2:9e07bed8e8ed 28 float omega02 = a1 - a2 - a3 - a4;
IgneousGuikas 2:9e07bed8e8ed 29 float omega12 = a1 - a2 + a3 + a4;
IgneousGuikas 2:9e07bed8e8ed 30 float omega22 = a1 + a2 + a3 - a4;
IgneousGuikas 2:9e07bed8e8ed 31 float omega32 = a1 + a2 - a3 + a4;
IgneousGuikas 2:9e07bed8e8ed 32
IgneousGuikas 2:9e07bed8e8ed 33 if (omega02 < 0.0f){
IgneousGuikas 2:9e07bed8e8ed 34 omega02 = 0.0f;
IgneousGuikas 0:332926683ee3 35 }
IgneousGuikas 2:9e07bed8e8ed 36 if (omega12 < 0.0f){
IgneousGuikas 2:9e07bed8e8ed 37 omega12 = 0.0f;
IgneousGuikas 0:332926683ee3 38 }
IgneousGuikas 2:9e07bed8e8ed 39 if (omega22 < 0.0f){
IgneousGuikas 2:9e07bed8e8ed 40 omega22 = 0.0f;
IgneousGuikas 0:332926683ee3 41 }
IgneousGuikas 2:9e07bed8e8ed 42 if (omega32 < 0.0f){
IgneousGuikas 2:9e07bed8e8ed 43 omega32 = 0.0f;
IgneousGuikas 0:332926683ee3 44 }
IgneousGuikas 0:332926683ee3 45
IgneousGuikas 0:332926683ee3 46 omega0 = sqrt(omega02);
IgneousGuikas 0:332926683ee3 47 omega1 = sqrt(omega12);
IgneousGuikas 0:332926683ee3 48 omega2 = sqrt(omega22);
IgneousGuikas 0:332926683ee3 49 omega3 = sqrt(omega32);
IgneousGuikas 0:332926683ee3 50 }
IgneousGuikas 0:332926683ee3 51
IgneousGuikas 0:332926683ee3 52 // Converts desired angular velocity (rad/s) to PWM signal
IgneousGuikas 0:332926683ee3 53 float Mixer::omega_to_pwm( float omega )
IgneousGuikas 0:332926683ee3 54 {
IgneousGuikas 0:332926683ee3 55 float pwm = alpha*pow(omega,2) + beta*omega;
IgneousGuikas 0:332926683ee3 56 return pwm;
IgneousGuikas 0:332926683ee3 57 }