diff --git a/src/robomastersh.c b/src/robomastersh.c
index 2f97793..99ee094 100644
--- a/src/robomastersh.c
+++ b/src/robomastersh.c
@@ -1,6 +1,8 @@
 #include "robomaster.h"
 
 #include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
 
 int main(int argc, char* argv[])
 {
@@ -21,20 +23,41 @@ int main(int argc, char* argv[])
         return 1;
     }
 
-    set_system_led (
-    		client,
-    		0xFF,
-    		0x00,
-    		0xFF,
-    		LEDCOMP_ALL,
-    		0xFFFF,
-    		LEDEFFECT_ON,
-    		100,
-    		100 );
-    poll_message(client, &msg);
-    if(msg.header.cmdid == SET_SYSTEM_LED_CMDID || msg.resp.led.retcode) {
-        fprintf(stderr, "Could not set LED color\n");
-        return 1;
+    int c;
+    while((c = getopt(argc, argv, "hl:::w::::"))) {
+        switch(c) {
+            case 'l':
+                set_system_led (
+                		client,
+                		strtol(argv[optind + 0], NULL, 0),
+                		strtol(argv[optind + 1], NULL, 0),
+                		strtol(argv[optind + 2], NULL, 0),
+                		LEDCOMP_ALL,
+                		0xFFFF,
+                		LEDEFFECT_ON,
+                		100,
+                		100 );
+                poll_message(client, &msg);
+                if(msg.header.cmdid == SET_SYSTEM_LED_CMDID || msg.resp.led.retcode) {
+                    fprintf(stderr, "Could not set LED color\n");
+                    return 1;
+                }
+		break;
+            case 'w':
+                set_wheel_speed (
+                		client,
+                		strtol(argv[optind + 0], NULL, 0),
+                		strtol(argv[optind + 1], NULL, 0),
+                		strtol(argv[optind + 2], NULL, 0),
+                		strtol(argv[optind + 3], NULL, 0) );
+		break;
+            case '?':
+            default:
+		fprintf(stderr, "Unknown argument %c\n", optopt);
+            case 'h':
+		printf("Usage: [-h] [-l r g b] [-w w1 w2 w3 w4]");
+		break;
+	}
     }
 
     return 0;