59 lines
1.7 KiB
C++
59 lines
1.7 KiB
C++
#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, 2 * (m / d1));
|
|
arm2.update(tArm2, 2 * (m / d2));
|
|
yaw.update(tYaw, 2 * (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);
|
|
}
|
|
|
|
public: void begin() {
|
|
/* Setup motors */
|
|
arm1.begin();
|
|
arm2.begin();
|
|
yaw.begin();
|
|
}
|
|
}; |