План
1. Библиотека Servo
2. Адаптация обратной задачи кинематики для собаки
Теоретическая часть
1. Библиотека Servo
Для программного управления серводвигателями существует библиотека Servo. Для серводвигателя заводится переменная типа Servo. Над данным серводвигателем можно осуществлять различные действия с помощью конструкции вида:
имя_переменной.имя функции(список параметров);
Ниже перечислены функции, входящие в библиотеку Servo.
attach() — задает номер пина для определенного серводвигателя. Функция может иметь два набора параметров:
servoname.attach(pin)
servoname.attach(pin, min, max), где
pin — номер пина, к которому подсоединен сервопривод
min и max — минимальная и максимальная длины импульсов в микросекундах, которые соответствуют min — 0 градусов, а max -180 градусов. По умолчанию min=544 мкс и max=2400 мкс.
write() — посылает сервоприводу команду повернуться на заданный в градусах угол. Синтаксис следующий: servoname.write(angle), где angle — угол, на который должен повернуться сервопривод.
writeMicroseconds() — посылает сервоприводу импульс заданной длины.
servoname.writeMicroseconds(uS), где uS — длина импульса в микросекундах.
read() — возвращает текущий угол сервопривода в градусах.
angle=servoname.read();
attached() — проверяет, был ли присоединен серводвигатель к какому-то пину. Возвращает истину, если присоединен, и ложь в обратном случае.
setted=servoname.attached()
detach() — функция, противоположная функции attach(). Отсоединяет сервопривод от пина. servoname.detach().
Практическая часть
Адаптировать решенную на прошлом занятии, обратную задачу кинематики для движения ноги собаки. Написать функцию, которая вычисляет углы между звеньями. Функция имеет параметры: длины звеньев, координаты точки, номер ноги. Проверить правильность работы в консоли на контрольном примере.
На рисунке 1 представлена кинематическая схема ноги собаки.
Рисунок 1 — Кинематическая схема ноги собаки
Последовательность работы:
1. Измерить длины звеньев для передней и задней ног. Создать с данными величинами двумерный массив [4][2], где 4 — количество ног, 2 — количество звеньев.
Схема собаки с нумерацией ног представлена на рисунке 2.
Рисунок 2 — Схема собаки
Нумерация звеньев начинается от туловища.
float leglong[4][2]={
{66,120}, //0 нога
{82,115}, // 1 нога
{82,115}, // 2 нога
{66,120} //3 нога
};
2. Модифицировать программу, написанную на прошлом уроке под собаку.
#include <iоstream>
#include <mаth.h>
using namespace std;
const float Pi=3.1415;
float Q[4][2];
float leglong[4][2]={
{66,120}, //0 нога
{82,115}, // 1 нога
{82,115}, // 2 нога
{66,120} //3 нога
};
int rad_to_micro(float q1)
{
int x;
x=1856*q1/3.14;
return x;
}
void two_angles(double L1, double L2, double x, double y,int leg)
{
/*
Q1 — угол между вертикалью и первым звеном
Q2 — угол между продолжением звена 1 и звеном 2
*/
float B;
float const Pi=3.1415;
B=sqrt(pow(x,2)+pow(y,2));
if (x>0)
Q[leg][0]=asin(y/B)-asin((pow(L1,2)+pow(B,2)-pow(L2,2))/(2*L1*B));
if (x<=0)
Q[leg][0]=Pi/2+acos(y/B)-asin((pow(L1,2)+pow(B,2)-pow(L2,2))/(2*L1*B));
Q[leg][1]=(Pi/2)+asin((pow(L1,2)+pow(L2,2)-pow(B,2))/(2*L1*L2));
}
int main()
{
float x,y;
cout<< «Введите певую координату»<< endl;
cin>>x;
cout<< «Введите вторую координату»<< endl;
cin>>y;
for (int j=0;j<4;j++)
for (int i=0;i<2;i++)
{
two_angles(leglong[j][0],leglong[j][1],x,y,j);
cout<<» Угол=»<<Q[j][i]<<‘ ‘;
}
return 0;
}
