Steuerung eines Hexapoden mittels C

**Dani3l** · 17.02.2015, 11:09

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.

shedepe · 17.02.2015, 11:56

Um es mal so auszudrücken. Erwarte nicht in einem Forum, dass jemand für dich die Dreckarbeit bzw. die Fleißarbeit abnimmt und sei es nur die Fleißarbeit die darin steckt eine Programmiersprache besser zu lernen.
Vor allem solltest du zumindest dazu schreiben welches System du verwendest, was dort wie abläuft.

Nach kurz drauf schauen auf deinen code: Berechne zuerst alle Positionen, speicher sie dir und setz sie dann auf einen Rutsch.

**oberallgeier** · 17.02.2015, 12:03

... ich habe da ein kleines Problem ... Wenn ja schreit mich einfach an ...

Wegen kleiner Probleme wird hier niemand schreien.

... ich habe da ein kleines Problem ... // See the "Serial Servo Commands" section of the user's guide ...

Vielleicht mal in dieser (?gefunden?, ?copy´n paste?) Quelle nachsehen???

**Dani3l** · 17.02.2015, 12:22

Oh watte, hier geht's ja ab. Sind hier alle wie so???shedepe
Da hast wohl was falsch verstanden. Das ist gar nicht meine Absicht. Keine Ahnung aus welchen Zeilen du herausgeholt hast.
Denke damit sollte die Unterstellung beseitigt sein.
Meine Erwartungen an ein Forum ist der freundliche Hinweise auf den Quatsch dem man verzapft hat und Tipps bzw. Tricks die ein anderer nicht kennt.
Ja meine Kenntnisse in C sind nicht die besten. Ich bitte um Entschuldigung das ich nicht die Weisheit besitze die dir innewohnt.
Dachte meine Kenntnisse würden ausreichen

Mein System eclipse+ maestro USB controller+towerpro Servos

Schon geschaut User guide gibt nicht viel her.
Musste auch festellen, ich hätte wohl eher mit arduino arbeiten sollen.

shedepe · 17.02.2015, 13:06

Entschuldige, ich wollte nicht böse rüberkommen, aber es gibt leider ziemlich viele Leute die in ein Forum mit der Erwartung kommen: Ach hier wird mir schon jemand die Arbeit abnehmen, wenn ich einfach mal irgendwelchen Code hinklatsche ohne wirklich was dazu zu sagen. Diese Erwartung dämpfe ich lieber gleich rechtzeitig.

Aber wie gesagt. Dein Problem liegt darin. Du liest sequeziell aus der Datei, verarbeitest das, machst dann ein "sleep(2)", und setzt dann dein Ziel, dann machst du das fürs nächste bein usw.
1. Das lesen aus der Datei und suchen braucht Zeit...viel zeit. Einmal die Datei einlesen und dann die Matrix draus bilden wäre schneller, danach nur noch auf die Matrix zugreifen
2. Sleep(2). Lässt deinen derzeitigen Thread schlafen. Je nach Betriebssystem 2sec oder 2msec. Das sollte nicht nötig sein. Daher kommt vermutlich auch der Großteil deiner Verzögerung
3. Du machst das lesen/Schlafen für jedes Bein, wie schon gesagt einzeln...daher kommt eine Verzögerung für jedes Bein. Les dir die Position für jedes Bein ein, Speicher sie dir, und setze danach alle Position in einem Rutsch. Darauf zielt auch der Ratschlag von oberallgeier. Schau nach ob du eine Funktion /command findest mit der du alle Servos aufeinmal positionieren kannst.

Noch ein kleiner Tipp zu Fragen in Foren. Wenn du ein Problem mit konkretem Quellcode/Implementierung hast, beschreibe den Quellcode, die Aufgabe, die Plattform. Liefer am besten auch gleich Links zur Dokumentation mit dazu, dann ist die "Helf-Schwelle" gleich niedriger. Vorallem aber, versuche dein Problem einzugrenzen und sage dazu was du bereits versucht hast. Formulierungen in der Art, kann mir mal jemand helfen meinen Code so und so zu schreiben kommen in fast allen Foren die ich kenne sehr schlecht an, weil es so wirkt als ob du dein Problem auf andere komplett abwälzen willst. Du wirst in Foren selten jemanden finden der für dich Code schreibt, aber viele Leute die dir gute Tipps und Ratschläge geben können.

**oberallgeier** · 17.02.2015, 14:02

