Целью данного урока является преобразование консольной программы, написанной на прошлом уроке в программу для управляющего устройства, которая устанавливает ногу собаки в точку с заданными координатами.
Последовательность выполнения задания:
1. В программу для Arduino берем функции two_angles() и rad_to_microsec() из программы, написанной на прошлом занятии. Создаем объекты типа Servo и привзываем серводвигатели к пинам, с помощью функции attach().
#include
Servo myServo[2];
int pin[2]={2,4};
void setup ()
{
myServo[0].attach(pin[0]);
myServo[1].attach(pin[1]);
}
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[0]=asin(y/B)-asin((pow(L1,2)+pow(B,2)-pow(L2,2))/(2*L1*B));
if (x<=0)
q[0]=Pi/2+acos(y/B)-asin((pow(L1,2)+pow(B,2)-pow(L2,2))/(2*L1*B));
q[1]=(Pi/2)+asin((pow(L1,2)+pow(L2,2)-pow(B,2))/(2*L1*L2));
}
void loop ()
{
}
2. Задаем ограничения поворота серводвигателей (минимальный и максимальный угол, на который может повернуться серводвигатель). Создаем функцию allow_value(int minval, int maxval, int angle), которая проверяет находится ли значение angle в допустимом диапазоне между minval и maxval.
#include
Servo myServo[2];
int pin[2]={2,4};
int extremvalue[2]={600,2300};
int q[2];
void setup ()
{
myServo[0].attach(pin[0]);
myServo[1].attach(pin[1]);
}
int allow_value(int minval, int maxval, int angle ){
if (angle>maxval)
return maxval;
else
if (angle<minval)
return minval;
else
return angle;
}
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[0]=asin(y/B)-asin((pow(L1,2)+pow(B,2)-pow(L2,2))/(2*L1*B));
if (x<=0)
q[0]=Pi/2+acos(y/B)-asin((pow(L1,2)+pow(B,2)-pow(L2,2))/(2*L1*B));
q[1]=(Pi/2)+asin((pow(L1,2)+pow(L2,2)-pow(B,2))/(2*L1*L2));
}
void loop (){
}
3. Создаем функцию addangle(), которая прибавляет(или отнимает) значения, вычисленные функцией two_angles() к нулевым положениям серводвигателей (когда нога совпадает с осью У). Затем создаем функцию, topoint(x,y), которая непосредственно позиционирует ногу в точку с заданными координатами.
#include
Servo myServo[2];
int pin[2]={2,4};
int val_imp[2]={600,2300};
int servodir[2]={-1,1};
int zero[2]={2150,1000};
float leglong[2]={66,120};
int Q[2];
void setup ()
{
myServo[0].attach(pin[0]);
myServo[1].attach(pin[1]);
}
int allow_value(int minval, int maxval, int angle ){
if (angle>maxval)
return maxval;
else
if (angle<minval)
return minval;
else
return angle;
}
int addangle(int angle, int zeropoint, int servodir)
{
return angle*servodir+zeropoint;
}
void topoint(float x, float y)
{
two_angles(leglong[0],leglong[0],x,y);
for (int j=0;j<2;j++)
{
Q[j]=rad_to_micro(Q[j]);
Q[j]=addangle(Q[j],zeropoints[j],servodirect[j]);
Q[j]=allowvalue(Q[j],legextrem[j]);
legservo[j].writeMicroseconds(Q[j]);
}
}
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)
{
/*
Q1 — угол между вертикалью и первым звеном
Q2 — угол между продолжением звена 1 и звеном 2
*/
float B;
float const Pi=3.1415;
B=sqrt(pow(x,2)+pow(y,2));
if (x>0)
Q[0]=asin(y/B)-asin((pow(L1,2)+pow(B,2)-pow(L2,2))/(2*L1*B));
if (x<=0)
Q[0]=Pi/2+acos(y/B)-asin((pow(L1,2)+pow(B,2)-pow(L2,2))/(2*L1*B));
Q[1]=(Pi/2)+asin((pow(L1,2)+pow(L2,2)-pow(B,2))/(2*L1*L2));
}
void loop (){
topoint (80, 120);
}
