Merge branch 'master' into auto_attach_ui

executor/gamestate_filter.c

@@ -7,7 +7,7 @@
  */
 
 // Microseconds between each time we poll shared memory for the current game state
-#define GAMESTATE_POLL_INTERVAL (0.5 * 1000000)
+#define GAMESTATE_POLL_INTERVAL (0.5 * 1e6)
 // Duration of POISON_IVY and DEHYDRATION in milliseconds
 #define DEBUFF_DURATION 10000
 // How much to slow motor velocities when HYPOTHERMIA is ACTIVE
@@ -20,48 +20,61 @@
 #define IDX_VELOCITY_A 0
 #define IDX_VELOCITY_B 8
 
-typedef struct gamestate {
-    uint8_t hypothermia;
-    uint8_t poison_ivy;
-    uint8_t dehydration;
-} gamestate_t;
-
-// A thread function that polls shared memory for gamestate updates
-// Sets POISON_IVY and DEHYDRATION to INACTIVE after 10 seconds of ACTIVE
+/**
+ * A thread function that acts as a timer on gamestates and blocks forever.
+ * Sets POISON_IVY and DEHYDRATION to INACTIVE after DEBUFF_DURATION milliseconds of ACTIVE.
+ * Deactivates all game states when run mode is set to IDLE.
+ * Arguments:
+ *    Unused
+ * Returns:
+ *    NULL
+ */
 static void* gamestate_handler(void* args) {
-    gamestate_t* gamestate = (gamestate_t*) args;
     uint64_t poison_ivy_start = 0;   // The timestamp of when poison ivy started; 0 if inactive
     uint64_t dehydration_start = 0;  // The timestamp of when dehydration started; 0 if inactive
     uint64_t curr_time = 0;          // The current timestamp
 
     // Poll the current gamestate
     while (1) {
         curr_time = millis();
-        gamestate->hypothermia = (robot_desc_read(HYPOTHERMIA) == ACTIVE) ? 1 : 0;
 
-        if (!gamestate->poison_ivy && robot_desc_read(POISON_IVY) == ACTIVE) {  // Poison ivy started
-            gamestate->poison_ivy = 1;
-            poison_ivy_start = curr_time;
-        } else if (gamestate->poison_ivy && (curr_time - poison_ivy_start > DEBUFF_DURATION)) {  // End poison ivy
-            gamestate->poison_ivy = 0;
-            poison_ivy_start = 0;
+        if (robot_desc_read(RUN_MODE) == IDLE) {
+            robot_desc_write(HYPOTHERMIA, INACTIVE);
             robot_desc_write(POISON_IVY, INACTIVE);
-        }
-
-        if (!gamestate->dehydration && robot_desc_read(DEHYDRATION) == ACTIVE) {  // Dehydration started
-            gamestate->dehydration = 1;
-            dehydration_start = curr_time;
-        } else if (gamestate->dehydration && (curr_time - dehydration_start > DEBUFF_DURATION)) {  // End dehydration
-            gamestate->dehydration = 0;
-            dehydration_start = 0;
             robot_desc_write(DEHYDRATION, INACTIVE);
+            poison_ivy_start = 0;
+            dehydration_start = 0;
+        } else {  // Game state are in play; Deactivate when debuff duration passes
+            // Update poison ivy
+            if (!poison_ivy_start && robot_desc_read(POISON_IVY) == ACTIVE) {  // Poison ivy had just started since the last poll
+                poison_ivy_start = curr_time;
+            } else if (poison_ivy_start && (curr_time - poison_ivy_start > DEBUFF_DURATION)) {  // Poison ivy had just ended since the last poll
+                poison_ivy_start = 0;
+                robot_desc_write(POISON_IVY, INACTIVE);
+            }
+
+            // Update dehydration
+            if (!dehydration_start && robot_desc_read(DEHYDRATION) == ACTIVE) {  // Dehydration had just started since the last poll
+                dehydration_start = curr_time;
+            } else if (dehydration_start && (curr_time - dehydration_start > DEBUFF_DURATION)) {  // Dehydration had just started since the last poll
+                dehydration_start = 0;
+                robot_desc_write(DEHYDRATION, INACTIVE);
+            }
         }
+
         // Throttle the gamestate polling
         usleep(GAMESTATE_POLL_INTERVAL);
     }
     return NULL;
 }
 
+void start_gamestate_handler_thread() {
+    static pthread_t gamestate_handler_tid = 0;
+    if (gamestate_handler_tid == 0) {  // There should be only one thread monitoring game states.
+        pthread_create(&gamestate_handler_tid, NULL, gamestate_handler, NULL);
+    }
+}
+
 // Scales the KoalaBear's velocities by SCALAR
 // Assumes PARAMS belong to a KoalaBear
 static void scale_velocity(param_val_t* params, float scalar) {
@@ -87,27 +100,22 @@ static void bound_velocity(param_val_t* params) {
 }
 
 int filter_device_write_uid(uint8_t dev_type, uint64_t dev_uid, process_t process, stream_t stream, uint32_t params_to_write, param_val_t* params) {
-    static pthread_t gamestate_handler_tid = 0;
-    static gamestate_t gamestate = {0};
-    // Spawn thread if it doesn't already exist
-    if (gamestate_handler_tid == 0) {
-        pthread_create(&gamestate_handler_tid, NULL, gamestate_handler, (void*) &gamestate);
-    }
+    // Spring 2021: Only KoalaBear is affected by game states
     if (dev_type == KOALABEAR) {
         // Bound velocity to [-1.0, 1.0]
         bound_velocity(params);
 
         // Implement each gamestate, modifying params as necessary
-        if (gamestate.hypothermia) {
+        if (robot_desc_read(HYPOTHERMIA) == ACTIVE) {
             scale_velocity(params, SLOW_SCALAR);
         }
-        if (gamestate.poison_ivy) {
+        if (robot_desc_read(POISON_IVY) == ACTIVE) {
             scale_velocity(params, -1.0);
         }
-        if (gamestate.dehydration) {
+        if (robot_desc_read(DEHYDRATION) == ACTIVE) {
             scale_velocity(params, 0);
         }
     }
-    // Call shared memory wrapper function
+    // Call the actual shared memory wrapper function with the (possibly modified) values
     return device_write_uid(dev_uid, process, stream, params_to_write, params);
 }

executor/gamestate_filter.h

@@ -21,10 +21,24 @@
  *           will flip the sign of VALUE before being written to shared memory.
  */
 
+#include <stdbool.h>
 #include "../logger/logger.h"
 #include "../runtime_util/runtime_util.h"
 #include "../shm_wrapper/shm_wrapper.h"
 
+/**
+ * Starts the gamestate handler thread so that gamestates are deactivated
+ * after the respective debuff duration passes.
+ * All game states are deactivated also when run mode is set to IDLE
+ * If the game state handler thread is already active for the process calling this,
+ * this won't spawn a new thread (no-op).
+ */
+void start_gamestate_handler_thread();
+
+/**
+ * A wrapper function to device_write_uid that modifies the input params
+ * based on the current active game states.
+ */
 int filter_device_write_uid(uint8_t dev_type, uint64_t dev_uid, process_t process, stream_t stream, uint32_t params_to_write, param_val_t* params);
 
 #endif

executor/runtime.pxd

@@ -28,6 +28,7 @@ cdef extern from "../runtime_util/runtime_util.h":
         GAMEPAD, KEYBOARD, START_POS, RUN_MODE
     ctypedef enum robot_desc_val_t:
         CONNECTED, DISCONNECTED, LEFT, RIGHT, AUTO, TELEOP
+    uint8_t is_param_to_kill(uint8_t dev_type, char* param_name) nogil
     char** get_button_names() nogil
     char** get_joystick_names() nogil
     char** get_key_names() nogil

executor/studentapi.pyx

@@ -3,6 +3,7 @@
 # from libc.stdint cimport *
 from runtime cimport *
 from cpython.mem cimport PyMem_Malloc, PyMem_Free
+from libc.string cimport strcmp
 
 import threading
 import sys
@@ -92,18 +93,17 @@ cdef class Gamepad:
         cdef uint64_t buttons
         cdef float joysticks[4]
         cdef int err = input_read(&buttons, joysticks, GAMEPAD)
-        if err == -1:
-            raise DeviceError(f"Gamepad isn't connected to Dawn")
+        gamepad_connected = (err != -1)
         cdef char** button_names = get_button_names()
         cdef char** joystick_names = get_joystick_names()
         # Check if param is button
         for i in range(NUM_GAMEPAD_BUTTONS):
             if param == button_names[i]:
-                return bool(buttons & (1 << i))
+                return bool(buttons & (1 << i)) if gamepad_connected else False
         # Check if param is joystick
         for i in range(4):
             if param == joystick_names[i]:
-                return joysticks[i]
+                return joysticks[i] if gamepad_connected else 0.0
         raise KeyError(f"Invalid gamepad parameter {param_name}")
 
 
@@ -132,7 +132,7 @@ cdef class Keyboard:
         cdef float joysticks[4]
         cdef int err = input_read(&buttons, joysticks, KEYBOARD)
         if err == -1:
-            raise DeviceError(f"Keyboard isn't connected to Dawn")
+            return False
         cdef char** key_names = get_key_names()
         for i in range(NUM_KEYBOARD_BUTTONS):
             if param == key_names[i]:
@@ -337,6 +337,14 @@ cdef class Robot:
         if param_idx == -1:
             raise DeviceError(f"Invalid device parameter {param_name} for device type {device.name.decode('utf-8')}({device_type})")
 
+        # EDGE CASE: If it's TELEOP but no UserInput is connected, robot is emergency stopped
+        # There are certain parameters that need to remain 0
+        if (robot_desc_read(RUN_MODE) == TELEOP \
+            and robot_desc_read(GAMEPAD) == DISCONNECTED \
+            and robot_desc_read(KEYBOARD) == DISCONNECTED) \
+            and is_param_to_kill(device_type, param):
+            value = 0
+
         # Allocating memory for parameter to write
         cdef param_val_t* param_value = <param_val_t*> PyMem_Malloc(sizeof(param_val_t) * MAX_PARAMS)
         if not param_value:

net_handler/Makefile

@@ -17,5 +17,5 @@ clean:
 	rm -f net_handler
 
 # makes the actual net_handler executable
-net_handler: net_util.c net_handler.c message.c connection.c ../logger/logger.c ../shm_wrapper/shm_wrapper.c ../runtime_util/runtime_util.c 
+net_handler: net_util.c net_handler.c message.c connection.c ../logger/logger.c ../shm_wrapper/shm_wrapper.c ../runtime_util/runtime_util.c  ../executor/gamestate_filter.c
 	gcc $^ $(GEN_FILES) -o $@ $(LIBS) $(LDFLAGS)

net_handler/net_handler.c

@@ -1,3 +1,4 @@
+#include "../executor/gamestate_filter.h"
 #include "connection.h"
 #include "net_util.h"
 
@@ -117,6 +118,9 @@ int main() {
     }
     shm_init();
 
+    // TODO: Net Handler is in charge of regulating game states.
+    // Net Handler may not have this responsibility in the future.
+    start_gamestate_handler_thread();
     log_printf(INFO, "Net handler initialized");
 
     //run net_handler main control loop

runtime_util/runtime_util.c

@@ -272,6 +272,44 @@ int8_t get_param_idx(uint8_t dev_type, char* param_name) {
     return -1;
 }
 
+param_id_t* get_params_to_kill(uint8_t* num_devices_with_params_to_kill) {
+    // Change this array as necessary
+    /**
+     * Parameters that should set to (and remain) 0, 0.0, or False, depending on the type,
+     * when the Robot should be emergency stopped.
+     * Ex: Robot may be emergency stopped if no user input is connected during TELEOP.
+     */
+    param_id_t params_to_kill[] = {
+        {.device_type = KoalaBear.type,
+         .param_bitmap = (1 << get_param_idx(KoalaBear.type, "velocity_a") | (1 << get_param_idx(KoalaBear.type, "velocity_b")))},
+        {.device_type = SimpleTestDevice.type,
+         .param_bitmap = 1 << get_param_idx(SimpleTestDevice.type, "MY_INT")},
+    };
+    // This properly calculates the length of the array
+    *num_devices_with_params_to_kill = sizeof(params_to_kill) / sizeof(param_id_t);
+    // Allocate a copied array and return a pointer
+    param_id_t* params_to_kill_copy = malloc(sizeof(params_to_kill));
+    memcpy(params_to_kill_copy, params_to_kill, sizeof(params_to_kill));
+    return params_to_kill_copy;
+}
+
+uint8_t is_param_to_kill(uint8_t dev_type, char* param_name) {
+    uint8_t num_devices_with_params_to_kill = 0;
+    param_id_t* params_to_kill = get_params_to_kill(&num_devices_with_params_to_kill);
+    uint8_t ret = 0;
+    for (uint8_t i = 0; i < num_devices_with_params_to_kill; i++) {
+        if (dev_type == params_to_kill[i].device_type) {  // There's a match for a device with param to kill
+            uint32_t bit = 1 << get_param_idx(dev_type, param_name);
+            if (bit & params_to_kill[i].param_bitmap) {  // There's a match for the specific parameter
+                ret = 1;
+                break;
+            }
+        }
+    }
+    free(params_to_kill);
+    return ret;
+}
+
 char* BUTTON_NAMES[NUM_GAMEPAD_BUTTONS] = {
     "button_a", "button_b", "button_x", "button_y", "l_bumper", "r_bumper", "l_trigger", "r_trigger",
     "button_back", "button_start", "l_stick", "r_stick", "dpad_up", "dpad_down", "dpad_left", "dpad_right", "button_xbox"};
@@ -328,6 +366,12 @@ char* field_to_string(robot_desc_field_t field) {
             return "Shepherd";
         case START_POS:
             return "Start Pos";
+        case HYPOTHERMIA:
+            return "Hypothermia";
+        case POISON_IVY:
+            return "Poison Ivy";
+        case DEHYDRATION:
+            return "Dehydration";
         default:
             return NULL;
     }

runtime_util/runtime_util.h

@@ -121,6 +121,13 @@ typedef struct param_desc {
     uint8_t write;      // Whether or not the param is writable
 } param_desc_t;
 
+// A struct that details a bitmap of parameters that should be killed for a device
+// when the robot needs to be emergency stopped (ex: motor velocities)
+typedef struct param_id {
+    uint8_t device_type;    // The type of the device that should have params killed
+    uint32_t param_bitmap;  // Bitmap of parameters that should be killd
+} param_id_t;
+
 // A struct defining a kind of device (ex: LimitSwitch, KoalaBear)
 typedef struct device {
     uint8_t type;                     // The type of device
@@ -192,6 +199,23 @@ param_desc_t* get_param_desc(uint8_t dev_type, char* param_name);
  */
 int8_t get_param_idx(uint8_t dev_type, char* param_name);
 
+/**
+ * Returns an array with identifiers of params that should be
+ * killed (0, 0.0, or False) when the Robot needs to be emergency stopped.
+ * Arguments:
+ *    num_devices_with_params_to_kill: Will be populated with the length of the output array
+ * Returns:
+ *    a newly allocated array of param identifiers to kill.
+ *    *** It's the caller's responsibility to free this array
+ */
+param_id_t* get_params_to_kill(uint8_t* num_devices_with_params_to_kill);
+
+/**
+ * Returns 1 iff the specified parameter should be killed
+ * during an emergency stop. Otherwise, 0
+ */
+uint8_t is_param_to_kill(uint8_t dev_type, char* param_name);
+
 /**
  * Returns an array of button names.
  */