Hello friends, I'm doing a SCARA arm project, just that I have a problem. When I try to automate it, the degrees I get are added as if I did not respect the point of origin.
for this reason I want to take the difference of angles so that the trajectory does not result in other coordinates
int retardo = 5;
int dato_rx;
int dato_rx2;
int numero_pasos = 0;
double valor=0;
double Q1g,Q2g;
double Pi = 3.14159;
void setup() {
Serial.begin(9600);
pinMode(11, OUTPUT);
pinMode(10, OUTPUT);
pinMode(9, OUTPUT);
pinMode(8, OUTPUT);
pinMode(7, OUTPUT);
pinMode(6, OUTPUT);
pinMode(5, OUTPUT);
pinMode(4, OUTPUT);
}
double radianesAGrados(double radianes)
{
return radianes * 180.0 / Pi;
}
void loop() {
delay(1000);
//double x_c = 24;
//double y_c = 0;
// Radio de la figura
//double r = 0.5;
//Ecuacion de la figura para su trazo en coordenadas X Y Z
//double x = x_c + r * sin(millis());
//double y = y_c + r * cos(millis());
double x = 24;
double y = 0;
double d1 = 0;
double l2 = 14.33;
double l3 = 12.2;
double c2 = ((pow(x, 2)) + (pow(y, 2)) - (pow(l2, 2)) - (pow(l3, 2))) / (2 * l2 * l3);
double Q2 = atan2(-sqrt(1 - (pow(c2, 2))), c2);
double Q1 = (atan2(y, x)) - atan2(l3 * sin(Q2), l2 + l3 * cos(Q2));
Q1g = radianesAGrados(Q1);
Q2g = radianesAGrados(Q2);
dato_rx = Q1g;
delay(retardo);
Serial.print(dato_rx);
// Serial.print(" Grados Q1 ");
delay(retardo);
dato_rx = (dato_rx * 0.477777);
dato_rx2 = Q2g;
//Serial.print(dato_rx2);
//Serial.println(" Grados Q2");
dato_rx2 = (dato_rx2 * 0.16444);
delay(retardo);
while (dato_rx > numero_pasos) {
paso_izq();
numero_pasos = numero_pasos + 1;
}
while (dato_rx < numero_pasos) {
paso_der();
numero_pasos = numero_pasos - 1;
}
apagado();
while (dato_rx2 > numero_pasos) {
paso_izq2();
numero_pasos = numero_pasos + 1;
}
while (dato_rx2 < numero_pasos) {
paso_der2();
numero_pasos = numero_pasos - 1;
}
apagado2();
}
void paso_der() {
digitalWrite(11, LOW);
digitalWrite(10, LOW);
digitalWrite(9, HIGH);
digitalWrite(8, HIGH);
delay(retardo);
digitalWrite(11, LOW);
digitalWrite(10, HIGH);
digitalWrite(9, HIGH);
digitalWrite(8, LOW);
delay(retardo);
digitalWrite(11, HIGH);
digitalWrite(10, HIGH);
digitalWrite(9, LOW);
digitalWrite(8, LOW);
delay(retardo);
digitalWrite(11, HIGH);
digitalWrite(10, LOW);
digitalWrite(9, LOW);
digitalWrite(8, HIGH);
delay(retardo);
}
void paso_izq() {
digitalWrite(11, HIGH);
digitalWrite(10, HIGH);
digitalWrite(9, LOW);
digitalWrite(8, LOW);
delay(retardo);
digitalWrite(11, LOW);
digitalWrite(10, HIGH);
digitalWrite(9, HIGH);
digitalWrite(8, LOW);
delay(retardo);
digitalWrite(11, LOW);
digitalWrite(10, LOW);
digitalWrite(9, HIGH);
digitalWrite(8, HIGH);
delay(retardo);
digitalWrite(11, HIGH);
digitalWrite(10, LOW);
digitalWrite(9, LOW);
digitalWrite(8, HIGH);
delay(retardo);
}
void apagado() {
digitalWrite(11, LOW);
digitalWrite(10, LOW);
digitalWrite(9, LOW);
digitalWrite(8, LOW);
}
void paso_izq2() {
digitalWrite(7, LOW);
digitalWrite(6, LOW);
digitalWrite(5, HIGH);
digitalWrite(4, HIGH);
delay(retardo);
digitalWrite(7, LOW);
digitalWrite(6, HIGH);
digitalWrite(5, HIGH);
digitalWrite(4, LOW);
delay(retardo);
digitalWrite(7, HIGH);
digitalWrite(6, HIGH);
digitalWrite(5, LOW);
digitalWrite(4, LOW);
delay(retardo);
digitalWrite(7, HIGH);
digitalWrite(6, LOW);
digitalWrite(5, LOW);
digitalWrite(4, HIGH);
delay(retardo);
}
void paso_der2() {
digitalWrite(7, HIGH);
digitalWrite(6, HIGH);
digitalWrite(5, LOW);
digitalWrite(4, LOW);
delay(retardo);
digitalWrite(7, LOW);
digitalWrite(6, HIGH);
digitalWrite(5, HIGH);
digitalWrite(4, LOW);
delay(retardo);
digitalWrite(7, LOW);
digitalWrite(6, LOW);
digitalWrite(5, HIGH);
digitalWrite(4, HIGH);
delay(retardo);
digitalWrite(7, HIGH);
digitalWrite(6, LOW);
digitalWrite(5, LOW);
digitalWrite(4, HIGH);
delay(retardo);
}
void apagado2() {
digitalWrite(7, LOW);
digitalWrite(6, LOW);
digitalWrite(5, LOW);
digitalWrite(4, LOW);
}
I'm controlling it with 2 steppers.
Please omit the comments.