RP6 Servo Ansteuerung

[Robotoni]

Hallo verfolge mit viel Interesse das Forum .. habe seit 3 Wochen einen RP6
ein wenig aufgerüstet ... (LCD, zusätzliche LEDs, Tastatur, Sensor) und komme trotz eingeschränkter Programmier Fähigkeiten ganz gut zurecht (dank Forum) Ich möchte gerne einen Servo mit Sensor anbringen, der nach links und rechts einen Programmierten Winkel sich bewegt. Ich bitte um Mithilfe

[Dirk]

Hallo Robotoni,

Glückwunsch zu deinem RP6. Er hat ja in den 3 Wochen schon eine Menge an Hardware spendiert bekommen!

Zur Ansteuerung eines Servos gibt es hier im Forum viele gute Lösungen, so dass ich mich nicht traue, eine in reinem GCC für den RP6 zu posten (wer weiß, was es dann an Kritik hagelt: Zu einfach! Gab es schon tausendmal und viel besser ...).

Aber das Prinzip gilt auch für den RP6:
Das Servo braucht einen positiven Impuls von 1ms (linker Anschlag) bis 2ms (rechter Anschlag). Dieser Impuls sollte sich ca. alle 20ms wiederholen.
Du must also einen Port z.B. auf High schalten und nach 1,5ms wieder auf Low, wenn das Servo in Mittelstellung sein soll. Das wiederholt sich alle 20ms. Sieh dir mal die Funktion "delayCycles" an! Damit könntest du auch feinere Abstimmungen für die Impulsdauer erreichen.
Ich würde als erstes eine Task schreiben, die alle 20ms einen Impuls erzeugt (die Stopwatches sind dafür ideal). Die Impulslänge veränderst du dann nach Bedarf.

Gruß Dirk

[radbruch]

Hallo

Mein Ansatz für eine Servoansteuerung mit dem RP6:

Bild hier  
(Bild anklicken für 1,8MB mpg)

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, Pin2=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.

Gruß

mic

[Robotoni]

Erst mal danke für die Antworten, komme aber leider nicht richtig weiter, wie ändere ich die ISR ? kurze Schrittweise Erklärung wäre super "radbruch"

PS ich glaube in deiner Pinbelegung ist ein kleiner Fehler :
" (Pin1=GND, Pin2=5V-Vcc) " Richtig Pin 1=GND Pin3=VDD

[radbruch]

Hallo

Entschuldigung, das war natürlich Quatsch gewesen, 5V sind am Pin3.

Meine ISR in "RP6RobotBaseLib.c" sieht so aus:

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) PORTC &= ~1; else PORTC |= 1;
		if (servo_zyklus>servo_winkel) extIntOFF(); else extIntON();
		if (servo_zyklus<1440) servo_zyklus++; else servo_zyklus=0;
	}

	//else
	if (0)
	{
#endif
	static uint8_t ircomm_pulse;
	if(acs_state < 2) { // If ACS is not active, perform IRCOMM transmissions
		if(ircomm_pulse) { // Do we have IR pulses to send?
			if(ircomm_pulse < 60) { // Bi-Phase encoding...
				if(ircomm_data & 0x4000) // check current bit
					PORTD ^= (1<<PIND7); // Toggle IRCOMM port
				else
					PORTD &= ~(1<<PIND7); // deactivate IRCOMM port
			}
			else if(ircomm_data & 0x4000) // The same as above, but the other way round:
				PORTD &= ~(1<<PIND7); // deactivate IRCOMM port
			else
				PORTD ^= (1<<PIND7); // Toggle IRCOMM port
			ircomm_pulse--;
		}
		else if(ircomm_send) { // Do we still have data?
			PORTD &= ~(1<<PIND7);
			ircomm_data <<= 1; // Next Bit!
			ircomm_pulse = 120;
			ircomm_send--;
		}
		else
			PORTD &= ~(1<<PIND7); // no more pulses - IR LEDs off!
	}
	else if(acs_pulse) { // Send ACS IR pulses?
		if(sysStatACS.channel == ACS_CHANNEL_LEFT) // which channel?
			PORTB ^= ACS_L;
		else
			PORTC ^= ACS_R;
		acs_pulse--;
	}
	else { // no more pulses - IR LEDs off!
		PORTB |= ACS_L;
		PORTC |= ACS_R;
	}
#ifdef Servo
	}
