Diff: Mixer/Mixer.cpp

Revision:

6:3188b00263e8

Parent:

5:b9947e3d20cf

Child:

9:15058b4fa090

--- a/Mixer/Mixer.cpp	Thu Sep 27 13:51:54 2018 +0000
+++ b/Mixer/Mixer.cpp	Thu Sep 27 14:09:05 2018 +0000
@@ -2,8 +2,12 @@
 #include "Mixer.h"
 
 // Class constructor
-Mixer::Mixer() : motor_1(PA_1), motor_2(PB_11), motor_3(PA_15), motor_4(PB_9_ALT1)
+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)
 {
+    led_1_red = 1;
+    led_1_green = 1;
+    led_4_red = 1;
+    led_4_green = 1;
 }
 
 // Actuate motors with desired total trust force (N) and torques (N.m)
@@ -14,6 +18,20 @@
     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))
+    {
+        led_1_red = 1;
+        led_1_green = 0;
+        led_4_red = 1;
+        led_4_green = 0;
+    }
+    else
+    {
+        led_1_red = !led_1_red;
+        led_1_green = 1;
+        led_4_red = !led_4_red;
+        led_4_green = 1;
+    }
 }
 
 // Converts total trust force (N) and torques (N.m) to angular velocities (rad/s)