// Эталонный скетч: робот-человек идёт вперёд (1.9.57 — непрерывная походка
// с обратной связью по датчику наклона).
//
// Походка без остановок: таз всё время едет вперёд, бок о бок качается над
// опорной ногой, шагающая нога переносится по плавной кривой. Шаг 12 см
// за 0,5 с — около 24 см/с (прежняя походка «перенос веса — шаг» шла 2 см/с).
// Положение стопы задаётся точкой в пространстве, углы считает обратная
// кинематика (двухзвенная нога: бедро и голень по 42 см).
//
// Обратная связь (MPU6050, 0x68): крен и тангаж корпуса считаются фильтром
// (гироскоп + акселерометр). Крен поправляет голеностопы и бёдра, шагающая
// нога ставится в сторону, куда робот заваливается, а при наклоне вперёд или
// назад шаг удлиняется или укорачивается. Без датчика робот тоже идёт, но
// падает от лёгкого толчка.
//
// Привод: PCA9685 по I2C (0x40), каналы 0–4 левая нога, 5–9 правая:
// крен бедра, бедро, колено, голеностоп вперёд-назад, голеностоп крен;
// 10 и 11 — поворот левого и правого бедра (TURN — поворот за шаг).
// Значение «прямо» для write(): колено 0, бедро 40, остальные 90.

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

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

// --- размеры и параметры походки -------------------------------------------
const float L1 = 0.42, L2 = 0.42;   // бедро, голень, м
const float HIP = 0.06;             // полуширина таза, м
const float H = 0.87;               // высота таза над полом, м
const float ANKLE = 0.08;           // высота голеностопа над полом, м
const float STEP = 0.12;            // длина шага, м
const float T_STEP = 0.5;           // длительность шага, с
const float SWAY = 0.02;            // качание таза вбок, м
const float LIFT = 0.05;            // подъём стопы, м
const float DOUBLE = 0.2;           // доля шага на двух ногах
const float T_START = 0.5;          // постоять перед ходом, с
const float RAMP = 2;               // шагов на разгон
const float TURN = 0;               // поворот за шаг, градусы (+ влево); 0 — прямо
const float K_HEAD = 0.3;           // выравнивание курса: доля ошибки курса за шаг

// Обратная связь: поправки в градусах на градус крена.
const float K_ANKLE = -0.3;         // голеностопы (крен)
const float K_HIP = 0.3;            // бёдра (крен)
const float K_FOOT = 0.3;           // шагающая стопа вбок, м на радиан крена
const float K_STEP = 0.6;           // шаг длиннее при наклоне вперёд, м на радиан
const float STEP_ADJ = 0.08;        // предел подстройки шага, м

const float ZERO[6] = {90, 40, 0, 90, 90, 90};   // 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();
}

// Угол сустава (градусы от «прямо») → импульс → канал PCA9685.
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();
}

// Нога по положению стопы относительно таза (м): dx вбок, fh вниз, fz вперёд.
// corrA — поправка голеностопа (крен), corrH — бедра (крен), градусы.
// yaw — поворот бедра (градусы, + носок влево) на канале yawCh: стопа
// задана в осях таза, нога под приводом поворота повёрнута — переводим.
void leg(uint8_t c0, uint8_t yawCh, float yaw, float dx, float fh, float fz, float corrA, float corrH) {
  float a = -yaw / R, ca = cos(a), sa = sin(a);
  float dx2 = dx * ca + fz * sa;
  fz = -dx * sa + fz * ca;
  dx = dx2;
  setJoint(yawCh, 5, yaw);
  float phi = atan2(dx, fh);                       // крен бедра
  float lv = sqrt(fh * fh + dx * dx);
  float d = sqrt(lv * lv + 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, lv) + acos((L1 * L1 + d * d - L2 * L2) / (2 * L1 * d));  // бедро
  setJoint(c0 + 0, 0, phi * R + corrH);
  setJoint(c0 + 1, 1, th * R);
  setJoint(c0 + 2, 2, k * R);
  setJoint(c0 + 3, 3, (k - th) * R);               // стопа параллельно полу
  setJoint(c0 + 4, 4, -phi * R + corrA);
}

// --- датчик наклона ---------------------------------------------------------
float roll = 0;                     // крен корпуса, градусы (+ — вправо)
float pitch = 0;                    // тангаж, градусы (+ — вперёд)
float heading = 0;                  // курс от старта, градусы (+ — влево)
unsigned long lastImu = 0;

int16_t rd16() { int16_t h = Wire.read(); return (h << 8) | Wire.read(); }

