first commit

include/motor.hpp

@@ -0,0 +1,64 @@
+#pragma once
+#include <Arduino.h>
+
+class Motor {
+    private: GPIO_TypeDef* gpio;
+    private: int pinMask, lastStepTime;
+    private: int sequence[4], currentStep;
+    private: int maxSteps, stepCount;
+
+    public: Motor(GPIO_TypeDef* gpio,
+        int pinOffset, int maxSteps) {
+        /* Initialize sequence and mask */
+        sequence[0] = 0x0003 << pinOffset;
+        sequence[1] = 0x0006 << pinOffset;
+        sequence[2] = 0x000C << pinOffset;
+        sequence[3] = 0x0009 << pinOffset;
+        pinMask = 0x000F << pinOffset;
+
+        /* Set initial values */
+        lastStepTime = millis();
+        this -> maxSteps = maxSteps;
+        this -> gpio = gpio;
+        currentStep = 0;
+        stepCount = 0;
+    }
+
+    public: void begin() {
+        /* A bad solution to a non-existing problem */
+        if(gpio == GPIOA) RCC->AHBENR |= RCC_AHBENR_GPIOAEN;
+        else if(gpio == GPIOB) RCC->AHBENR |= RCC_AHBENR_GPIOBEN;
+        else if(gpio == GPIOC) RCC->AHBENR |= RCC_AHBENR_GPIOCEN;
+        else if(gpio == GPIOD) RCC->AHBENR |= RCC_AHBENR_GPIODEN;
+        else if(gpio == GPIOF) RCC->AHBENR |= RCC_AHBENR_GPIOFEN;
+
+        /* Set initial values */
+        lastStepTime = millis();
+        gpio -> MODER = 0x5555555;
+        gpio -> ODR = 0;
+        currentStep = 0;
+        stepCount = 0;
+    }
+
+    private: void step(bool clockwise, int delay) {
+        /* Progress sequence and thus turn motor */
+        if(millis() - lastStepTime < delay) return;
+        int next = (currentStep + 1) % 4, back = (currentStep + 3) % 4;
+        gpio -> ODR = (gpio -> ODR & ~pinMask) | sequence[currentStep];
+        currentStep = clockwise ? next : back;
+        stepCount += clockwise ? 1 : -1;
+        lastStepTime = millis();
+    }
+
+    public: int angle() {
+        /* Calculate rotation angle */
+        float count = (float) stepCount;
+        return (count / maxSteps) * 360;
+    }
+
+    public: void update(int target, int delay) {
+        /* Rotate motor towards given angle */
+        if(angle() == target) return;
+        step(angle() < target, delay);
+    }
+};

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