#include "Movement.h"

// У библиотеки есть два конструктора для создания класса.
// 1 - пустой, как указано ниже.
// 2 - Movement m(double wheel_rad, int steps_per_turn) - задает два дополнительных параметра (радиус колеса в миллиметрах и количество шагов на оборот у шаговика)
Movement m;



void setup() {
  Serial.begin(9600);

  // Если был использован первый тип конструктора, необходимые константы надо задать вручную:
  m.set_wheel_rad(100);
  m.set_steps_per_turn(360);

  // Поскольку подсчет шагов и измерения радиуса колеса могут иметь погрешность, библиотека имеет
  // скаляр для модификации количества подсчитанных шагов. Если робот делает их слишком мало,
  // сюда можно поставить, например, "1.3", и количество шагов увеличится. Если слишком много - "0.7"
  // и так далее. Этот параметр нужно будет найти подбором путем большого количества тестов.
  // Это обязательный параметр, без него ничего работать не будет.
  m.set_steps_mod(0.9);


  // Метод set_speed(int speed) устанавливает относительную скорость робота.
  // Все значения скорости находятся в интервале [0, 1]. 0 - отсутствие движения,
  // 1 - максимальная скорость. В этом методе, по сути, задается штука, на которую
  // будут умножаться все скорости колес после их вычисления.
  m.set_speed(1);

  // На данный момент работает функция для движения по прямой - m.move_straight(double angle, double distance_mm);
  // Она принимает значение угла (в радианах) и расстояния (в миллиметрах) и делает все подсчеты.
  m.move_straight(3.14, 500);

  // Получить доступ к скоростям и шагам можно через соответствующие функции.
  // Метод get_speed(int wheel) возвращает скорость для определенного колеса.
  // 0 - верхнее левое
  // 1 - верхнее правое
  // 2 - нижнее левое
  // 3 - нижнее правое
  Serial.print(m.get_speed(0));
  Serial.print('\t');
  Serial.print(m.get_speed(1));
  Serial.print('\t');
  Serial.print(m.get_speed(2));
  Serial.print('\t');
  Serial.print(m.get_speed(3));
  Serial.print("\t\t");

  // Метод get_steps(int type) возвращает количество шагов, которое должно сделать
  // колесо каждого типа. Тип - это, по сути, угол шарниров на колесе.
  // Тип 1 - верхнее левое и нижнее правое
  // Тип 2 - верхнее правое и нижнее левое
  // Количество шагов может быть отрицательным! Это просто значит, что крутить колесо надо в противоположную сторону.
  Serial.print(m.get_steps(1));
  Serial.print('\t');
  Serial.println(m.get_steps(2));

  // Через библиотеку также можно напрямую задать угол без расстояния и она
  // просто подсчитает скорости для каждого колеса.
  for (double angle_deg = 0; angle_deg <= 360; ++angle_deg) {
    // Из градусов в радианы ( acos(-1) это Пи )

    // Метод set_force_angle(double angle) принимает угол в радианах и вычисляет
    // скорости для колес.
    m.set_force_angle(angle_deg / 180 * acos(-1));

    Serial.print(angle_deg);
    Serial.print(":  \t");
    Serial.print(m.get_speed(0));
    Serial.print('\t');
    Serial.print(m.get_speed(1));
    Serial.print('\t');
    Serial.print(m.get_speed(2));
    Serial.print('\t');
    Serial.println(m.get_speed(3));
  }
}

void loop() {

}