// Эталонный скетч: робот-собака прыгает на месте.
//
// Присесть, резко выпрямить все четыре лапы, в воздухе поджать их, мягко
// приземлиться — и так каждые три секунды. Задние лапы при толчке стоят
// чуть позади бедра, передние — чуть позади плеча: иначе толчок задних
// задирает корпус носом вверх и собака садится на «хвост».
//
// Высота прыжка ограничена приводами: 10 рад/с — как у быстрых сервоприводов.
// Полёт около 0,18 с, центр масс поднимается примерно на 4 см.
//
// (Ниже — общая часть со скетчем рыси dog_walk.ino.)
//
// Рысь: диагональные лапы (передняя левая + задняя правая, передняя правая +
// задняя левая) шагают парами. Пока одна пара переносится вперёд по дуге,
// другая стоит и толкает корпус. Положение лапы задаётся точкой под осью
// бедра, углы считает обратная кинематика (бедро и голень по 21 см, колено
// назад — голень уходит вперёд).
//
// Привод: PCA9685 по I2C (0x40). Каналы: 0–2 передняя левая, 3–5 передняя
// правая, 6–8 задняя левая, 9–11 задняя правая — отведение, бедро, колено.
// Значение «прямо» для write(): отведение и бедро 90, колено 0.
// Курс держит гироскоп MPU6050 (0x68): левые и правые лапы шагают чуть
// по-разному, пока курс не выровняется. TURN — поворот (разница шага).

#include <Wire.h>
#include <math.h>

const uint8_t PCA = 0x40, MPU = 0x68;

const float L1 = 0.21, L2 = 0.21;   // бедро, голень, м
const float H0 = 0.4195;            // ноги почти прямые (как собака стоит при включении)
const float H_STAND = 0.30;         // стойка: ось бедра над стопой, м
const float H_CROUCH = 0.20;        // присед
const float H_PUSH = 0.41;          // толчок: лапы почти прямые
const float H_LAND = 0.30;          // в воздухе и при приземлении
const float Z_FRONT = -0.04;        // передние лапы при толчке: назад от плеча, м
const float Z_REAR = -0.08;         // задние лапы при толчке: назад от бедра, м
const float PERIOD = 3.0;           // прыжок раз в столько секунд
const float T_PUSH = 0.12;          // сколько длится толчок, с

const float ZERO[3] = {90, 90, 0};  // write() при прямом суставе: отведение, бедро, колено
const float R = 180.0 / M_PI;

float smooth(float x) { x = x < 0 ? 0 : (x > 1 ? 1 : x); return x * x * (3 - 2 * x); }

void reg(uint8_t dev, uint8_t r, uint8_t v) {
  Wire.beginTransmission(dev); Wire.write(r); Wire.write(v); Wire.endTransmission();
}

void setJoint(uint8_t ch, uint8_t kind, float deg) {
  float w = ZERO[kind] + deg;
  if (w < 0) w = 0;
  if (w > 180) w = 180;
  uint16_t us = 500 + (uint16_t)(w * (2000.0 / 180.0));
  uint16_t off = (uint32_t)us * 4096UL / 19988UL;
  Wire.beginTransmission(PCA);
  Wire.write(0x06 + 4 * ch);
  Wire.write(0); Wire.write(0); Wire.write(off & 0xff); Wire.write(off >> 8);
  Wire.endTransmission();
}

// Лапа: стопа относительно оси бедра — fh вниз, fz вперёд (м).
void paw(uint8_t c0, float fh, float fz) {
  float d = sqrt(fh * fh + fz * fz);
  if (d > L1 + L2 - 0.0005) d = L1 + L2 - 0.0005;
  float k = M_PI - acos((L1 * L1 + L2 * L2 - d * d) / (2 * L1 * L2));
  float th = atan2(fz, fh) - acos((L1 * L1 + d * d - L2 * L2) / (2 * L1 * d));
  setJoint(c0 + 0, 0, 0);
  setJoint(c0 + 1, 1, th * R);
  setJoint(c0 + 2, 2, k * R);
}


void setup() {
  Serial.begin(9600);
  Wire.begin();
  Wire.setClock(400000);
  reg(PCA, 0x00, 0x10);
  reg(PCA, 0xFE, 121);         // 50 Гц
  reg(PCA, 0x00, 0x20);
  delay(5);
  reg(PCA, 0x00, 0xA0);
}

int jumps = 0;

void loop() {
  float t = millis() / 1000.0;
  // Цикл: 0–1 с — присесть, 1–1.3 — ждать, толчок T_PUSH, дальше — лапы
  // под себя и стойка до следующего цикла.
  float c = fmod(t, PERIOD);
  float h, zf = 0, zr = 0;
  if (t < 1.0) h = H0 + (H_STAND - H0) * smooth(t);            // встать с прямых лап
  else if (c < 1.0) h = H_STAND + (H_CROUCH - H_STAND) * smooth(c);
  else if (c < 1.5) { h = H_CROUCH; zf = Z_FRONT * (c - 1.0) / 0.5; zr = Z_REAR * (c - 1.0) / 0.5; }
  else if (c < 1.5 + T_PUSH) {
    h = H_PUSH; zf = Z_FRONT; zr = Z_REAR;
    static float last = -10;
    if (t - last > 1) { last = t; jumps++; Serial.print("прыжок "); Serial.println(jumps); }
  }
  else h = H_LAND;
  paw(0, h, zf); paw(3, h, zf);    // передние
  paw(6, h, zr); paw(9, h, zr);    // задние
}