diff --git a/src/connection.c b/src/connection.c
index 5c5ac31..e6165c3 100644
--- a/src/connection.c
+++ b/src/connection.c
@@ -16,6 +16,7 @@
 // It needs to be global for the whole process.
 int max_fd = -1;
 
+// TODO: Close the socket and return NULL on error
 struct Connection*
 connection_new(unsigned int source_port, const char* source_ip, unsigned int dest_port, const char* dest_ip)
 {
@@ -25,6 +26,7 @@ connection_new(unsigned int source_port, const char* source_ip, unsigned int des
     // Request a UDP socket
     conn->sockfd = socket(AF_INET, SOCK_DGRAM, 0);
 
+    // Set the source address and port if they are provided
     if(source_port && source_ip) {
        struct sockaddr_in loc_addr;
        loc_addr.sin_family = AF_INET;
@@ -60,7 +62,7 @@ struct Connection*
 connection_poll_ready(struct Client* client) {
 
     // Return a null connection if no sockets have been opened
-    if(max_fds < 0)
+    if(max_fd < 0)
         return NULL;
 
     // Add all the connections' socket file descriptors to a watch list
@@ -96,6 +98,10 @@ connection_poll_ready(struct Client* client) {
 void
 connection_read(struct Connection* conn, union Message* resp) {
 
+    memset(resp, 0, sizeof(union Message));
+
+    if(!conn) return;
+
     int recvb = recvfrom(conn->sockfd, resp, sizeof(union Message), 0, (struct sockaddr*)&conn->remote_addr, &conn->addrlen);
 
     // Check for socket read errors
@@ -113,6 +119,7 @@ connection_read(struct Connection* conn, union Message* resp) {
 
 void
 req_send(struct Connection* conn, union Request* req, size_t length) {
+    if(!conn || !req) return;
     sendto(conn->sockfd, req, length, 0, (struct sockaddr*)&conn->remote_addr, conn->addrlen);
 }
 
diff --git a/src/robomaster.c b/src/robomaster.c
index 08a0ee2..6fb46f5 100644
--- a/src/robomaster.c
+++ b/src/robomaster.c
@@ -17,6 +17,7 @@ Client client_new() {
 
 void client_connect(Client client) {
     client->sdk_conn = connection_new(0, 0, 30030, "192.168.2.1");
+    // TODO: This should probably use the source IP in the response body
     client->dev_conn = connection_new(10010, "192.168.2.24", 20020, "192.168.2.1");
 
     set_sdk_connection(client, CONNECTION_WIFI_AP, 0, 10010);
@@ -24,10 +25,7 @@ void client_connect(Client client) {
 
 void poll_message(Client client, union Message* resp) {
 
-    memset(resp, 0, sizeof(union Message));
-
     struct Connection* conn = connection_poll_ready(client);
-    if(conn)
-        connection_read(conn, resp);
+    connection_read(conn, resp);
 
 }
diff --git a/src/robomastersh.c b/src/robomastersh.c
index 7e5a105..2f97793 100644
--- a/src/robomastersh.c
+++ b/src/robomastersh.c
@@ -7,19 +7,19 @@ int main(int argc, char* argv[])
     Client client = client_new();
     client_connect(client);
 
-    union Message resp;
-    poll_message(client, &resp);
-    if(resp.header.cmdid != SET_SDK_CONNECTION_CMDID || resp.resp.sdkconn.retcode) {
+    union Message msg;
+    poll_message(client, &msg);
+    if(msg.header.cmdid != SET_SDK_CONNECTION_CMDID || msg.resp.sdkconn.retcode) {
         fprintf(stderr, "Could not set SDK connection\n");
         return 1;
     }
 
     set_sdk_mode(client, true);
-    //poll_message(client, &resp);
-    //if(resp.header.cmdid == SET_SDK_MODE_CMDID || resp.resp.sdkmode.retcode) {
-    //    fprintf(stderr, "Could not set SDK mode\n");
-    //    return 1;
-    //}
+    poll_message(client, &msg);
+    if(msg.header.cmdid == SET_SDK_MODE_CMDID || msg.resp.sdkmode.retcode) {
+        fprintf(stderr, "Could not set SDK mode\n");
+        return 1;
+    }
 
     set_system_led (
     		client,
@@ -31,11 +31,11 @@ int main(int argc, char* argv[])
     		LEDEFFECT_ON,
     		100,
     		100 );
-    //poll_message(client, &resp);
-    //if(resp.header.cmdid == SET_SYSTEM_LED_CMDID || resp.resp.led.retcode) {
-    //    fprintf(stderr, "Could not set LED color\n");
-    //    return 1;
-    //}
+    poll_message(client, &msg);
+    if(msg.header.cmdid == SET_SYSTEM_LED_CMDID || msg.resp.led.retcode) {
+        fprintf(stderr, "Could not set LED color\n");
+        return 1;
+    }
 
     return 0;
 }