Add gimble control to simple robot API

2023-02-21 15:29:25 -06:00
parent 0b238ce048
commit 2c42352c85
2 changed files with 31 additions and 0 deletions
@@ -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,