Remove pause for enabling image recognition

The low level API needed to change to accomodate the hight level API,
which is a major revision.

CMakeLists.txt

@@ -15,8 +15,12 @@ add_library(robomaster
     src/message.c
     src/modules/sdk.c
     src/modules/chassis.c
+    src/modules/gimbal.c
+    src/modules/blaster.c
+    src/modules/camera.c
     src/connection.c
     src/robomaster.c
+    src/robo.c
 )
 
 target_include_directories(robomaster
@@ -26,14 +30,19 @@ target_include_directories(robomaster
     include
 )
 
-find_package(SDL2)
+include(FetchContent)
+FetchContent_Declare (
+    SDL2
+    GIT_REPOSITORY https://github.com/libsdl-org/SDL
+    GIT_TAG release-2.0.20
+    GIT_SHALLOW TRUE
+)
+FetchContent_MakeAvailable(SDL2)
 
-if(SDL2_FOUND)
-    add_executable(robomasterapp
+add_executable(robomasterapp
     src/sdl.c
-    )
-    target_link_libraries(robomasterapp
+)
+target_link_libraries(robomasterapp
     robomaster
-	SDL2::SDL2-static
-    )
-endif()
+    SDL2-static
+)

include/blaster.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include "message.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+static const uint8_t BLASTER_HOST = 23;
+static const uint8_t BLASTER_INDEX = 0;
+
+#define BLASTER_FIRE_CMD 0x513F
+
+enum FIRETYPE {
+    FIRETYPE_WATER = 0,
+    FIRETYPE_INFRARED = 1
+};
+
+enum FIRETIMES {
+    ONE = 1,
+    TWO = 2,
+    THREE = 3,
+    FOUR = 4,
+    FIVE = 5
+};
+
+void
+blaster_fire (
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		enum FIRETYPE type,
+		enum FIRETIMES times );
+

include/camera.h

@@ -0,0 +1,65 @@
+#pragma once
+
+#include "message.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+static const uint8_t STREAM_HOST = 1;
+static const uint8_t STREAM_INDEX = 0;
+
+#define STREAM_CTRL_CMD 0xD23F
+
+#define VIDEO_STREAM_PORT 40921
+
+enum STREAMSTATE {
+    STREAMSTATE_OFF = 0,
+    STREAMSTATE_ON = 1
+};
+
+enum STREAMRESOLUTION {
+    RES_720P = 0,
+    RES_360P = 1,
+    RES_540P = 2
+};
+
+enum STREAMCTRL {
+    STREAMCTRL_SDK = 1,
+    STREAMCTRL_VIDEO = 2,
+    STREAMCTRL_AUDIO = 3
+};
+
+void
+stream_ctrl (
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		enum STREAMSTATE state,
+		enum STREAMRESOLUTION res,
+		enum STREAMCTRL ctrl );
+
+static const uint8_t VISION_HOST = 17;
+static const uint8_t VISION_INDEX = 7;
+
+#define VISION_DETECT_ENABLE_CMD 0xA30A
+
+#define VISION_DETECT_ENABLE_PERSON 2
+#define VISION_DETECT_ENABLE_GESTURE 4
+#define VISION_DETECT_ENABLE_LINE 16
+#define VISION_DETECT_ENABLE_MARKER 32
+#define VISION_DETECT_ENABLE_ROBOT 128
+void
+vision_enable (
+		union Request* req,
+        uint16_t seq,
+        bool ack );
+
+#define VISION_DETECT_INFO_CMD 0xA40A
+enum VISIONTYPE {
+    VISIONTYPE_SHOULDER,
+    VISIONTYPE_PERSON,
+    VISIONTYPE_GESTURE,
+    VISIONTYPE_LINE,
+    VISIONTYPE_MARKER,
+    VISIONTYPE_ROBOT
+};

include/gimbal.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include "message.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+
+static const uint8_t GIMBAL_HOST = 4;
+static const uint8_t GIMBAL_INDEX = 0;
+
+#define GIMBAL_CTRL_SPEED_CMD 0x0C04
+
+void
+gimbal_ctrl_speed (
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		int16_t p,
+		int16_t y,
+		int16_t r );

include/message.h

@@ -168,6 +168,30 @@ struct PACKED SetChassisWheelSpeedResp
     struct Footer footer;
 };
 
