Rodrigo Gikas / CrazyflieController

Crazyflie 2.0 Controller

Dependents:   Drone_Controlador_Atitude

Mixer/Mixer.cpp@0:332926683ee3, 2018-10-08 (annotated)

Committer:
IgneousGuikas
Date:
Mon Oct 08 11:07:14 2018 +0000
Revision:
0:332926683ee3
Child:
1:2abfa02e99d5
First version

Who changed what in which revision?

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