а иначе смысл)) если интересно
float interpolation (float v0, float v1, float v2, float v3, float v4, float t) {
return v0
+ (- 3 * v4 + 16 * v3 - 36 * v2 + 48 * v1 - 25 * v0) / 3 * t
+ (22 * v4 - 112 * v3 + 228 * v2 - 208 * v1 + 70 * v0) / 3 * t * t
+ (- 48 * v4 + 224 * v3 - 384 * v2 + 288 * v1 - 80 * v0) / 3 * t * t * t
+ (32 * v4 - 128 * v3 + 192 * v2 - 128 * v1 + 32 * v0) / 3 * t * t * t * t;
}
это ф-ия вычисляет значение многочлена 4й степени при парметре t, меняющемся от 0 до 1, причем такая кривая будет проходить через точки v0, v1, v2, v3 и v4. По идее коэффициенты перед степенями можно предвычислить для каждой кривой заранее, но я не парился. На 3д просто эта ф-ия применяется для каждой координаты отдельно, в результате получается примерно следующее
дополнительным бонусом вы можете имитировать ускорение, т.к. ф-ия параметрическая, и некоторые точки могут быть друг к другу ближе чем другие - между ними движение будет медленнее при равнозначном приросте t, и наоборот для точек, которые дальше.
// поворот сервопривода до указанного угла без проверки переданного значения угла на вхождение в допустимый диапазон!
void set (float _angle) {
// вычисление длины импульса ШИМ согласно калибровочным значениям и требуему углу положения вала.
// Поскольку функция map() оперирует целыми числами, отбрасывая дробную часть,
// для более точных вычислений производится домножение значений всех углов на 100
uint16_t pulse = map(_angle * 100, minAngle * 100, maxAngle * 100, minAnglePulse, maxAnglePulse);
// вывод нового ШИМ
servo.writeMicroseconds(pulse);
}
так производится отправка нового шим, соответствующего переданному углу. Можно повлиять на направление вращения, т.е. на то, с какого положения будет отсчитываться угол - просто при калибровке minAnglePulse и maxAnglePulse получаются какими надо, хранятся в EEPROM, а функция map как раз масштабирует даже перевернутые диапазоны вполне корректно.
// установка когтя конечности в указанную позицию (цель), заданную в локальной системе координат
// возвращает true если удалось вычислить все значения углов,
// и они не выходят за пределы допустимых для соответствующих сервоприводов, иначе возвращает false
bool set (tDot &target) {
// считается, что конечность не должна "подламываться" под себя
// хотя механически такое движение возможно
if (target.x >= 0) {
// длина проекции на горизонтальную плоскость отрезка,
// проведенного от начала координат (сервопривод 0) до цели
float xoy_target_dl = hypot(target.x, target.y);
// угол поворота серво 0
// задает направление конечности в сторону целевой точки
float a0 = asin(target.y / xoy_target_dl) * deg;
// длина отрезка от серво 1 до цели
// не должна быть короче L2 - L1
// не должна быть длиннее L1 + L2
float s1_target_dl = hypot(xoy_target_dl - L0, target.z);
if ((s1_target_dl >= L2 - L1) && (s1_target_dl <= L1 + L2)) {
// угол наклона отрезка, проведенного от серво 1 до цели
// минус угол серво 1 в треугольнике,
// образованном L1, L2 и отрезком между серво 1 и целью
float a1 = (atan2((xoy_target_dl - L0), target.z) - acos((L1 * L1 + s1_target_dl * s1_target_dl - L2 * L2) / (2 * L1 * s1_target_dl))) * deg;
// угол поворота серво 2 в треугольнике,
// образованном L1, L2 и отрезком между серво 1 и целью
float a2 = acos((L1 * L1 + L2 * L2 - s1_target_dl * s1_target_dl) / (2 * L1 * L2)) * deg;
// проверка всех вычисленных углов на вхождение в допустимые диапазоны соответствующих сервоприводов
// воизбежание игнорирования ошибок кинематики, движение конечности будет осуществляться только в том случае, если все углы достижимы
if (servos[0].check(a0) && servos[1].check(a1) && servos[2].check(a2)) {
servos[0].set(a0);
servos[1].set(a1);
servos[2].set(a2);
return true;
} else return false;
} else return false;
} else return false;
}
тут самая сложная тригонометрия, но посути тоже ничего непостижимого… Длины плеч и номера серв у меня с нуля. Оффсета в горизонтальной плоскости нет - лапы расчертил так, чтобы именно по положениям линий плеч в одной плоскости лежали, т.е. чуть упрощенные вычисления. Я хз как объяснить, короче бывает у многих в проектах, что кончик ноги не получается разместить ровно на одной линии с сервоприводом, который вертикально вращает… вот этого косяка я избежал, просто чтоб еще один atan не вычислять.
Это так… выдержки из классов, для понимания что всё на самом деле просто и посильно для меги
