RP6 Servo Ansteuerung

[radbruch]

Sorry, wenn meine Vorschläge nicht wirklich einfach nachzuvollziehen sind. Ich bastle eben schon eine Weile dran rum und das ist alles eher "natürlich gewachsen".

Die einfachste Lösung für den RP6 habe ich wohl hier beschrieben:
https://www.roboternetz.de/phpBB2/ze...ag.php?t=34950

Oder ohne sleep(), dafür mit Verzögerungsschleifen:
https://www.roboternetz.de/phpBB2/ze...=321483#321483

Oder so wie bei meinem Monoleg:

#include "RP6RobotBaseLib.h" 

uint8_t  i, pause, servo_stellzeit, servo_delay; 

void servo(uint8_t w0, uint8_t w1, uint8_t w2) 
{ 
unsigned int count=0; 
   do{ 
      PORTA |= E_INT1;      // E_INT1 (Pin8) 
      sleep(w0); 
      PORTA &= ~E_INT1; 
      PORTC |= 1;            // SCL (Pin10) 
      sleep(w1); 
      PORTC &= ~1; 
      PORTC |= 2;            // SDA (Pin12) 
      sleep(w2); 
      PORTC &= ~2; 
      sleep(servo_delay-(w0+w1+w2)); 
      //sleep(127); 
   } while (count++ < servo_stellzeit); 
   mSleep(10*pause); 
} 

int main(void) { 

initRobotBase(); 
i=0; 
servo_stellzeit=15; 
DDRA |= E_INT1;            // E_INT1 als Ausgang 
DDRC |= 3;                  // SCL und SDA als Ausgang 
// 5 - 15 - 25             // Min - Mitte - Max 
servo(15,15,15);           // Grundstellung 
while (1) 
{ 
   switch (i) 
   { 
      case 0: i++; pause=30; servo_delay=255; mSleep(1000); break; 
      case 1: i++; pause=10; servo_delay=255; break; 
      case 2: i++; pause=10; servo_delay=127; break; 
      case 3: i++; pause=1;  servo_delay=127; break; 
      case 4: i=0; pause=1;  servo_delay=127; break; 
   } 
   servo(10,15,15);        // Finger zurück 
   servo(15,15,15); 
   servo(20,15,15);        // Finger vor 
   servo(15,15,15); 
    
   servo(15,10,15);        // Arm runter 
   servo(15,15,15); 
   servo(15,20,15);        // Arm hoch 
   servo(15,15,15); 
    
   servo(15,15,10);         // Schulter links 
   servo(15,15,15); 
   servo(15,15,20);         // Schulter rechts 
   servo(15,15,15); 

   servo(20,10,15);        // Finger vor, Arm runter 
   servo(25,5,15); 
   servo(20,10,15); 
   servo(15,15,15); 
    
   servo(20,20,15);        // Finger vor, Arm hoch, Schulter links 
   servo(15,15,10); 

   servo(10,20,10);        // Finger zurück, arm hoch 
   servo(5,25,10); 
   servo(10,20,10); 
   servo(15,15,15); 

} 
return 0; 
}

Alles Lösungen mit der orginalen Library und Anschluß am XBUS-Stecker. Einfach zu finden wenn man hier im RN-Forum nach "servo AND rp6 AND radbruch" sucht.

Einfacher geht's wohl nur so:

https://www.roboternetz.de/phpBB2/ze...ag.php?t=29902

rblib muss ins selbe Verzeichniss wie das Programm (wegen den "", bei <> müßte es in den Pfad).

Gruß

mic

[inka]

Hardware ist ein 5€-Miniservo von Conrad, aufgeklebt auf den RP6 mit einem Power-Strip. Aus dem Stecker(JBL) habe ich den Taktpin (orange) ausgebaut. Nun kann ich den Servo-Stecker mit +(rot) und -(braun) in den XBUS-stecken (Pin1=GND, Pin3=5V-Vcc) und das Signal z.b. in Pin8 (INT1) oder Pin10 (SCL).

