roboforum.ruТехнический форум по робототехнике. |
|
|
Scorpio писал(а):Dmitry__ порой производит впечатление умного, образованного человека
#define L1 100 //длина первого плеча манипулятора в мм.
#define L2 100 //длина второго плеча манипулятора в мм.
#define Xmax 150
#define Ymax 150
//Функция выставляет servo1 servo2, на точку с координатами X,Y
void SetServosToPoint(float X, float Y)
{
if(abs(X)>Xmax || Y>Ymax || Y<0) return;
float L=sqrt(X*X+Y*Y);
float a1=acos(X/L);
float a2=acos((L*L+L1*L1-L2*L2)/2*L*L1);
float A=a1+a2;
float b1=PI/2-a1;
float b2=acos((L2*L2+L*L-L1*L1)/2*L2*L);
float B=PI/2-(b1+b2);
servo1.writeMicroseconds((int) map(A, 0, PI, 500, 2500));
servo2.writeMicroseconds((int) map(B, 0, PI, 500, 2500));
}
Scorpio писал(а):Не заводись. Для меня ты незыблемый авторитет.
Scorpio писал(а):Будет работать вилимо только в левом квадранте, в правом наверное по другому надо считать.
Dmitry__ писал(а):Ну скажи, где ошибаюсь
#define L1 100 //длина первого плеча манипулятора в мм.
#define L2 100 //длина второго плеча манипулятора в мм.
#define Xmax 150
#define Ymax 150
//Функция выставляет servo1 servo2, на точку с координатами X,Y
//X<0 для левого квадранта, X>0 для правого
void SetServosToPoint(float X, float Y)
{
if(abs(X)>Xmax || Y>Ymax || Y<0 || X==0) return;
float L=sqrt(X*X+Y*Y);
float a1=acos(abs(X)/L);
float a2=acos((L*L+L1*L1-L2*L2)/2*L*L1);
float A=a1+a2;
if(X>0)A=PI-A;
float b1=PI/2-a1;
float b2=acos((L2*L2+L*L-L1*L1)/2*L2*L);
float B=PI/2-(b1+b2);
if(X>0)B+=PI/2;
servo1.writeMicroseconds((int) map(A, 0, PI, 500, 2500));
servo2.writeMicroseconds((int) map(B, 0, PI, 500, 2500));
}
#define L1 100 //длина первого плеча манипулятора в мм.
#define L2 100 //длина второго плеча манипулятора в мм.
#define Xmax 150
#define Ymax 150
//Функция выставляет servo1 servo2, на точку с координатами X,Y
//X<0 для левого квадранта, X>0 для правого
void SetServosToPoint(float X, float Y)
{
if(abs(X)>Xmax || Y>Ymax || Y<0 || X==0) return;
float L=sqrt(X*X+Y*Y);
float a1=acos(abs(X)/L);
float a2=acos((L*L+L1*L1-L2*L2)/2*L*L1);
float A=a1+a2;
if(X>0)A=PI-A;
float b1=PI/2-a1;
float b2=acos((L2*L2+L*L-L1*L1)/2*L2*L);
float B=PI/2-(b1+b2);
if(X>0)B+=PI/2;
//servo1.writeMicroseconds((int) map(A, 0, PI, 500, 2500));
//servo2.writeMicroseconds((int) map(B, 0, PI, 500, 2500));
Serial.print("A= ");
Serial.println(map(A, 0, PI*2, 0, 360));
Serial.print("B= ");
Serial.println(map(B, 0, PI*2, 0, 360));
}
dimamichev писал(а):Так что?,сервы отключить?, и в монитор порта смотреть, подставляя разные х и y?.
///////////// МАНИПУЛЯТОР ДЛЯ ГРАФИЧЕСКИХ ИЗОБРАЖЕНИЙ скетч СЕТКА КООРДИНАТНАЯ ДЕКАРТОВА ПРОТО //////////////////////////////
#include <Servo.h> // подключение библиотеки Servo
#define L1 65 //длина первого плеча манипулятора в мм.
#define L2 65 //длина второго плеча манипулятора в мм.
#define Xmax 100
#define Ymax 100
void setup()
{
Serial.begin(9600);
}
//////////////////////////////////////////////////////////////////////////////////////////////
void loop()
/////////////////////////////////////////////////////////////////////////
{
SetServosToPoint (-100,100);delay(10000);
}
//Функция выставляет servo1 servo2, на точку с координатами X,Y
//X<0 для левого квадранта, X>0 для правого
void SetServosToPoint(float X, float Y)
{
if(abs(X)>Xmax || Y>Ymax || Y<0 || X==0) return;
float L=sqrt(X*X+Y*Y);
float a1=acos(abs(X)/L);
float a2=acos((L*L+L1*L1-L2*L2)/2*L*L1);
float A=a1+a2;
if(X>0)A=PI-A;
float b1=PI/2-a1;
float b2=acos((L2*L2+L*L-L1*L1)/2*L2*L);
float B=PI/2-(b1+b2);
if(X>0)B+=PI/2;
//servo1.writeMicroseconds((int) map(A, 0, PI, 500, 2500));
//servo2.writeMicroseconds((int) map(B, 0, PI, 500, 2500));
Serial.print("A= ");
Serial.println(map(A, 0, PI*2, 0, 360));
Serial.print("B= ");
Serial.println(map(B, 0, PI*2, 0, 360));
}
Scorpio писал(а):Потому что 2пи.
Надо тогда по ходу расчета выводить. Подозреваю что acos не считает дуня
Вернуться в Arduino и другие Xduino
Сейчас этот форум просматривают: нет зарегистрированных пользователей и гости: 1