Minimallösung: Servo-Sensor

[radbruch]

Hallo

Die Idee: Was macht die Signalleitung des Servos, wenn kein Impuls ansteht?

Das Ergebniss: Mein RP6 erkennt, wenn ich mit dem Daumen gegen das Servohorn drücke:

Bild hier   Bild hier   Bild hier  
http://www.youtube.com/watch?v=fPnOa-c63zY
http://www.youtube.com/watch?v=ADkI8TLyGUY
http://www.youtube.com/watch?v=RDQ292-rrWc

Als Timer für das Servo dient der ADC-Interrupt. Die Steuerleitung des Servos hängt an E_INT (XBUS Pin mit 10K gegen GND. Nach dem Impuls wird der Pin auf Eingang geschaltet und etwas verzögert wird dann der Wert eingelesen:

// Servo-Lastmessung am Steuerpin                                 7.2.2008  mic

// In der 20ms-Signalpause wird die Spannung am Steuerpin des Servos gemessen
// und ausgewertet. (Servoansteuerung kompatibel zur RP6-Lib durch ADC-ISR)

#include "RP6RobotBaseLib.h"

uint8_t adc_pos, adc_count; // Position des Servos, Belastungszähler
uint16_t adc_servo;         // Messwert der Signalleitung

void servo_ON(void)         // Servo-ISR starten
{
	cli();
// Freeruning, 5V-Ref, ISR enable, prescale /16
// AVCC with external capacitor at AREF pin, Ergebniss rechtsbündig, Kanal ADC4 (E_INT)
	ADMUX = (0<<REFS1) | (1<<REFS0)  | (0<<ADLAR) | 4;
// setzte free running triggern
	SFIOR = (0<<ADTS2) | (0<<ADTS1) | (0<<ADTS0);
// Interrupt ein, Wandler einschalten, prescaller /16
	ADCSRA = (1<<ADIE) | (1<<ADEN) | (1<<ADPS2) | (0<<ADPS1)  | (0<<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);
	sei();
}

void servo_OFF(void) // Servo-ISR stoppen
{
	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();
}

int main(void)
{
	initRobotBase();
	adc_pos=55;				// 20-90 ist der Drehbereich der Servos
	adc_servo=0;         // Startwerte setzen
	adc_count=0;
	servo_ON();          // Servo-ISR starten

	while(1){
		if (adc_servo)    // wenn die Messspannung größer 0 ist
		{
		   if (adc_count < 255) adc_count++; // counter hochzählen
		   if (adc_count>4) setLEDs(63);     // Schwelle für Belastung
		}
		else
		{
		   adc_count=0; // keine Belastung
		   setLEDs(0);  //
		}
		mSleep(200); // kurz warten
	}
	return 0;
}

ISR(ADC_vect)
{
	static uint16_t count=0; 				// Zykluszähler
	if (count>adc_pos)                  // Servoimpuls senden?
	{
		DDRA &= ~16;                     // nein: E_INT auf Eingang ohne PullUp
		PORTA &= ~16;
		if (count == adc_pos+100)        // seit 100 Takten auf Eingang gesetzt,
		{
			adc_servo=ADC;                // jetzt messen wir die Spannung!
		}
	}
	else
	{
		DDRA |= 16;                      // Impuls senden, Pin auf Ausgang
		PORTA |= 16;                     // und high
	}
	if(count<1000) count++; else count=0; // Zyklus fertig?
}

(Ansteuerung des Servos wie in RN-Wissen beschrieben.)

Gruß

mic

[rp6flash]

servus,
ganz schön clever die sache.

ich hab bei youtube andere videos entdeckt, auch die mit der jukebox und wollt mal fragen ob du die codes noch hast?

wenn ja poste sie doch

danke

[radbruch]

Hallo

Zur RP6-Jukebox:

Meist verlinke ich den endsprechenden Thread bei meinen Filmchen:
https://www.roboternetz.de/phpBB2/vi...=339495#339495

Der Zugriff auf's Flash ist nicht so einfach, denn nur aus dem Loaderbereich heraus kann man darauf schreibend zugreifen. Die einzige Chance dafür wär wohl eine Funktion im Loader mit der man indirekt was ins Flash schreibt.

Mein aktueller Ansatz: Der RP6 sampled die Daten und sendet sie zum Terminal. Per Copy&Paste erzeugt man daraus ein Daten[]-Array und included es im Programm:

// RP6 als Tonband mit Aufnehmen und Abspielen (Daten im Flash)     1.1.2008 mic

#include "rblib.h"								// bindet u.a io.h und stdlib.h ein
#include "rblib.c"								// und dient vorrangig dem Debuggen
#define speichergroesse 2048					// Anzahl der einzulesenden Bytes

int main(void)
{
uint8_t daten[]="";
uint8_t temp, bit_nr;							// Speicherplatz bereitstellen
uint16_t byte_nr, i, dummy;					// und ein paar Hilfsvariablen

	rblib_init();									// ein paar Initialisierungen
	setMotorPWM(0,0);								// PWM0= sterrt den PWM-Interrupt
	PORTA &= ~(1 << PINA4);						// PINA4 liest die Daten ein
	DDRA &= ~(1 << PINA4);						// PullUp PINA4 aus
	DDRC |= (1 << PINC0);						// an PINC4 hängt der Speaker
	byte_nr=speichergroesse;					// Zähler für die Bytes
	daten[0]=daten[0];

	while(0)											// Zum Einstellen des Pegels
	{													// wird nicht verwendet
		if (PINA & (1 << PINA4))
		{
		   setLEDs(1);
			PORTC |= (1 << PINC0);
			writeInteger(255,16);
		}
		else
		{
		   setLEDs(0);
			PORTC &= ~(1 << PINC0);
			writeInteger(0,16);
		}
		if (!temp--) writeString("\n\r");
	}                                      // Ende Testschleife

	writeString("\n\rStart\n\r");
	cli();                                 // Keine Störung zulassen, wir lesen ein
	do                                     // Schleife der Datenbytes
	{
		temp=0;
		bit_nr=8;                           // acht Bit einlesen
		do                                  // Schleife der Bits
		{
			if (PINA & (1 << PINA4))         // Pegel als High erkannt?
			{
				setLEDs(1);                   // Ja, LED an
				PORTC |= (1 << PINC0);        // Speaker an
				temp += 1;       					// und Bit0 im Datenbyte setzen
			}
			else
			{
				setLEDs(0);                   // Nein, LED aus
				PORTC &= ~(1 << PINC0);       // Speaker aus
				temp += 0;       					// für's Timing Bit0 nicht setzen
			}
			if (--bit_nr)							// alle Bits komplett?
			{
				temp *= 2;          				// Speicherbyte nach links shiften
				//for(i=0;i<100;i++) dummy+=i;  // kurze Verzögerung (8MHz!)
				if (byte_nr % 32)
				{
					if (bit_nr == 6) writeString(",");
					if (bit_nr == 4) writeString("&");
					if (bit_nr == 2) writeString("H");
				}
				else
				{
					if (bit_nr == 6) writeString(",\\");
					if (bit_nr == 4) writeString("\n");
					if (bit_nr == 2) writeString("&H");
				}
			}
			else
			{
				writeInteger(temp,16);        // Fertig, Byte als Hex senden
			}
		}
		while(bit_nr);                    	// Wenn nötig, nächstes Bit einlesen
	}
	while(--byte_nr);    // nächstes Byte bis Speicher voll
	sei();                                 // eigentlich nicht nötig
	writeString("\n\rFertig\n\r");
	
	return(0);                             // das war's. Einfach und blechern
}

Gruß

mic

[Michael]

Hallo radbruch,

Die Idee: Was macht die Signalleitung des Servos, wenn kein Impuls ansteht?

Das Ergebniss: Mein RP6 erkennt, wenn ich mit dem Daumen gegen das Servohorn drücke:

was mißt du denn am Servopin?
Steigt die Spannung? Wenn ja, um welchen Betrag?

Gruß, Michael

[robo.fr]

Hallo radbruch,

Ähm, der Zusammenhang, dass die Spannung am Stuerpin von der Last beeinflusst wird, ist mir nicht klar.
Da wäre es doch interessant zu wissen: was ist die Ursache? Sinkt durch die Belastung des Servos die gesammte Versorgungsspannung des Roboters ab und kann diese dann gemessen werden? Tritt der Effekt nur bei einem bestimmten Typ von Servo auf? Hängt es von der Innenschaltung des Servos ab? Welcher Hersteller, Typ ist das Servo genau?

Wie bist Du denn auf die Idee gekommen, so etwas auszuprobieren?

Gruß,
robo

[radbruch]

Hallo

Wie bist Du denn auf die Idee gekommen, so etwas auszuprobieren?

Über eine gedankliche Strommessung an der 5V-Leitung. Dabei fiel mir dann auf, dass der Steuerpin intern ja weder zu + noch zu - verbunden sein kann, bei Belastung also möglicherweise etwas zu messen wäre. Nach ein paar Timeingversuchen beim Einlesen der Spannung klappt es nun ganz gut. Die Verzögerung im Demo kommt vom relativ langem Verzögern zwischen den Auswertungen in der Hauptschleife (5*200ms). Da kann man aber noch einiges rausholen, das sind noch die allerersten Versuche.

Zum Messzeitpunkt: Innerhalb der 20ms-Zyklen wird erst der Impuls ausgegeben, dann kurz gewartet und dann erst der Wert eingelesen. Mit 5V-Referenz, schnellem Prescaler /16 (bei 8MHz-Kontrollertakt. Der ADC-Interrupt ist gleichzeitig der Timer für die Servointerrupts.) und 10Bit-Sampling lese ich Spitzenwerte von ca. 4.

Wichtig ist die kurze Wartezeit nach dem Impuls, denn es dauert bei dem verwendeten Takt bis zu 10 Aufrufe der ISR bis der Wert am Pin auf 0V sinkt. (Ich messe im Beispiel oben genau 100 Aufrufe nach dem Impuls) Der Wert bleibt auf 0 bis das Servo belastet wird. Beim Drehen ohne Last bleibt der Wert auch bei 0, aber möglicherweise kann man mit Last auch schon die Drehung selbst erkennen. Ich habe es auch noch nicht an einem freien ADC-Pin ausprobiert, in meinem Versuch ist ein 10k-PullDown verschaltet.

Bei mir funktioniert das mit den 5€-Miniservos und den 3€-Servos gut, mit den großen 5€-Servos funktioniert es noch nicht. Andere Typen stehen mir zum Testen nicht zur Verfügung. Anwenden möchte ich das bei einem Greifer, aber auch für Spinnenbeine dürfte das interesant sein.

Gruß

mic

[Manf]

Das ist sicher interessant und wichtig.
Ich bin mir nicht sicher wie weit die Sache untersucht ist und was ich davon richtig verstanden habe.

An die Steuerleitung eines Sevos wird wie üblich ein Signal mit einer Impulsdauer ausgegeben.
Abhängig von der Last ändert sich die Stromaufnahme des Servos und die Belastung ist am einfachsten(ohne weitere Schaltungen am deutlichsten ) an der Spannungsänderung auf der Signalleitung messbar.

Manfred

[radbruch]

Hallo Manfred

Ja, das trifft es im wesentlichen.

Was letztlich dazu führt, dass sich die Spannung an der Steuerleitung nach dem Impuls erhöht, kann ich im Moment noch nicht sagen. Auch ob das ein genereller Servo-Effekt ist oder ob das nur zufällig ein paar Servos machen weiß ich noch nicht. Man könnte es vermutlich auch mit dem Oszi sehen, wenn man darauf achtet. Die Spannung ist zwar recht klein, aber mit dem ADC eindeutig messbar. Ich werde mir mal die Ansteuerelektronik eines Servos genauer anschauen und natürlich werde ich auch mal versuchen, die Funktion sinnvoll anzuwenden.

Gruß

mic

[robo.fr]

Man könnte vielleicht einen relativ niederohmigen Widerstand ( 1 Ohm oä. ) in die Versorgungsleitung des Servos schalten und schauen, ob sich der Effekt verstärkt.

[Manf]

Vielen Dank erst einmal, ich denke dass es für das Herumtasten mit einem Bein, das den Boden erreichen soll, sehr hilfreich sein kann.

Mal sehen ob es noch jemand nachmisst, mit einem Shunt in der Versorgungsleitung sollte es natürlich grundsätztlich auch gehen.
Für die interaktive Führung eines Beins sind solche Signale sicher nötig.

Dass es jetzt auf der Steuerleitung am deutlichsten messbar ist, ist originell. Mal sehen wie weit es auf diese Weise geht.
Manfred