From b415b9d5919133eb7fe792e08e52f7a8ab6d31ec Mon Sep 17 00:00:00 2001
From: PgSocks <pgsocks@pm.me>
Date: Wed, 1 Feb 2023 10:57:56 -0600
Subject: [PATCH] Add simple drive function to API

The wheel speed function requires setting individual wheel speed. And
knowledge of the max and min values. The simple drive function can
accept x, y, and r speeds from -1 to 1, and will compute the wheel
speeds for the user to achieve the desired velocity.
---
 include/robomaster.h | 9 +++++++++
 src/robomaster.c     | 5 +++++
 2 files changed, 14 insertions(+)

diff --git a/include/robomaster.h b/include/robomaster.h
index a89703f..9cc70ea 100644
--- a/include/robomaster.h
+++ b/include/robomaster.h
@@ -27,3 +27,12 @@ static inline void byte2host(uint8_t b, uint8_t* host, uint8_t* index) {
     *index = b >> 5;
 }
 
+/*
+ * Drive the wheels of the robot. Values should be [-1, 1]
+ *
+ * x: positive is forward, negative is backwards
+ * y: positive is right, negative is left
+ * r: positive is clockwise, negative is counterclockwise
+ */
+void drive(Client client, float x, float y, float r);
+
diff --git a/src/robomaster.c b/src/robomaster.c
index 382544a..951c042 100644
--- a/src/robomaster.c
+++ b/src/robomaster.c
@@ -29,3 +29,8 @@ void poll_message(Client client, union Message* resp) {
     connection_read(conn, resp);
 
 }
+
+void drive(Client client, float x, float y, float r) {
+    float w1 = y + x + r, w2 = y - x -r, w3 = y + x -r, w4 = y - x + r;
+    set_wheel_speed(client, w1 * 1000, -w2 * 1000, -w3 * 1000, w4 * 1000);
+}