"Multithreading" mit C ?

[voidpointer]

Wie UlrichC bin ich der Meinung, dass für das synchrone Ansteuern von Servos kein Multithreading, Realtime OS oder sonstiger Software-Overhead notwendig ist.

Zuerst solltest Du die Servo-Ansteuerung mal mit Hilfe von Timer-Interrupts realisieren. Damit hat das Hauptprogramm auf einmal ne Menge Zeit, sich um andere Dinge zu kümmern. Als nächstes muss die Servoansteuerung schrittweise geschehen, also nicht sofort die Zielposition ansteuern, sondern eine Menge von Zwischenpositionen. Das Inkrementieren der Zwischenposition kann man ebenfalls in der Interrupt-Routine ausführen, wenn man vorher die Steps ausgerechnet hat.

Welcher Servo welche Steps macht, mit welcher Geschwindigkeit dies getan wird und wer das Tempo angibt, kommt auf die Servos an und ist durch entspr. Testläufe zu ermitteln.

Gruß, Achim.

[jagdfalke]

Hi, ich habe jetzt mal folgendes geschrieben:

#define F_CPU	8000000

#include <avr/delay.h>
#include <avr/interrupt.h>
#include <avr/signal.h>

#include "uart.h"
#include "servo.h"

volatile uint8_t servo_flag = 0;

SIGNAL (SIG_OVERFLOW0)
{
	servo_flag = 1;
} 

int main (void) {
	uart_init(1, 0);

	sei();
	TCCR0 |= (1<<CS02) | (1<<CS00);
	TIMSK |= (1<<TOIE0);



	while(1)
	{
		if(servo_flag == 1) {
			cli();
			uint8_t move_made = 0;  // 1 wenn eine Änderung gemacht wurde
			int8_t step = +1;
			if(shoulder_pos != shoulder_dest) {
				if(shoulder_pos > shoulder_dest) { step = -1; }
				shoulder_pos += step;
				move(1, shoulder_pos); // hier werden sie 2
				move(2, shoulder_pos); // 400 Ncm Servos gleichtzeitig bewegt
				move_made = 1;
			}
			if(elbow_pos != elbow_dest) {
				step = +1;
				if(elbow_pos > elbow_dest) { step = -1; }
				elbow_pos += step;
				move(3, elbow_pos);
				move_made = 1;
			}
			if(wrist_pos != wrist_dest) {
				step = +1;
				if(wrist_pos > wrist_dest) { step = -1; }
				wrist_pos += step;
				move(4, wrist_pos);
				move_made = 1;
			}
			if(gripper_pos != gripper_dest) {
				step = +1;
				if(gripper_pos > gripper_dest) { step = -1; }
				gripper_pos += step;
				move(5, gripper_pos);
				move_made = 1;
			}
			sei();
		}
	}
	return 0;
}

servo.h

volatile uint8_t shoulder_pos = 127;
volatile uint8_t elbow_pos = 127;
volatile uint8_t wrist_pos = 127;
volatile uint8_t gripper_pos = 127;

volatile uint8_t shoulder_dest = 255;
volatile uint8_t elbow_dest = 1;
volatile uint8_t wrist_dest = 1;
volatile uint8_t gripper_dest = 80;

void move(int servo, int pos)
{
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = '#';
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = 's';
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = servo;
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = pos;
}

Ist der Code so in Ordnung? Bewegen tun sich die 4 "Gelenke" schon mal gleichzeitig. Das mit der Steuerung der Geschwindigkeit verstehe ich nicht. Ich glaube aber so wie ich den Code jetzt geschrieben habe kann man die Geschwindigkeit nicht mit berücksichtigen. Was hier jetzt lustig ist: Wenn ein Servo noch bewegen muss wenn die anderen schon fertig sind ist er schneller als wenn noch einer mitlaufen muss. (Das stört irgendwie mein ästhetisches Empfinden )

mfg
jagdfalke

[SprinterSB]

[quote="jagdfalke"]Hi, ich habe jetzt mal folgendes geschrieben:
servo.h[/qoute]