void readImu() {
  Wire.beginTransmission(MPU); Wire.write(0x3B); Wire.endTransmission(false);
  Wire.requestFrom(MPU, (uint8_t)14);
  int16_t ax = rd16(), ay = rd16(), az = rd16(); rd16();
  int16_t gx = rd16(), gy = rd16(), gz = rd16();
  unsigned long now = micros();
  float dt = lastImu ? (now - lastImu) * 1e-6 : 0;
  lastImu = now;
  // Ось X датчика смотрит вперёд: крен — поворот вокруг неё. Акселерометр
  // на шагающем роботе шумит (удары стоп), поэтому он лишь медленно
  // подтягивает угол, а быстрые изменения даёт гироскоп.
  heading += gz / 131.0 * dt;                  // курс: только гироскоп (ось Z датчика — вверх)
  if (az < 4000) return;                       // удар или свободное падение — не верим
  float accRoll = atan2((float)ay, (float)az) * R;
  float accPitch = atan2(-(float)ax, (float)az) * R;
  float w = millis() < 400 ? 0.1 : 0.005;      // пока робот садится на ноги — быстрее
  roll = (1 - w) * (roll + gx / 131.0 * dt) + w * accRoll;
  pitch = (1 - w) * (pitch + gy / 131.0 * dt) + w * accPitch;
}

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);
  reg(MPU, 0x6B, 0);           // разбудить датчик
}

// Шаги с памятью: стопа встаёт туда, куда её довели, а не по расписанию —
// иначе подстройка шага по наклону на следующем шаге «отыгралась» бы назад.
int k = -1;                     // номер текущего шага
float zl = 0, zr = 0;           // где стоят стопы (вперёд, м)
float zFrom = 0, zTo = 0;       // шагающая нога: откуда и куда по плану
float adj = 0;                  // подстройка цели по наклону, м
float course = 0;               // курс, который должен быть, градусы
float turnStep = 0;             // поворот в этом шаге, градусы

void loop() {
  readImu();
  float t = millis() / 1000.0;

  float px = 0, pz = 0;         // таз: вбок (+ влево), вперёд, м
  float ZL = zl, ZR = zr;       // стопы сейчас
  float yl = 0, yr = 0;         // подъём стоп, м
  float ql = 0, qr = 0;         // поворот бёдер, градусы

  float s = t - T_START;
  if (s > 0) {
    int kk = (int)(s / T_STEP);                // номер шага
    float u = (s - kk * T_STEP) / T_STEP;      // 0…1 внутри шага
    if (kk != k) {                             // новый шаг
      if (k >= 0) { if (k % 2 == 0) zr = zTo + adj; else zl = zTo + adj; }
      k = kk;
      float stance = (k % 2 == 0) ? zl : zr;
      zFrom = (k % 2 == 0) ? zr : zl;
      zTo = stance + STEP * min(1.0, k / RAMP);
      adj = 0;
      // Курс: задуманный копится по TURN, ошибку по гироскопу выбираем
      // частью на каждом шаге — робот не уводит в сторону.
      float t1 = TURN * min(1.0, k / RAMP);
      course += t1;
      turnStep = constrain(t1 + K_HEAD * (course - heading), -15, 15);
    }
    float dir = (k % 2 == 0) ? 1 : -1;         // +1 опора на левой
    float stance = dir > 0 ? zl : zr;
    px = dir * SWAY * min(1.0, (k + u) / RAMP) * sin(M_PI * u);
    float v = (u - DOUBLE / 2) / (1 - DOUBLE);
    // Пока нога в воздухе, цель сдвигается: клонится вперёд — шаг длиннее.
    if (v > 0 && v < 0.8) adj = constrain(K_STEP * pitch / R, -STEP_ADJ, STEP_ADJ);
    float target = zTo + adj;
    float lift = (v > 0 && v < 1) ? LIFT * sin(M_PI * v) : 0;
    float zs = zFrom + (target - zFrom) * smooth(v);
    if (dir > 0) { ZR = zs; yr = lift; } else { ZL = zs; yl = lift; }
    // Поворот: пока одна нога в воздухе, опорное бедро доворачивается на
    // TURN (корпус поворачивается над стопой), шагающее — в обратную сторону,
    // чтобы стопа встала уже по новому курсу. На двух ногах углы стоят.
    float qs = turnStep / 2 - turnStep * smooth(v), qw = -qs;
    if (dir > 0) { ql = qs; qr = qw; } else { qr = qs; ql = qw; }
    pz = (zFrom + stance) / 2 + (target - zFrom) / 2 * u;
  }

  float fx = -K_FOOT * roll / R;              // крен вправо → стопа правее
  float cA = K_ANKLE * roll, cH = K_HIP * roll;
  leg(0, 10, ql, HIP - (px + HIP) + (yl > 0 ? fx : 0), H - (ANKLE + yl), ZL - pz, cA, cH);
  leg(5, 11, qr, -HIP - (px - HIP) + (yr > 0 ? fx : 0), H - (ANKLE + yr), ZR - pz, cA, cH);
}