#endif
}

Mit
//else
if (0)

wird die orginale ISR nicht mehr abgearbeitet. Wie geschrieben ist das noch in der Entwicklung und soll nur zeigen, wie man es machen könnte. Möglichkeiten gibt es sicher mehrere, aber ich mag Lösungen, die keinen großen Aufwand bedeuten.

Gruß

mic

[Dirk]

Hallo Robotoni,

hier auch von mir noch eine kleine Demo zu Servo-Ansteuerung.

Sie soll eigentlich nur zeigen, dass es auch ohne Einklinken in eine ISR geht, allerdings mit dem Nachteil einer Verschwendung von Rechenpower.

Damit soll dies hier nur zum Experimentieren dienen!

Viel Spaß!

Gruß Dirk

[SlyD]

Hallo Robotoni,

kleiner Tipp:
da Du das RP6-M32 Erweiterungsmodul hast, gäbe es neben den Möglichkeiten die mic und Dirk angegeben haben auch die Möglichkeit ein PWM Modul des MEGA32 auf dem RP6-M32 zu verwenden. Timer1 ist beim RP6-M32 komplett frei und PD5(OC1A) ist auf dem I/O Erweiterungsanschluss verfügbar. Das Hardware PWM Modul zu verwenden hätte den Vorteil, dass es keinerlei Rechenzeit kostet und immer nur einmal kurz die Position des Servos gesetzt werden muss.

Beispielcode habe ich gerade leider nicht parat, aber nähere Infos dazu findest Du z.B. im Datenblatt des MEGA32 im Abschnitt zum Timer1.

MfG,
SlyD

[Dirk]

Hallo Robotoni,

ich habe mir gerade noch einmal dein Bild vom RP6 angesehen:
Da hast du ja auch eine Tastatur draufgebaut.

Wie hast du die angeschlossen?

Gruß Dirk

[Robotoni]

Hallo Dirk,

Schaue unter http://www.avr-asm-tutorial.net/avr_.../keyboard.html
nach. Gemäß dem Plan "Widerstandsmatrix" habe ich diese gebaut und an ADC7 an das RP6 Erweiterungsmodul angeschlossen. Dazu habe ich mit meinen Eingeschränkten Programmierfähigkeiten ein Programm erstellt womit
der ADC7 ausgelesen wird, und das Wandler-Ergebniss einer Taste zugeordnet wurde :

if ((adc7 > 40) && (adc7< 65)) //Taste 1
{ setLEDs(0b0001);
sound(140,10,0);
writeStringLCD_P("Taste 1");
mSleep(500);
}

else if ((adc7 > 66) && (adc7< 105)) //Taste 2
{setLEDs(0b0010);
sound(140,10,0);
writeStringLCD_P("Taste 2");
mSleep(500);
}...........
meine ermittelten Werte
1=50/2=88/3=131/4=196/5=310/6=412/7=537/8=650/9=770/0=885
Demnächst Starte ich damit meine Programme...
Die Teile sind von Conrad alle Teile 7€

Gruß Toni

[Robotoni]

Das Programm von Dirk läuft gut.. aber es wäre schon toll, wenn das Signal über den Timer des RP6-M32 Erweiterungsmodules laufen würde.
Brauche eine kleine Programmierhilfe ! Ziel ist es das das Servo nach links läuft dann eine Messung durchgeführt (Entfernungsmessung) wird... und das selbe rechts abläuft...

Gruß Toni