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

@@ -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);
+

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);
+}