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