#ifndef _SERVO_H_
#define _SERVO_H_

extern volatile uint8_t shoulder_pos;
extern volatile uint8_t elbow_pos;
...
extern void move(int servo, int pos);

#endif /* _SERVO_H_ */

Der ganze Klumbatsch ausm *.h gehört in ein *.c, im Header verbleiben nur die Deklarationen/Prototypen.

[jagdfalke]

Hä? Ich versteh kein Wort von dem Code den du da gepostet hast.
Warum in ein .c-File? So klappts doch!! Und es ist übersichtlicher. Wenn C schon keine Klassen hat, muss man doch irgendwie den Code gruppieren.

mfg
jagdfalke

[voidpointer]

Ist es richtig, dass die move-Methode die Servoposition über die serielle Schnittstelle ausgibt? Heisst das, dass Du einen externen Controller zur Ansteuerung der Servos benutzt? Wenn ja, wozu?

Egal, eigentlich sehe ich keine Probleme im Code (aber normalerweise programmiere ich die AVRs in Assembler). Die Effekte, die Du beobachtest, können natürlich durch die serielle Übertragung der Kommandos (Überholeffekte?) oder durch den Servocontroller selbst verursacht sein. Besser wäre es, wenn man im Servocontroller selbst einen Synchron-Modus programmieren könnte, aber das ist wohl eine Black Box.

Mit welcher Bitrate arbeitet die serielle Verbindung? Hab mal ausgerechnet: wenn es nur 2400 bit/s sind, dauert das Senden der 5 Servopositionen über UART 0.083 Sekunden, wogegen ein Timerinterrupt alle 0.032 Sekunden auftritt. Bei 9600 bit/s reicht das Timing.

Zur Geschwindigkeit: Du solltest zu Beginn einer synchronen Bewegung die Wege aller Servos ausrechnen. Der Servo mit dem längsten Weg gibt die Geschwindigkeit vor. Er macht pro Zyklus n Schritte, wobei n = Zykluszeit (z.B. 0.032s) * max Servogeschwindigkeit (z.B. 200/s) = z.B. 6.4. Die Steps der anderen Servos werden über das Verhältnis der Wege ausgerechnet. Hat z.B. Servo 3 einen Weg von 50, so macht er einen Step von 3.2 (6.4*50/100). Haben die verschiedenen Servos unterschiedliche Geschwindigkeiten, wird es allerdings komplizierter...

So, muss jetzt arbeiten.

[jagdfalke]

Der "externe Controller" ist auf dem selben Board (rnbfra 1.1). Er übernimmt das Senden der Pulse an die Servos. Auf ihm ist der RNS1-Servotreiber drauf.
Du sagst 3.2 Schritte? Hab bisher immer nur Integer-Werte rübergegeben. Mal schau ob das auch so klappt.
Die Baud-Rate ist 9600. Soll ich sie höher einstellen? Wenn sie höher ist, kann der Servotreiber dann noch was empfangen?

mfg
jagdfalke

[voidpointer]

Du sagst 3.2 Schritte? Hab bisher immer nur Integer-Werte rübergegeben. Mal schau ob das auch so klappt.

Integer ist schon korrekt. Du solltest die Schritte runden. Sonst gibt es eine Typverletzung. Und der Controller mag wohl auch nur Integer. Die fehlenden Nachkommastellen könnten allerdings geringe Ungenauigkeiten im Timing verursachen.

Die Baud-Rate ist 9600. Soll ich sie höher einstellen? Wenn sie höher ist, kann der Servotreiber dann noch was empfangen?

Nein. Wenn Du die Baudrate nur einseitig änderst, verstehen sich Sender und Empfänger nicht mehr. 9600 ist ja auch OK. Es muss noch etwas anderes geben, was zu dem komischen Verhalten führt.

[jagdfalke]

Ok, das hab ich soweit verstanden. Hab auch gleichmal ein bisschen Code geschrieben:

#define F_CPU	8000000

#include <avr/delay.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <math.h>
#include <stdlib.h>

#include "uart.h"