Für die Ansteuerung zapfe ich den 72kHz-Interrupt an, der Timerinterrupt hat mit nur 10 kHz eine zu geringe Auflösung (=weniger mögliche Servopositionen). Das ergibt zwar "krumme" Werte für die Positionen, funktioniert aber wunderbar:

#include "RP6RobotBaseLib.h"
#define servo_min 26
#define servo_mitte 85
#define servo_max 155
volatile unsigned char servo_winkel;
uint8_t servo_pos=servo_mitte;

void servo(uint8_t winkel)
{
	uint16_t servo_stellzeit;
	if (servo_winkel != winkel)
	{
	   TIMSK |= (1 << OCIE2);
		servo_stellzeit=200*abs(servo_winkel-winkel);
		servo_winkel=winkel;
		while (servo_stellzeit--);
		TIMSK &= ~(1 << OCIE2);
	}
}
int main(void) {
uint8_t i;

	initRobotBase();
	DDRC |= 1; // SCL auf Ausgang stellen

	while (1)
	{
		servo(servo_mitte);
		mSleep(500);
		servo(servo_min);
		mSleep(200);
		for (i=servo_max; i>servo_min; i-=2)
		{
			servo(i);
			setLEDs(16);
		}
		for (i=servo_min; i<servo_max; i+=2)
		{
			servo(i);
			setLEDs(32);
		}
	}
while(1);
return 0;
}

/*
ISR (TIMER2_COMP_vect)
{
#define Servo on
#ifdef Servo
	extern unsigned char servo_winkel;
	static unsigned int servo_zyklus=0;
	if (servo_winkel)
	{
		if (servo_zyklus>servo_winkel) extIntOFF(); else extIntON();
		//if (servo_zyklus>servo_winkel) PORTC &= ~1; else PORTC |= 1;
		if (servo_zyklus<1440) servo_zyklus++; else servo_zyklus=0;
	}

	//else
	if (0)
	{
#endif
...
#ifdef Servo
	}
#endif
}

/**
*/

Der einfache Democode definiert die Lageendpunkte und die Mitte, und steuert das Servo endlos auf diese Punkte. In der Funktion servo() wird die Stellzeit berechnet und der Winkel in servo_winkel eingetragen. Dann wird der Interrupt freigegeben und die Erweiterung des Interrupts steuert dann das Servo automatisch. Nach Ablauf der Stellzeit wird der Interrupt wieder gesperrt.

Um Seiteneffekte zu vermeiden, wird in der Library der restliche Code der ISR ausgeblendet. Der Code für die Änderung der ISR befindet sich am Ende des Democodes. Das Ganze ist noch in der Entwicklung.

Hi mic,
es ist schon lange her, aber vielleicht erinerst du dich noch. Ich habe es so versucht, wie hier beschrieben, der servo bewegt sich leider nicht (der kontakt 1 am XBUS1 ist ja auf der platine gekennzeichnet)...
Was muss ich da mit dem ISR tun?
danke

@Dirk:
ich habe mir auch Deine version angeschaut, wie schliesse ich den servo an die LED1, bzw. wo finde ich freie anschlüsse für SL1? Bin ich blind, oder zu doof?

thanx...

[radbruch]

Hallo

Das funktioniert nur, wenn du in der RP6BaseLib.c die Timer2-Overflow-ISR komplett ersetzt:
https://www.roboternetz.de/phpBB2/vi...=318456#318456

Inzwischen, das ist ja schon zwei Jahre her, erkenne ich auch, dass diese Art der Ansteuerung ziemlicher Mist ist. Deshalb rate ich von der Benutzung eher ab.

Da die RP6-Library alle Timer verwendet, muss man bei der Servoansteuerung immer Kompromisse eingehen, die von der jeweiligen Anwendung abhängig sind:

