From 2c42352c850ce6f5ffe630492b70048fef225f21 Mon Sep 17 00:00:00 2001
From: PgSocks <pgsocks@pm.me>
Date: Tue, 21 Feb 2023 15:14:25 -0600
Subject: [PATCH] Add gimble control to simple robot API

---
 include/roboeasy.h | 11 +++++++++++
 src/robo.c         | 20 ++++++++++++++++++++
 2 files changed, 31 insertions(+)

diff --git a/include/roboeasy.h b/include/roboeasy.h
index 3c9741e..a1ea2e8 100644
--- a/include/roboeasy.h
+++ b/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.
diff --git a/src/robo.c b/src/robo.c
index 501870a..084bb1b 100644
--- a/src/robo.c
+++ b/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,