first commit

include/robot.hpp

@@ -0,0 +1,52 @@
+#pragma once
+#include "motor.hpp"
+#include <math.h>
+
+class Robot {
+    private: float size;
+    private: Motor arm1;
+    private: Motor arm2;
+    private: Motor yaw;
+
+    public: Robot(float size,
+        const Motor& yaw, const Motor& arm1, const Motor& arm2
+    ) : size(size), arm1(arm1), arm2(arm2), yaw(yaw) {}
+
+    public: bool pose(int tYaw, int tArm1, int tArm2) {
+        /* Calculate the absolute distance to target */
+        const int d1 = abs(tArm1 - arm1.angle());
+        const int d2 = abs(tArm2 - arm2.angle());
+        const int d3 = abs(tYaw - yaw.angle());
+
+        /* 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));
+
+        /* Is destination reached */
+        return arm1.angle() == tArm1
+        && arm2.angle() == tArm2
+        && yaw.angle() == tYaw;
+    }
+
+    public: bool point(float x, float y) {
+        /* Calculate pythagorean theorem */
+        const float dist = sqrtf(x * x + y * y);
+        const int tYaw = (int) degrees(atan2f(x, y));
+        float r = (dist / 2.0f) / size;
+        r = constrain(r, -1.0f, 1.0f);
+
+        /* Calculate angles and rotate */
+        const float a1 = degrees(acosf(r));
+        const float a2 = (90.0f - a1) * 2.0f;
+        const int tArm1 = 90 - (int) a1;
+        const int tArm2 = (int) a2 - 90;
+        return pose(tYaw, tArm1, tArm2);
+    }
+
+    public: bool reset() {
+        /* Rotate to origin */
+        return pose(0, 0, 0);
+    }
+};