+struct PACKED GimbalCtrlSpeedReq
+{
+    struct Header header;
+    // Values between -360 and 360
+    union {
+        int16_t yrp[3];
+        struct {
+            int16_t yaw;
+            int16_t roll;
+            int16_t pitch;
+        };
+    };
+    // Always 0xDC
+    uint8_t ctrl;
+    struct Footer footer;
+};
+
+struct PACKED GimbalCtrlSpeedResp
+{
+    struct Header header;
+    int8_t retcode;
+    struct Footer footer;
+};
+
 struct PACKED SetWheelSpeedReq
 {
     struct Header header;
@@ -224,6 +248,87 @@ struct PACKED SetSdkConnectionResp {
     struct Footer footer;
 };
 
+struct PACKED BlasterFireReq {
+    struct Header header;
+    struct {
+        uint8_t type : 4;
+        uint8_t times : 4;
+    };
+    struct Footer footer;
+};
+
+struct PACKED BlasterFireResp {
+    struct Header header;
+    uint8_t retcode;
+    struct Footer footer;
+};
+
+struct PACKED StreamCtrlReq {
+    struct Header header;
+    uint8_t ctrl;
+    struct {
+        uint8_t state : 4;
+        uint8_t conn_type : 4;
+    };
+    uint8_t resolution;
+    struct Footer footer;
+};
+
+struct PACKED StreamCtrlResp {
+    struct Header header;
+    uint8_t retcode;
+    struct Footer footer;
+};
+
+struct PACKED VisionRect {
+    union {
+        struct {
+            float x;
+            float y;
+            float w;
+            float h;
+            uint32_t info;
+        } rect;
+        struct {
+            float x;
+            float y;
+            float theta;
+            float c;
+            uint32_t info;
+        } line;
+        struct {
+            float x;
+            float y;
+            float w;
+            float h;
+            uint16_t info;
+            uint16_t distance;
+        } marker;
+    };
+};
+
+struct PACKED VisionDetectInfo {
+    struct Header header;
+    uint8_t type;
+    uint8_t status;
+    uint16_t errcode;
+    uint8_t count;
+    struct VisionRect rects[];
+};
+
+struct PACKED VisionDetectEnableReq {
+    struct Header header;
+    uint16_t type;
+    struct Footer footer;
+};
+
+struct PACKED VisionDetectEnableResp {
+    struct Header header;
+    uint8_t retcode;
+    uint16_t error;
+    struct Footer footer;
+};
+
 union Request {
     struct Header header;
     struct SetSdkConnectionReq sdkconn;
@@ -236,6 +341,10 @@ union Request {
     struct SetRobotModeReq mvmode;
     struct SubNodeResetReq subnodereset;
     struct SubscribeAddNodeReq subnodeadd;
+    struct GimbalCtrlSpeedReq gimbspeed;
+    struct BlasterFireReq blaster;
+    struct StreamCtrlReq stream;
+    struct VisionDetectEnableReq enablevision;
 };
 union Response {
     struct Header header;
@@ -249,11 +358,19 @@ union Response {
     struct SetRobotModeResp mvmode;
     struct SubNodeResetResp subnodereset;
     struct SubscribeAddNodeResp subnodeadd;
+    struct GimbalCtrlSpeedResp gimbspeed;
+    struct BlasterFireResp blaster;
+    struct StreamCtrlResp stream;
+    struct VisionDetectEnableResp enablevision;
+};
+union Push {
+    struct VisionDetectInfo vision;
 };
 union Message {
     struct Header header;
     union Request req;
     union Response resp;
+    union Push push;
 };
 
 enum MESSAGEERR {

include/roboeasy.h

@@ -1,8 +1,27 @@
 #pragma once
 
+#include "message.h"
+
+#include <stdbool.h>
+
 // Handle for the high level robot interface
-struct Robot;
-typedef struct Robot* Robot;
+struct RobotImp;
+typedef struct RobotImp* Robot;
+
+#define FRAGMENT_MESSAGE 1
+#define FRAGMENT_RECTANGLE 2
+#define FRAGMENT_LINE 3
+#define FRAGMENT_MARKER 4
+
+struct Fragment {
+    int type;
+    union {
+        union Message message;
+        struct VisionRect rect;
+    };
+};
+
+int robot_poll(Robot robot, struct Fragment* fragment);
 
 /*
  * Return a handle to the high level robot interface, or NULL on error.
@@ -29,6 +48,17 @@ int robot_init(Robot robot);
  */
 int robot_stop(Robot robot);
 
+/*
+ * Set the gimble speed. A packet will be sent to the robot on
+ * the next ready tick of the work function.
+ *
+ * robot: The robot to set the velocity of
+ * p: pitch speed
+ * y: yaw speed
+ * returns: 0 on success, non-zero on failure
+ */
+int robot_aim(Robot, float p, float y);
+
 /*
  * Set the velocity of the robot chassis. A packet will be sent to the robot on
  * the next ready tick of the work function.
@@ -54,6 +84,10 @@ int robot_drive(Robot, float x, float y, float r);
  */
 int robot_led(Robot, unsigned char r, unsigned char g, unsigned char b);
 
+int robot_blast(Robot);
+
+int robot_stream(Robot, bool);
+
 /*
  * If the robot is in ready mode, this makes the robot send a heartbeat in
  * the next tick of the work function. The work function will not do anything

include/robomaster.h

@@ -7,6 +7,9 @@ typedef struct Client* Client;
 #include "message.h"
 #include "chassis.h"
 #include "sdk.h"
+#include "gimbal.h"
+#include "blaster.h"
+#include "camera.h"
 
 Client client_new();
 void client_connect(Client client);

src/connection.c

@@ -35,6 +35,14 @@ message_length(int cmd) {
             return sizeof(struct SetWheelSpeedReq);
         case CHASSIS_SPEED_MODE_CMD:
             return sizeof(struct ChassisSpeedModeReq);
+        case GIMBAL_CTRL_SPEED_CMD:
+            return sizeof(struct GimbalCtrlSpeedReq);
+        case BLASTER_FIRE_CMD:
+            return sizeof(struct BlasterFireReq);
+        case STREAM_CTRL_CMD:
+            return sizeof(struct StreamCtrlReq);
+        case VISION_DETECT_ENABLE_CMD:
+            return sizeof(struct VisionDetectEnableReq);
         default:
             return 0;
     }
@@ -56,6 +64,14 @@ message_module(int cmd) {
         case SET_WHEEL_SPEED_CMD:
         case CHASSIS_SPEED_MODE_CMD:
             return host2byte(CHASSIS_HOST, CHASSIS_INDEX);
+        case GIMBAL_CTRL_SPEED_CMD:
+            return host2byte(GIMBAL_HOST, GIMBAL_INDEX);
+        case BLASTER_FIRE_CMD:
+            return host2byte(BLASTER_HOST, BLASTER_INDEX);
+        case STREAM_CTRL_CMD:
+            return host2byte(STREAM_HOST, STREAM_INDEX);
+        case VISION_DETECT_ENABLE_CMD:
+            return host2byte(VISION_HOST, VISION_INDEX);
         default:
             return 0;
     }

src/message.c

@@ -1,9 +1,14 @@
 #include "message.h"
+#include "camera.h" // FIXME: hack
 #include "crc.h"
 
 enum MESSAGEERR
 message_validate(const union Message* message) {
 
+    // FIXME: hack
+    if(message->header.cmd == VISION_DETECT_INFO_CMD)
+        return MESSAGEERR_NONE;
+
     uint16_t length = message->header.length & 0x1FFF & ~0x400;
 
     if(message->header.crc != crc8(message, 3))

src/modules/blaster.c

@@ -0,0 +1,16 @@
+#include "message.h"
+#include "connection.h"
+#include "robomaster.h"
+
+void
+blaster_fire (
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		enum FIRETYPE type,
+		enum FIRETIMES times ) {
+    req->blaster.type = type;
+    req->blaster.times = times;
+    req_finalize(seq, BLASTER_FIRE_CMD, ack, req);
+}
+

src/modules/camera.c

@@ -0,0 +1,28 @@
+#include "message.h"
+#include "connection.h"
+#include "robomaster.h"
+
+void
+stream_ctrl (
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		enum STREAMSTATE state,
+		enum STREAMRESOLUTION res,
+		enum STREAMCTRL ctrl ) {
+    req->stream.state = state;
+    req->stream.resolution = res;
+    req->stream.ctrl = ctrl;
+    req->stream.conn_type = 0; // Hardcode to WiFi
+    req_finalize(seq, STREAM_CTRL_CMD, ack, req);
+}
+
+void
+vision_enable (
+		union Request* req,
+        uint16_t seq,
+        bool ack ) {
+    req->enablevision.type = VISION_DETECT_ENABLE_PERSON;
+    req_finalize(seq, VISION_DETECT_ENABLE_CMD, ack, req);
+}
+

src/modules/chassis.c

@@ -15,7 +15,7 @@ set_wheel_speed (
     req->wheel.wheel_speed[1] = w2;
     req->wheel.wheel_speed[2] = w3;
     req->wheel.wheel_speed[3] = w4;
-    req_finalize(seq, SET_WHEEL_SPEED_CMD, false, req);
+    req_finalize(seq, SET_WHEEL_SPEED_CMD, ack, req);
 }
 
 void
@@ -29,5 +29,5 @@ chassis_speed_mode (
     req->chsspeed.speed[0] = x;
     req->chsspeed.speed[1] = y;
     req->chsspeed.speed[2] = z;
-    req_finalize(seq, CHASSIS_SPEED_MODE_CMD, false, req);
+    req_finalize(seq, CHASSIS_SPEED_MODE_CMD, ack, req);
 }

src/modules/gimbal.c

@@ -0,0 +1,18 @@
+#include "message.h"
+#include "connection.h"
+#include "robomaster.h"
+
+void
+gimbal_ctrl_speed (
+		union Request* req,
+        uint16_t seq,
+        bool ack,
+		int16_t p,
+		int16_t y,
+		int16_t r ) {
+    req->gimbspeed.yaw = y;
+    req->gimbspeed.roll = r;
+    req->gimbspeed.pitch = p;
+    req->gimbspeed.ctrl = 0xDC;
+    req_finalize(seq, GIMBAL_CTRL_SPEED_CMD, ack, req);
+}

src/modules/sdk.c

@@ -17,7 +17,7 @@ set_sdk_connection(
     req->sdkconn.protocol = 0;
     req->sdkconn.ip_address = ip_address;
     req->sdkconn.port = port;
-    req_finalize(seq, SET_SDK_CONNECTION_CMD, true, req);
+    req_finalize(seq, SET_SDK_CONNECTION_CMD, ack, req);
 }
 
 void
@@ -27,7 +27,7 @@ set_sdk_mode(
         bool ack,
 		bool enable ) {
     req->sdkmode.enable = enable;
-    req_finalize(seq, SET_SDK_MODE_CMD, true, req);
+    req_finalize(seq, SET_SDK_MODE_CMD, ack, req);
 }
 
 void
@@ -35,7 +35,7 @@ sdk_heartbeat(
 		union Request* req,
         uint16_t seq,
         bool ack ) {
-    req_finalize(seq, SDK_HEARTBEAT_CMD, false, req);
+    req_finalize(seq, SDK_HEARTBEAT_CMD, ack, req);
 }
 
 void
@@ -45,7 +45,7 @@ set_robot_mode (
         bool ack,
 		enum MOVEMENTMODE mode ) {
     req->mvmode.mode = mode;
-    req_finalize(seq, SET_ROBOT_MODE_CMD, true, req);
+    req_finalize(seq, SET_ROBOT_MODE_CMD, ack, req);
 }
 
 void
@@ -54,7 +54,7 @@ subnode_reset (
         uint16_t seq,
         bool ack ) {
     req->subnodereset.hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX);
-    req_finalize(seq, SUBNODE_RESET_CMD, true, req);
+    req_finalize(seq, SUBNODE_RESET_CMD, ack, req);
 }
 
 void
@@ -64,7 +64,7 @@ subscribe_add_node (
         bool ack ) {
     req->subnodeadd.hostbyte = host2byte(CLIENT_HOST, CLIENT_INDEX);
     req->subnodeadd.sub_vision = 0x03000000;
-    req_finalize(seq, SUBSCRIBE_ADD_NODE_CMD, true, req);
+    req_finalize(seq, SUBSCRIBE_ADD_NODE_CMD, ack, req);
 }
 
 void
@@ -93,6 +93,6 @@ set_system_led (
     req->led.t1 = t1;
     req->led.t2 = t2;
 
-    req_finalize(seq, SET_SYSTEM_LED_CMD, true, req);
+    req_finalize(seq, SET_SYSTEM_LED_CMD, ack, req);
 
 }

src/robo.c

@@ -5,7 +5,19 @@
 #include <string.h>
 #include <stdlib.h>
 
-struct Robot {
+#include <stdio.h>
+
+struct FragmentLink {
+    struct Fragment fragment;
+    struct FragmentLink *next;
+};
+
+struct RobotImp {
+
+    struct {
+        struct FragmentLink *head;
+        struct FragmentLink *free;
+    } frag_queue;
 
     struct Client* client;
     uint16_t seq;
@@ -16,6 +28,14 @@ struct Robot {
     uint8_t colors[3];
     bool dirty_colors;
 
+    int16_t gimbal[2];
+    bool dirty_gimbal;
+
+    bool dirty_blaster;
+
+    bool stream_state;
+    bool dirty_stream;
+
     bool sdk_mode;
 
     enum {
@@ -34,6 +54,19 @@ struct Robot {
 
 };
 
+int
+robot_stream(Robot robot, bool state) {
+    robot->stream_state = state;
+    robot->dirty_stream = true;
+    return 1;
+}
+
+int
+robot_blast(Robot robot) {
+    robot->dirty_blaster = true;
+    return 1;
+}
+
 int
 robot_drive(Robot robot, float x, float y, float r) {
     // TODO: move individual wheel speed calculation to work function
@@ -46,6 +79,14 @@ robot_drive(Robot robot, float x, float y, float r) {
     return 1;
 }
 
+int
+robot_aim(Robot robot, float p, float y) {
+    robot->gimbal[0] = p * 360;
+    robot->gimbal[1] = y * 360;
+    robot->dirty_gimbal = true;
+    return 1;
+}
+
 int
 robot_led(Robot robot, unsigned char r, unsigned char g, unsigned char b) {
     robot->colors[0] = r;
@@ -67,6 +108,61 @@ robot_heartbeat(Robot robot) {
 
 }
 
+inline
+static
+void
+enqueue_fragment (
+        Robot robot,
+        const struct Fragment fragment ) {
+
+    struct FragmentLink *link;
+    if(robot->frag_queue.free) {
+        link = robot->frag_queue.free;
+        robot->frag_queue.free = link->next;
+    } else {
+        link = malloc(sizeof(struct FragmentLink));
+        link->next = NULL;
+    }
+
+    link->fragment = fragment;
+
+    if(robot->frag_queue.head) {
+        robot->frag_queue.head->next = link;
+    } else {
+        robot->frag_queue.head = link;
+    }
+
+}
+
+inline
+static
+void
+dequeue_fragment (
+        Robot robot,
+        struct Fragment *fragment ) {
+
+    fragment->type = 0;
+    if(!robot->frag_queue.head)
+        return;
+
+    struct FragmentLink *link = robot->frag_queue.head;
+    *fragment = link->fragment;
+
+    robot->frag_queue.head = link->next;
+    link->next = NULL;
+    if(robot->frag_queue.free) {
+        robot->frag_queue.free->next = link;
+    } else {
+        robot->frag_queue.free = link;
+    }
+
+}
+
+int robot_poll(Robot robot, struct Fragment* fragment) {
+    dequeue_fragment(robot, fragment);
+    return fragment->type;
+}
+
 int
 robot_work(Robot robot) {
 
@@ -151,6 +247,15 @@ robot_work(Robot robot) {
                 req_send(robot->client->dev_conn, &req);
                 robot->dirty_wheels = false;
             }
+            if(robot->dirty_gimbal) {
+                gimbal_ctrl_speed (
+                        &req, robot->seq++, false,
+                        robot->gimbal[0],
+                        robot->gimbal[1],
+                        0 );
+                req_send(robot->client->dev_conn, &req);
+                robot->dirty_gimbal = false;
+            }
             if(robot->dirty_colors) {
                 set_system_led (
                         &req, robot->seq++, false,
@@ -165,6 +270,32 @@ robot_work(Robot robot) {
                 req_send(robot->client->dev_conn, &req);
                 robot->dirty_colors = false;
             }
+            if(robot->dirty_blaster) {
+                blaster_fire (
+                    &req, robot->seq++, false,
+                    FIRETYPE_INFRARED,
+                    ONE
+                );
+                req_send(robot->client->dev_conn, &req);
+                robot->dirty_blaster = false;
+            }
+            if(robot->dirty_stream) {
+                stream_ctrl (
+                    &req, robot->seq++, true,
+                    robot->stream_state ? STREAMSTATE_ON : STREAMSTATE_OFF,
+                    RES_720P,
+                    STREAMCTRL_SDK
+                );
+                req_send(robot->client->dev_conn, &req);
+                stream_ctrl (
+                    &req, robot->seq++, true,
+                    robot->stream_state ? STREAMSTATE_ON : STREAMSTATE_OFF,
+                    RES_720P,
+                    STREAMCTRL_VIDEO
+                );
+                req_send(robot->client->dev_conn, &req);
+                robot->dirty_stream = false;
+            }
             break;
         }
 
@@ -175,9 +306,38 @@ robot_work(Robot robot) {
 
     union Message resp;
     poll_message(robot->client, &resp);
+    if(resp.header.cmd) {
+        const struct Fragment fragment = {
+            .type = FRAGMENT_MESSAGE,
+            .message = resp
+        };
+        enqueue_fragment(robot, fragment);
+    }
     switch(resp.header.cmd) {
         case 0:
             break;
+        case VISION_DETECT_INFO_CMD:
+            // enqueue rectangles
+            for(int i = 0; i < resp.push.vision.count; i++) {
+                int type;
+                switch(resp.push.vision.type) {
+                    case VISIONTYPE_SHOULDER:
+                    case VISIONTYPE_PERSON:
+                    case VISIONTYPE_GESTURE:
+                    case VISIONTYPE_ROBOT:
+                        type = FRAGMENT_RECTANGLE;
+                    case VISIONTYPE_LINE:
+                        type = FRAGMENT_LINE;
+                    case VISIONTYPE_MARKER:
+                        type = FRAGMENT_MARKER;
+                }
+                const struct Fragment fragment = {
+                    .type = type,
+                    .rect = resp.push.vision.rects[i]
+                };
+                enqueue_fragment(robot, fragment);
+            }
+            break;
         case SET_SDK_CONNECTION_CMD:
             // TODO: Do more with this
             if(resp.resp.sdkconn.retcode) {
@@ -229,8 +389,8 @@ robot_work(Robot robot) {
 }
 
 Robot robot_new() {
-    struct Robot* robot = malloc(sizeof(struct Robot));
-    memset(robot, 0, sizeof(struct Robot));
+    struct RobotImp* robot = malloc(sizeof(struct RobotImp));
+    memset(robot, 0, sizeof(struct RobotImp));
     return robot;
 }
 

src/sdl.c

@@ -1,3 +1,4 @@
+#include "robomaster.h"
 #include "roboeasy.h"
 #include "SDL2/SDL.h"
 
@@ -16,9 +17,11 @@ struct {
 };
 int color = 0;
 
+float pitch = 0, yaw = 0;
 float x = 0, y = 0, z = 0;
 static Uint32 drive_timer_handler(Uint32 interval, void* param) {
     robot_drive((Robot)param, x, y, z);
+    robot_aim((Robot)param, pitch, yaw);
     return 75;
 }
 static Uint32 heartbeat_timer_handler(Uint32 interval, void* param) {
@@ -27,10 +30,15 @@ static Uint32 heartbeat_timer_handler(Uint32 interval, void* param) {
 }
 
 int main(int argc, char* argv[]) {
-    if(SDL_Init(SDL_INIT_VIDEO | SDL_INIT_GAMECONTROLLER) < 0) {
+    if(SDL_Init(SDL_INIT_VIDEO | SDL_INIT_JOYSTICK) < 0) {
         fprintf(stderr, "%s", SDL_GetError());
         return 1;
     }
+    printf("Detected %d joysticks\n", SDL_NumJoysticks());
+    SDL_Joystick* joystick = NULL;
+    if(SDL_NumJoysticks() > 0) {
+        joystick = SDL_JoystickOpen(0);
+    }
     SDL_Window* win = SDL_CreateWindow(
 		    "Robomaster",
 		    SDL_WINDOWPOS_UNDEFINED,
@@ -44,10 +52,73 @@ int main(int argc, char* argv[]) {
 
     Robot robot = robot_new();
     robot_init(robot);
+    robot_stream(robot, true);
+
     SDL_AddTimer(75, drive_timer_handler, robot);
     SDL_AddTimer(1000, heartbeat_timer_handler, robot);
 
+    int h, w;
+    SDL_GetWindowSize(win, &w, &h);
+
     while(robot_work(robot)) {
+
+        struct Fragment fragment;
+        while(robot_poll(robot, &fragment)) {
+            switch(fragment.type) {
+                case FRAGMENT_RECTANGLE:
+                case FRAGMENT_LINE:
+                case FRAGMENT_MARKER:
+                    printf("Rect(%f,%f,%f,%f)\n",
+                            fragment.rect.rect.x,
+                            fragment.rect.rect.y,
+                            fragment.rect.rect.w,
+                            fragment.rect.rect.h );
+                    break;
+                case FRAGMENT_MESSAGE:
+                    switch(fragment.message.header.cmd) {
+                        case VISION_DETECT_INFO_CMD:
+                            if(fragment.message.push.vision.errcode) {
+                                printf("Vision error %d\n", fragment.message.push.vision.errcode);
+                                break;
+                            }
+                            printf("Received %d rectangles\n", fragment.message.push.vision.count);
+                            printf("\tVision type %d\n", fragment.message.push.vision.type);
+                            printf("\tVision status %d\n", fragment.message.push.vision.status);
+                            break;
+                        case SDK_HEARTBEAT_CMD:
+                            if(fragment.message.resp.heartbeat.retcode) {
+                                printf("ERROR: Heartbeat error\n");
+                                break;
+                            }
+                            printf("Heartbeat\n");
+                            break;
+                        case STREAM_CTRL_CMD:
+                            if(fragment.message.resp.stream.retcode) {
+                                printf("ERROR: Stream not enabled\n");
+                                break;
+                            }
+                            printf("Stream enabled\n");
+                            break;
+                        case VISION_DETECT_ENABLE_CMD:
+                            if(fragment.message.resp.enablevision.retcode) {
+                                printf("ERROR: Vision not enabled\n");
+                                break;
+                            }
+                            if(fragment.message.resp.enablevision.error) {
+                                printf("ERROR: Vision not enabled %d\n",
+                                    fragment.message.resp.enablevision.error);
+                                break;
+                            }
+                            printf("Vision enabled\n");
+                            break;
+                    }
+                    break;
+                default:
+                    printf("Unhandled fragment type\n");
+                    break;
+            }
+        }
+
         SDL_Event event;
         while(SDL_PollEvent(&event)) {
             switch(event.type) {
@@ -79,8 +150,39 @@ int main(int argc, char* argv[]) {
                         case SDL_SCANCODE_SPACE:
                             robot_led(robot, colors[color].r, colors[color].g, colors[color].b);
                             color = (color + 1) % 3;
+                            break;
+                        case SDL_SCANCODE_RETURN:
+                            robot_blast(robot);
+                            break;
                         default: break;
 		            }
+                    break;
+                case SDL_MOUSEMOTION:
+                    yaw = (float)event.motion.xrel;
+                    pitch = (float)event.motion.yrel;
+                    break;
+                case SDL_JOYAXISMOTION:
+                    switch(event.jaxis.axis) {
+                        case 0:
+                            x = (float)event.jaxis.value / 32767;
+                            break;
+                        case 1:
+                            y = (float)event.jaxis.value / 32767;
+                            break;
+                        case 4:
+                            z = (float)event.jaxis.value / 32767 / 2;
+                            break;
+                        case 2:
+                            yaw = (float)event.jaxis.value / 32767;
+                            break;
+                        case 3:
+                            pitch = (float)event.jaxis.value / 32767;
+                            break;
+                        default:
+                            printf("axis: %d\n", event.jaxis.axis);
+                            break;
+                    }
+                    break;
                 case SDL_WINDOWEVENT:
                     if(event.window.event != SDL_WINDOWEVENT_CLOSE) break;
                 case SDL_QUIT:
@@ -90,6 +192,7 @@ int main(int argc, char* argv[]) {
 	    }
     }
 
+    SDL_JoystickClose(joystick);
     SDL_Quit();
     return 0;
 }