Das einfachste (wie oben schon beschrieben) ist eine blockierende Ansteuerung (mit Sleep() oder Zählschleife) bei der das Programm wartet, bis das Servo seine Position erreicht hat. Das ist für erste Servoversuche die übersichtlichste Lösung. Diesen Ansatz mit einem Servo an SCL hatte ich hier mal in das Tasksystem eingebunden:

#include "RP6RobotBaseLib.h"

#define pos1 800
#define pos2 1600

extern uint8_t acs_state;

uint16_t servo_pos, servo_timer, servo_dummy;

int main(void) {

	initRobotBase();
	DDRC |= 1;
	
	startStopwatch1();
	startStopwatch2();
	servo_pos = pos1;
	
	while (1)
	{
		if ((getStopwatch1() > 20) && (acs_state < 2)) // alle 20ms einen Servoimpuls erzeugen
		{
	      setStopwatch1(0);
			servo_timer = servo_pos;
	      PORTC|=1;
			while(servo_timer--) servo_dummy ^= servo_timer;
	      PORTC&=~1;
		}

		if (getStopwatch2() > 1000) // alle 1000ms Servo auf andere Seite fahren
		{
			if (servo_pos == pos1) servo_pos=pos2; else servo_pos=pos1;
			setStopwatch2(0);
		}
      //task_RP6System();
      task_ADC();
		task_ACS();
		task_Bumpers();
      task_motionControl();
	}
return 0;
}

Alle Servoansteuerungen, die im Hintergrund ausgeführt werden, müssen einen Timer umbiegen und blockieren damit die Lib-Funktion des betreffenden Timers. Ohne Timer0 gibt es keine StopWatch() und andere Zeitfunktionen mehr, ohne Timer1 funktionieren die Motoren nicht mehr und ohne Timer2 gibt es keine 36kHz-Trägerfrequenz fürs IR. Das bedeutet, man muss sich entscheiden, auf welche Funktionsgruppe man verzichten möchte und steuert die Servos dann mit dem freien Timer an. Dabei kann man natürlich jederzeit wieder die orginale Funktion des Timers aktivieren indem man initRobotBase() aufruft und das Timersetup der Lib wiederherstellt. Hier eine resourcenschonende Variante mit Timer2 und drei Servos an SDA, SCL und E-INT am XBUS:

//Servos mit Timer2 Overflow-ISR                                 27.2.2008 mic

#include "RP6RobotBaseLib.h"

uint8_t pos[3]={255,170,85}; // Positionen der drei Servos an SDA, SCL und E_INT

int main(void)
{
	initRobotBase();
	DDRA |= 16;
	PORTA &= ~16;
	DDRC |= 3;
	PORTC &= ~3;

	//Timer2 Initialisierung
	TCCR2 = (0 << WGM21) | (0 << COM20) | (1 << CS22); // Normal Mode, prescaler /64
	TIMSK |= (1 << TOIE2); // Timer2 Overflow-Interrupt erlauben

	while(1)
	{
	   pos[0]=pos[1]=pos[2]=170;
	   mSleep(1000);
	   pos[0]=pos[1]=pos[2]=220;
	   mSleep(1000);
	   pos[0]=pos[1]=pos[2]=170;
	   mSleep(1000);
	   pos[0]=pos[1]=pos[2]=120;
	   mSleep(1000);
	}
	return(0);
}
ISR (TIMER2_OVF_vect)
{
	static uint8_t servo_nr=0;
	static uint16_t impulspause;
	if(servo_nr)
	{
	   if(servo_nr==1) {TCNT2=-pos[0]; PORTC |= 1; impulspause-=pos[0];}
	   if(servo_nr==2) {TCNT2=-pos[1]; PORTC &= ~1; PORTC |= 2; impulspause-=pos[1];}
	   if(servo_nr==3) {TCNT2=-pos[2]; PORTC &= ~2; PORTA |= 16; impulspause-=pos[2];}
	   if(servo_nr==4) {PORTA &= ~16; servo_nr=0;}
	   if(servo_nr) servo_nr++;
	}
	else
	{
	   if(impulspause>256) impulspause-=256;
			else {TCNT2=-impulspause; servo_nr++; impulspause=1500;}
	}
}

