Revision 24:7b9e3beb61d5, committed 2018-12-06

Comitter:

fbob

Date:

Thu Dec 06 16:44:40 2018 +0000

Parent:

23:4905fbc2b31a

Commit message:

Adjust

Changed in this revision

--- a/Mixer/Mixer.cpp	Wed Oct 31 13:21:09 2018 +0000
+++ b/Mixer/Mixer.cpp	Thu Dec 06 16:44:40 2018 +0000
@@ -4,6 +4,56 @@
 // Class constructor
 Mixer::Mixer() : motor_1(PA_1), motor_2(PB_11), motor_3(PA_15), motor_4(PB_9_ALT1), led_1_red(PC_3), led_1_green(PC_2), led_4_red(PC_0), led_4_green(PC_1)
 {
+    armed = false;
+    led_1_red = 1;
+    led_1_green = 0;
+    led_4_red = 1;
+    led_4_green = 0;
+    motor_1.period(period_pwm);
+    motor_2.period(period_pwm);
+    motor_3.period(period_pwm);
+    motor_4.period(period_pwm);
+    motor_1 = 0.0f;
+    motor_2 = 0.0f;
+    motor_3 = 0.0f;
+    motor_4 = 0.0f;
+}
+
+void Mixer::arm()
+{
+    led_1_red = 0;
+    led_1_green = 1;
+    led_4_red = 0;
+    led_4_green = 1;
+    motor_1.period(1.0/440.0);
+    motor_1 = 0.2f;
+    wait(motor_test_on_time);
+    motor_1.period(period_pwm);
+    motor_1 = 0.0f;
+    wait(motor_test_off_time);
+    motor_2.period(1.0/880.0);
+    motor_2 = 0.2f;
+    wait(motor_test_on_time);
+    motor_2.period(period_pwm);
+    motor_2 = 0.0f;
+    wait(motor_test_off_time);
+    motor_3.period(1.0/660.0);
+    motor_3 = 0.2f;
+    wait(motor_test_on_time);
+    motor_3.period(period_pwm);
+    motor_3 = 0.0f;
+    wait(motor_test_off_time);
+    motor_4.period(1.0/550.0);
+    motor_4 = 0.2f;
+    wait(motor_test_on_time);
+    motor_4.period(period_pwm);
+    motor_4 = 0.0f;
+    wait(motor_test_off_time);
+    armed = true;
+}
+
+void Mixer::disarm()
+{
     led_1_red = 1;
     led_1_green = 0;
     led_4_red = 1;
@@ -12,29 +62,19 @@
     motor_2 = 0.0f;
     motor_3 = 0.0f;
     motor_4 = 0.0f;
+    armed = false;
 }
 
 // Actuate motors with desired total trust force (N) and torques (N.m)
 void Mixer::actuate(float f_t, float tau_phi, float tau_theta, float tau_psi)
 {
-    force_and_torques_to_angular_velocities(f_t,tau_phi,tau_theta,tau_psi);
-    motor_1 = angular_velocity_to_pwm(omega_1);
-    motor_2 = angular_velocity_to_pwm(omega_2);
-    motor_3 = angular_velocity_to_pwm(omega_3);
-    motor_4 = angular_velocity_to_pwm(omega_4);
-    if ((omega_1 == 0) & (omega_2 == 0) & (omega_3 == 0) & (omega_4 == 0))
+    if(armed)
     {
-        led_1_red = 1;
-        led_1_green = 0;
-        led_4_red = 1;
-        led_4_green = 0;
-    }
-    else
-    {
-        led_1_red = 0;
-        led_1_green = 1;
-        led_4_red = 0;
-        led_4_green = 1;
+        force_and_torques_to_angular_velocities(f_t,tau_phi,tau_theta,tau_psi);
+        motor_1 = angular_velocity_to_pwm(omega_1);
+        motor_2 = angular_velocity_to_pwm(omega_2);
+        motor_3 = angular_velocity_to_pwm(omega_3);
+        motor_4 = angular_velocity_to_pwm(omega_4);
     }
 }
 

--- a/Mixer/Mixer.h	Wed Oct 31 13:21:09 2018 +0000
+++ b/Mixer/Mixer.h	Thu Dec 06 16:44:40 2018 +0000
@@ -12,11 +12,17 @@
     Mixer();
     // Actuate motors with desired total trust force (N) and torques (N.m)
     void actuate(float f_t, float tau_phi, float tau_theta, float tau_psi);
+    
+    void arm();
+    
+    void disarm();
   private:
     // Motors PWM outputs
     PwmOut motor_1, motor_2, motor_3, motor_4;
     // LED digital outputs
     DigitalOut led_1_red, led_1_green, led_4_red, led_4_green;
+    //
+    bool armed;
     // Angular velocities (rad/s)
     float omega_1, omega_2, omega_3, omega_4;
     // Converts total trust force (N) and torques (N.m) to angular velocities (rad/s)

--- a/Parameters/Parameters.h	Wed Oct 31 13:21:09 2018 +0000
+++ b/Parameters/Parameters.h	Thu Dec 06 16:44:40 2018 +0000
@@ -1,6 +1,13 @@
 #ifndef Parameters_h
 #define Parameters_h
 
+// PWM frequencies
+const float f_pwm = 500.0f;
+const float period_pwm = 1.0f/f_pwm;
+
+const float motor_test_on_time = 0.05f;
+const float motor_test_off_time = 0.15f;
+
 // Interrupt frequencies
 const float f = 500.0f;             // Hz
 const float f_range = 20.0f;        // Hz