update

include/robot.hpp

@@ -20,9 +20,9 @@ class Robot {
 
         /* Rotate with according speeds */
         const int m = max(max(d1, d2), d3);
-        arm1.update(tArm1, 4 * (m / d1));
-        arm2.update(tArm2, 4 * (m / d2));
-        yaw.update(tYaw, 4 * (m / d3));
+        arm1.update(tArm1, 2 * (m / d1));
+        arm2.update(tArm2, 2 * (m / d2));
+        yaw.update(tYaw, 2 * (m / d3));
 
         /* Is destination reached */
         return arm1.angle() == tArm1
@@ -49,4 +49,11 @@ class Robot {
         /* Rotate to origin */
         return pose(0, 0, 0);
     }
+
+    public: void begin() {
+        /* Setup motors */
+        arm1.begin();
+        arm2.begin();
+        yaw.begin();
+    }
 };