Add gimble control to simple robot API

include/roboeasy.h

@@ -29,6 +29,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.

src/robo.c

@@ -16,6 +16,9 @@ struct Robot {
     uint8_t colors[3];
     bool dirty_colors;
 
+    int16_t gimbal[2];
+    bool dirty_gimbal;
+
     bool sdk_mode;
 
     enum {
@@ -46,6 +49,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;
@@ -151,6 +162,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,