... Sleep(2). Lässt deinen derzeitigen Thread schlafen ... Das sollte nicht nötig sein ...

Ich kann nicht speziell auf Deinen Code eingehen. A) ich habe noch nie nen Hexa programmiert und B) Mein C ist eher bescheiden.

Aber von mehreren Servos "simultan" ansprechen verstehe ich schon ein bisschen. Nur sieht meine Lösung völlig anders aus, ich erledige das sozusagen nebenher, in einer ISR - mit dem Timer 1 (mega1284, 20 MHz). Hier mal skizziert, wie das für 8 Servos aussähe - bei mir laufen die Zeiten etwas anders, weil ich zehn Servos bediene.

Der Timer 1 läuft so, das sein Kanal A (OCR1A) pro Sekunde runde 8 x 50 = 400 Interrupts auslöst (COMPARE A). In der angstossenen ISR wird der Servopointer "berechnet" und damit dann der der aktuell angewählte Servo gestartet sowie der Kanal B gestartet mit der Zeit, die der angewählte Servo zu laufen hat (OCR1B). OCR1B MUSS immer einen Klacks kleiner sein als OCR1A - daher kommt die ISR COMP B noch vor der nächsten ISR COMP A. Dieser Wert von OCR1B wird im Wertefeld Srv_tm [i] abgelegt {i ε 1 .. 8:). In der folgenden ISR COMP B wird der aktuelle Servo gestoppt (bei mir sicherheitshalber - falls Überschneidungsprobleme auftauchen auch der Servo "davor"). Danach kommt die nächste ISR COMPA - (und weiter gehts mit "oben" >>..wird der Servopointer "berechnet"..<<). Nachdem die ISR COMPA A acht mal durchgelaufen ist, sind runde 20 ms vergangen - die gewünschte Periodenzeit für einen Servo; es kommt also jeder Servo alle 20 ms EINMAL dran.

Fazit: die Servos werden nur durch Vorgabe der jeweiligen Werte OCR1A und OCR1B bedient, der Rest läuft selbst - ohne irgendein sleep. So geht das bis alle acht Servos bedient sind.

Die Servos erhalten die Werte von "irgendwoher" - hier könnte Deine Matritze abgefragt werden - Du solltest dann aber vermutlich bei jedem Rundumschlag der ISR COMP A ==> EINMAL ein Flag setzen, damit Du die Werte wenigstens einmal pro Durchgang benutzt.

Laufen tut das dann z.B. um die zehn Servos im Kopf meines Roboters zu bedienen. Dort, in diesem Posting, siehst Du auch meine Liste verschiedener Flags, mit denen ich zusätzlich noch verschiedene Zustände der Servos schalte.

Wie betont/geschrieben - ich habe noch nie nen Hexa programmiert, dort gibt es sicher andere, klevere Lösungen.

**Geistesblitz** · 17.02.2015, 14:15

So kompliziert muss er das noch nichtmal machen, da er ja ein Maestro-Board verwendet. Da werden dann ja eigentlich nur serielle Befehle an das Board gesendet und um die Servo-PWM kümmert sich das Board dann selbst für alle Kanäle, für Digitalservos sogar bis zu 333Hz hoch. Die Befehle selbst sind allerdings ein bisschen tricky, da die wirklich auf Bitebene verarbeitet werden müssen, aber die Funktionen wurden anscheinend schon gut reinkopiert.

**oberallgeier** · 17.02.2015, 14:35

Zitat von Geistesblitz

So kompliziert muss er das noch nichtmal machen, da er ja ein Maestro-Board verwendet ...

Ahhh, danke. Mein Handycap - ich mache das alles "zu Fuß" - weil ich möglichst viel kapieren will. Und bis ich die Funktion einer fremden Lösung richtig gut kenne und andwenden kann, ists manchmal auch schon in die Tasten gehackt. Na ja, ok - nicht ganz so schnell.

**Dani3l** · 18.02.2015, 10:46

@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

@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

Klicke auf die Grafik für eine größere Ansicht

Name: Unbenannt.jpg
Hits: 10
Größe: 83,0 KB
ID: 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

shedepe · 18.02.2015, 11:15

Ich nehme mal an du versuchst so etwas wie Trajektorienplanung für einzelne Beine ? Wenn du dort die Jakobi Matrix für eine Inverse Kinematik nutzt musst du auf Singularitäten bzw. Invertierbarkeit achten. Das wird bei der Implementierung häufig falsch gemacht.