Diff: main.cpp

Revision:

0:c1930e7050d1

diff -r 000000000000 -r c1930e7050d1 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 03 13:38:24 2017 +0000
@@ -0,0 +1,25 @@
+#include "robotarm.h"
+
+Robotarm arm;
+
+int main()
+{
+    //Run the main initialisation routine
+    arm.init();
+    display.clear_display();
+    while(1) {
+        int base = servo.GetPosition(BASE);
+        int shoulder = servo.GetPosition(SHOULDER);
+        int elbow = servo.GetPosition(ELBOW);
+        int wrist = servo.GetPosition(WRIST);
+        char s1[17];
+        char s2[17];
+        sprintf(s1,"B:%4d S:%4d",base,shoulder);
+        sprintf(s2,"E:%4d W:%4d",elbow,wrist);
+        display.set_position(0,0);
+        display.write_string(s1);
+        display.set_position(1,0);
+        display.write_string(s2);
+        wait(0.2);
+    }
+}
\ No newline at end of file