In diesem Zusammenhang möchte ich noch den ADC-Interrupt erwähnen. Er wird von der RP6-Lib nicht verwendet und eignet sich auch dazu, Servoimpulse im Hintergrund zu erzeugen. Der ADC läuft dabei im Dauerlauf, dadurch wird allerdings der Zugriff auf unterschiedliche ADC-Kanäle etwas komlizierter. Ein Beispiel mit Servos an den LEDs und SDA/SCL:

// Domino Day für den RP6 mit neuem Greifarm und ADC-ISR         19.1.2008  mic

#include "RP6RobotBaseLib.h"

uint16_t s1, s2, s3, s4 ,s5, s6;

void servo_ON(void)
{
	cli();
// Freeruning, 5V-Ref, ISR enable, prescale /32 ((8MHZ/32)/12,5 sind ca. 20kHz bzw. 0,05us)
// AVCC with external capacitor at AREF pin, Ergebniss linksbündig, Kanal ADC7 (UBAT)
	ADMUX = (0<<REFS1) | (1<<REFS0)  | (1<<ADLAR) | 7;
// setzte free running triggern
	SFIOR = (0<<ADTS2) | (0<<ADTS1) | (0<<ADTS0);
// Interrupt ein, Wandler einschalten, prescaller /32
	ADCSRA = (1<<ADIE) | (1<<ADEN) | (1<<ADPS2) | (0<<ADPS1)  | (1<<ADPS0);
// Autotriggern bedeutet jetzt free running aktivieren, altes Flag löschen
	ADCSRA |= (1<<ADATE) | (1<<ADIF);
// Initialisierung starten
	ADCSRA |= (1<<ADSC);
// und noch die wohl eher unnötige Initiallesung
	while (!(ADCSRA & (1<<ADIF)));
	ADCSRA |= (1<<ADIF);

	DDRC |= 0x70;		// LED1-3 auf Ausgang und low
	PORTC &= ~0x70;
	DDRB |= 0x83;		// LED4-6 auf Ausgang und low
	PORTB &= ~0x83;
	DDRC |= 0x3;		// SCL und SDA auf Ausgang und low
	PORTC &= ~0x3;
	sei();
}
void servo_OFF(void)
{
	cli();
	// Initialize ADC: (Defaultsetup der RP6-Library)
	ADMUX = 0; //external reference
	ADCSRA = (0<<ADIE) | (0<<ADEN) | (1<<ADPS2) | (1<<ADPS1) | (1<<ADIF);
	SFIOR = 0;
	sei();
	setLEDs(0); // LEDs restaurieren
}
int main(void)
{
	initRobotBase();

	s1=25;
	s2=25;
	s3=25;
	s4=25;
	s5=25;
	s6=25;
	servo_ON();
	setLEDs(63);
	mSleep(2000);
	setLEDs(0);
	while(1)
	{
		mSleep(1000);
		s1=40;
		mSleep(1000);
		s1=25;
		mSleep(1000);
		s2=35;
		mSleep(1000);
		s2=15;
	}
	return 0;
}

ISR(ADC_vect)
{
	static uint16_t count=0;
	(count>s1)?(PORTC&=~SL1):(PORTC|=SL1);
	(count>s2)?(PORTC&=~SL2):(PORTC|=SL2);
	(count>s3)?(PORTC&=~SL3):(PORTC|=SL3);
	(count>s4)?(PORTB&=~SL4):(PORTB|=SL4);
	(count>s5)?(PORTB&=~SL5):(PORTB|=SL5);
	(count>s6)?(PORTB&=~SL6):(PORTB|=SL6);

	(count>s1)?(PORTC&=~1):(PORTC|=1); // SCL
	(count>s2)?(PORTC&=~2):(PORTC|=2); // SDA

	(count<500)?count++:(count=0);
}

Bitte beachtet, dass ich inzwischen ziemlich viel Zeit mit Servoansteuerungen vergeudet habe...