// METHODEN DEKLARATIONEN
void keep_moving(void);
void move(int servo, int pos);
void calc_steps(void);
int signum(int val);


//VARIABLEN DEKLARATIONEN
volatile uint8_t servo_flag = 0;
volatile uint8_t recalc_flag = 1;

volatile uint8_t shoulder_pos = 127;
volatile uint8_t shoulder_dest = 255;
volatile float shoulder_step = 0;

volatile uint8_t elbow_pos = 127;
volatile uint8_t elbow_dest = 255;
volatile float elbow_step = 0;





int main (void) {

	//UART AN
	uart_init(1, 0);

	//INTERUPT UND TIMER AN
	sei();
	TCCR0 |= (1<<CS02) | (1<<CS00);
	TIMSK |= (1<<TOIE0);

	while(1)
	{
		//WENN DIE EINMALE ANGEGEBENE POSITION ERREICHT WURDE
		//DÜRFEN DIE STEPS NEU BERECHNET WERDEN
		if(recalc_flag == 1) {
			calc_steps();
		}
		//NUR AUFRUFEN, WENN EIN TIMER INTERUPT DA WAR
		if(servo_flag == 1) {
			keep_moving();
		}
	}
	return 0;
}

void keep_moving() {
	cli();

	//NEUE POSITIONEN AUSRECHNEN UND RUNDEN
	shoulder_pos = abs(shoulder_pos + shoulder_step);
	elbow_pos = abs(elbow_pos + elbow_step);

	//POSITIONEN SCHICKEN
	move(1, shoulder_pos);
	move(2, shoulder_pos);
	move(3, elbow_pos);

	//WENN DIE POSITIONEN GERUNDET ÜBEREINSTIMMEN
	//NEUBERECHNUNG DER STEPS ERLAUBEN UND NEUES ZIEL FÜR ELBOW SETZEN
	if(abs(shoulder_pos) == shoulder_dest && abs(elbow_pos) == elbow_dest) {
		elbow_dest = 1;
		recalc_flag = 1;
	} else {
		recalc_flag = 0;
	}

	servo_flag = 0;
	sei();
}

void calc_steps(void) {
	//BERECHNEN WIE VIEL SICH DIE Servos BEWEGEN MÜSSEN
	uint16_t shoulder_to_go = shoulder_dest - shoulder_pos;
	uint16_t elbow_to_go = elbow_dest - elbow_pos;

	//HIER ALS ABSOLUTE WERTE
	int16_t shoulder_abs = abs(shoulder_to_go);
	int16_t elbow_abs = abs(elbow_to_go);

	//STEP ERSTMAL AUF +/- 1 SETZEN, JE NACH RICHTUNG
	elbow_step = signum(elbow_to_go);
	shoulder_step = signum(shoulder_to_go);

	
	if(elbow_abs < shoulder_abs) {
		elbow_step = 1 * signum(elbow_to_go);
		shoulder_step = (shoulder_abs / elbow_abs) * signum(shoulder_to_go);
	}
	if(elbow_abs > shoulder_abs) {
		shoulder_step = 1 * signum(shoulder_to_go);
		elbow_step = elbow_abs / shoulder_abs * signum(shoulder_to_go);
	}

}

void move(int servo, int pos){
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = '#';
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = 's';
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = servo;
      loop_until_bit_is_set(UCSRA, UDRE);
      UDR = pos;
}

int signum(int val) {
	if(val != 0) {
		return val/abs(val);
	} else {
		return 0;
	}
}

SIGNAL (SIG_OVERFLOW0){
	servo_flag = 1;
}

Der Code bewegt zur Vereinfachung erstmal nur die Schulter und den Ellbogen. Die Bewegung klappt schon ganz gut. Allerdings passt das Neusetzen des Ziels für Ellbogens nicht ("elbow_dest = 1;" in keep_moving() )
Wer Interesse und Zeit hat kann sich das ja mal anschauen warum das nicht klappt. Ich weiß es ist viel Code zum "kurz" anschauen aber ich habs ein wenig auskommentiert, jetzt isses recht gut zu verstehen.

mfg
jagdfalke