Steuerung eines Hexapoden mittels C
Hallo,
ich habe da ein kleines Problem bei dem ich mir nicht sicher bin ob ich hier richtig bin.
Wenn ja schreit mich einfach an.
Mein Versuch ist die Ansteuerung meiner 6 Servos. Das klappt auch, leider kann ich diese aber nicht gleichzeitig steuern.
Das macht mein Konzept des Hexapoden leider etwas schlecht.
Hier mal mein Code. Die Matrix wird aus einer einfachen .txt Datei gelesen. Falls mir jemand helfen könnte den Code so zu schreiben, dass alle Servos gleichzeitig ihrer arbeit nachgehen wäre ich echt dankbar.
Code:
#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>
#ifdef _WIN32
#define O_NOCTTY 0
#else
#include <termios.h>
#endif
// Gets the position of a Maestro channel.
// See the "Serial Servo Commands" section of the user's guide.
int maestroGetPosition(int fd, unsigned char channel)
{
unsigned char command[] = {0x90, channel};
if(write(fd, command, sizeof(command)) == -1)
{
perror("error writing");
return -1;
}
unsigned char response[2];
if(read(fd,response,2) != 2)
{
perror("error reading");
return -1;
}
return response[0] + 256*response[1];
}
// Sets the target of a Maestro channel.
// See the "Serial Servo Commands" section of the user's guide.
// The units of 'target' are quarter-microseconds.
int maestroSetTarget(int fd, unsigned char channel, unsigned short target)
{
unsigned char command[] = {0x84, channel, target & 0x7F, target >> 7 & 0x7F};
if (write(fd, command, sizeof(command)) == -1)
{
perror("error writing");
return -1;
}
return 0;
}
int main()
{
// Open the Maestro's virtual COM port.
const char * device = "\\\\.\\USBSER000"; // Windows, "\\\\.\\COM6" also works
//const char * device = "/dev/ttyACM0"; // Linux
//const char * device = "/dev/cu.usbmodem00034567"; // Mac OS X
int fd = open(device, O_RDWR | O_NOCTTY);
if (fd == -1)
{
perror(device);
return 1;
}
#ifndef _WIN32
struct termios options;
tcgetattr(fd, &options);
options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
options.c_oflag &= ~(ONLCR | OCRNL);
tcsetattr(fd, TCSANOW, &options);
#endif
#define m 6
int matrix[m][m] ={0};
{
FILE *fp;
int i,j,k,l,b,v;
int position= maestroGetPosition(fd, 0);
int target;
fp= fopen("Werte", "r");
if (fp==NULL)
{
printf("Can`t open file");
exit(-1) ;
}
for(i=0; i<m;i++)
{
fscanf(fp,"%d",& matrix[0][i]);
fflush( stdout );
sleep(3);
target=matrix[0][i];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 0, target);
}
for(j=0; j<m;j++)
{
fscanf(fp,"%d",& matrix[1][j]);
fflush( stdout );
sleep(2);
target=matrix[1][j];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 1, target);
}
for(k=0; k<m;k++)
{
fscanf(fp,"%d",& matrix[2][k]);
fflush( stdout );
sleep(2);
target=matrix[2][k];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 2, target);
}
for(l=0; l<m;l++)
{
fscanf(fp,"%d",& matrix[3][l]);
fflush( stdout );
sleep(2);
target=matrix[3][l];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 3, target);
}
for(b=0; b<m;b++)
{
fscanf(fp,"%d",& matrix[4][b]);
fflush( stdout );
sleep(2);
target=matrix[4][b];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 4, target);
}
for(v=0; v<m;v++)
{
fscanf(fp,"%d",& matrix[5][v]);
fflush( stdout );
sleep(2);
target=matrix[5][v];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 5, target);
}
close(fd);
}
return 0;
}
Ich weiß, ist nicht gerade die beste Lösung aber meine Kenntnisse in C sind nicht die besten.
Liste der Anhänge anzeigen (Anzahl: 1)
@shedepe
Ja das habe ich auch schon mitbekommen. Es ist aber wirklich nicht mein Ziel hier einen fertigen Code zu bekommen. Bringt mich ja nicht weiter. Mit dem eingrenzen hast du ja recht, dadurch bekommt jeder erstmal eine Ansage die bestimmt notwendig ist und man siebt erstmal die Leute aus, die keine Lust haben sich mit dem Fall zu beschäftigen.
Ja habe es wirklich schlecht eingegrenzt. Mein Fehler :)
Ich habe gestern noch etwas probiert und habe eine nicht sehr schöne aber dennoch funktionierende Lösung gefunden.(Sitze gerade dran und will sie verschönern, denn gefallen tut mir das gerade echt nicht)
Code:
fp= fopen("Werte", "r");
if (fp==NULL)
{
printf("Can`t open file");
exit(-1) ;
}
for(i=0;i<m;i++)
{
sleep(4);
fscanf(fp,"%d",& matrix[0][i] );
fflush( stdout );
fscanf(fp,"%d",& matrix[1][i] );
fflush( stdout );
fscanf(fp,"%d",& matrix[2][i] );
fflush( stdout );
fscanf(fp,"%d",& matrix[3][i] );
fflush( stdout );
fscanf(fp,"%d",& matrix[4][i] );
fflush( stdout );
fscanf(fp,"%d",& matrix[5][i] );
fflush( stdout );
target=matrix[0][i];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 0, target);
target=matrix[1][i];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 1, target);
target=matrix[2][i];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 2, target);
target=matrix[3][i];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 3, target);
target=matrix[4][i];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 4, target);
target=matrix[5][i];
printf("Setting target to %d (%d us).\n", target, target/4);
maestroSetTarget(fd, 5, target);
}
}
close(fd);
getchar();
return 0;
}
Ich lese nun einfach die Zeile einzeln ein und gebe sie zu den Servos. Also bekommt jeder Servo eine Spalte und eine Zeile entspricht einer Bewegung. So habe ich den Tipp von dir verstanden bzw. so konnte ich es umsetzen. Und siehe da, es läuft. Danke dir. Ich gehe zwar von aus dass du es anders gemeint hattest aber es hat mich zu einer vorläufigen Lösung geführt :D @shedepe
Ich will aber die Matrix nun komplett einlesen und dann wie du schon gesagt hattest einzeln weitergeben.
Die Pause muss ich einfügen da ich keine Geschwindigkeiten vorgebe. Das wird ein neues Kapitel.
Was habe ich schon gemacht:
1. Konstruktion steht/Prototyp
Anhang 29855
2. Jacobi-Matrix steht(Muss ich aber nochmal mit meinem Prof. schauen. Bringt nicht das was sie sollte :( Achso wir nehmen, um den ganzen Fall zu vereinfachen an, sie ist linear)
3. Vorläufiger Code steht(nicht schön aber bestimmt sehr selten)
Ziele/Herausforderungen:
1. Funktionierende Jacobi-Matrix
2. Eingabe der gewünschten Position über Konsole