Gruß

mic

[inka]

danke erstmal mic,

servoanschluss braun an XBUS1-1, rot an XBUS1-3, orange an XBUS1-10, codebeispiel 1 fehlerfrei kompiliert und upgeloaded - servo (conrad, VSD-1) steht, nur die SL5 blinkt in bestimmten abständen 10x...

[radbruch]

Hallo

Der Anschluß 1(GND, bn), 3(Vdd, rt) und 10(PC0/SCL, orange) sollte passen(, wenn du wirklich am XBUS bist). Zuckt das Servo eigentlich beim Einschalten des RP6 oder wenn du es unter Spannung einsteckst? Wenn nicht, sind vielleicht die Anschlussleitungen im Servo "abgefallen". Das Problem habe ich häufig bei meinen (Billig-)Servos.

Das Blinken der LED ist die Power-On-Anzeige der RP6-Lib. Daran erkennt man, dass der RP6 eingeschaltet ist. Die Anzeige wird nur aktiv, wenn das Programm selbst keine LEDs ansteuert und soll verhindern, dass die Akkus leergesaugt werden.

Gruß

mic

[Edit]
Die Anschlüsse SL1-6 sind die Lötpads IO1-4 und BPL/PBR auf der RP6-Platine neben den LEDs:
Bild hier  
https://www.roboternetz.de/phpBB2/ze...=356247#356247

[inka]

hi mic,

also XBUS1- 1,3, und 10 stimmen
- wenn ich den RP6 einschalte leuchtet SL2, 5 und PWRON auf, erlischen dann und SL1 blinkt. (servo ist angeschlossen)
- beim drücken des tasters leuchten kurz SL2 und 6 auf.
- beim abziehen, bzw. wiederaufstecken des servokontaktes auf SCL bewegt sich der servo und bleibt in der endposition (unter spannung) stehen
- beim abgeschalteten RP6 und beim bewegen des servorotors leuchten SL2, 5 und PWRON

das blinken der SL5 bei laufendem programm ist die anzeige dafür das der RP6 eingeschaltet ist und ein programm läuft, ok?

[radbruch]

Hallo

Ich habe jetzt die drei Beispiele von oben nochmals kompiliert und getestet. Mein Servo an SCL dreht dabei wie erwartet (im StopWatch()-Beispiel zuckelt es etwas), also muss dein Servo oder der Anschluss fehlerhaft sein.

Das Hex vom Timer2-OVL-ISR-Demo oben erzeugt Servoimpulse an SDA, SCL und E-INT.

Gruß

mic

[inka]

hi mic,
danke erstmal, werde also schauen, dass ich ein enderes servo zum testen bekommme.
Zwei fragen noch:
- das servo ist extra, ich habe es bei conrad sonts an keinem gesehen, mit "digital" gekennzeichnet. muss es deshalb evtl. anders behandelt werden?
- vielleicht ist durch editieren meine frage untergegangen: sind die SL1-6 anschlüsse noch irgendwo herausgeführt, oder muss ich an den dioden rumlöten?

[Virus]

man kann digitale Servos auch an normalen rcempfängern anschliesen,
also auch wie normale Servos behandeln !
digitale sind etwas genauer und schneller, brauchen aber auch mehr strom, weil sie öfter nachregeln.

[inka]

hi,
ich hab´s jetzt mit der sw von Dirk zum laufen gebracht, also versorgung am PIN 1 & 3 der XBUS1 und steuerung über SL1. Sogar die herausgeführten anschlüsse habe ich gefunden, man muss halt doch in den schaltplan schauen dann findet man sachen!

noch eine frage: der servo fährt zuerst die 0-pos an, dann die zweite position (von vorn gesehen nach rechts) und kehrt nach einer weile wieder in die 0-pos zurück. manchmal zuckt er in der rechten position zuerst etwas inn richtung 0-pos, aber erst nach ca. 1 sec fährt er los. Nicht bei jedem rücklauf. Woran kann es liegen?

danke...