Мне нужно сделать код для перемещения одного плеча робота, который содержит три луча на основе определенной заданной (x,y)
точки, а theta0
должен начинаться с 0
. Затем xd
и yd
рассчитываются на основе следующего:
xd = x - L cos(theta)
yd = y - L cos(theta)
где L
- первый луч единственного плеча и где theta
начинается с 0
и x
и y
вводятся. Теперь, основываясь на рассчитанных xd
и yd
, они будут использоваться для расчета theta1
и theta2
. Эти три угла должны быть отправлены на серводвигатели.
Однако, когда я вхожу в x
и y
, ничего не отображается! Если вы знаете некоторые полезные ссылки, которые могут помочь мне в этой проблеме, это будет действительно оценено.
Серийный код:
#include <Servo.h>
#include <String.h>
#include <math.h>
Servo motor;
String content = "";
char character;
int yindex;
char End ;
String x,y;
int xt, yt;
float L0=297;
float L1=198;
float L2=165;
float theta0=0;
float theta1, theta2;
float xd,yd;
float xc,yc;
void setup()
{
Serial.begin(9600);
motor.attach(9,660,2600);
}
void loop()
{
if (Serial.available ()>0)
{
while(Serial.available())
{
character = Serial.read();
content.concat(character);
content += character;
Serial.println (content);
End=content.length();
yindex= content.indexOf("y");
x=content.substring(1,yindex);
y=content.substring(yindex+1, End);
Serial.println (x);
Serial.println (y);
xt=x.toInt();
Serial.println(xt);
yt=y.toInt();
Serial.println(yt);
for ( theta0=0; theta0<180; theta0+=0.1)
{
xd= xt-L0*cos(theta0);
yd= yt-L0*cos(theta0);
theta2 = acos((sq(xd)+sq(yd)- sq(L1)-sq(L2))/(2*L1*L2));
theta1 = asin((L2*sin(theta2))/sqrt(sq(xd)+sq(yd)))+atan(yd/xd);
Serial.println("The value of theta2 is : ");
Serial.println(theta2);
Serial.println("The value of theta1 is : ");
Serial.println(theta1);
xc = L0*cos(theta0) + L1*cos(theta1) + L2*cos(theta2);
yc= L0*sin(theta0) + L1*sin(theta1) + L2*sin(theta2);
Serial.println("The value of the calculated x is : ");
Serial.println(xc);
Serial.println("The value of the calculated y is : ");
Serial.println(yc);
Serial.println("The value of theta0:");
Serial.println(theta0);
theta0=theta0*(180/3.14);
theta1=theta1*(180/3.14);
theta2=theta2*(180/3.14);
if (theta1<=-180 && theta1>=0 && theta2>=0 && theta2 <=-180)
{
break;
}
}
}
}
content="";
}
Здесь много проблем. Вы должны подойти к подобным проблемам, протестировав каждый шаг решения за раз, так что вы не получите большой беспорядок. Во-первых, вам нужно убедиться, что вы правильно читаете строку.
Эта часть кода работает некорректно, потому что {}
окружающий цикл while(Serial.available())
находится не в нужном месте. Кроме того, вы добавляете новый символ дважды, используя как +=
и concat
. Здесь простое исправление, чтобы получить строку правильно. Задержка заключается в том, чтобы разрешить доступ к следующему байту, чтобы он не вышел из цикла преждевременно.
content="";
while (Serial.available() < 1) {}
while (Serial.available())
{
character = Serial.read();
content += character;
delay(10);
}
Serial.print("content = "); Serial.println (content);
Затем вам нужно проанализировать значения для x и y из строки. Здесь есть пара проблем. Я не думаю, что вы хотите использовать целые числа для x и y, поэтому вам нужен способ интерпретировать строку как float. Вот как вы можете анализировать значения в поплавках.
End=content.length();
yindex= content.indexOf("y");
x=content.substring(1,yindex);
y=content.substring(yindex+1, End);
Serial.print("x = "); Serial.println (x);
Serial.print("y = "); Serial.println (y);
char xbuf[10] = "";
x.toCharArray(xbuf, 10);
xt = atof(xbuf);
Serial.print("xt = "); Serial.println(xt);
char ybuf[10] = "";
y.toCharArray(ybuf, 10);
yt = atof(ybuf);
Serial.print("yt = "); Serial.println(yt);
Это должно быть достаточной помощью, чтобы заставить вас двигаться вперед. В вашей математике есть некоторые проблемы позже в коде, но это должно быть легче работать после того, как у вас есть рабочие заявления печати. Примечание. Формат, который этот код ожидает от последовательного монитора, имеет формат x0.34y0.678
где 0.34
- значение x
а 0.678
- значение y
.