RP6 Servo Ansteuerung

[radbruch]

Ups, ich dachte, ihr redet über Servos am Erweiterungsmodul. Inwischen steure ich die Servos etwas anders an:

// Vierradantrieb mit RC-Steuerung v1.0 9.12.2007 mic

#include "rblib.h"
#include "rblib.c"

uint8_t servo_l, servo_r, demo;
uint16_t i, j, time;
uint8_t rc_input_pwr, rc_input_dir, rc_pwr, rc_dir, rc_misch=6;

ISR(TIMER0_COMP_vect)
{
	static uint16_t count=0;
	uint8_t rc_in;
	static uint8_t rc_temp_pwr=0;
	static uint8_t rc_temp_dir=0;
	if(count>servo_l) PORTA &= ~1;  else PORTA |= 1;	// ADC0
	if(count>servo_r) PORTA &= ~2;  else PORTA |= 2;	// ADC1
	if(count<2000)count++; else { count=0; time++; }
	rc_in=PINC;
	if (rc_in & 1) rc_temp_dir++; else						// SCL (Pin10)
		if (rc_temp_dir) { rc_input_dir=rc_temp_dir; rc_temp_dir=0; }
	if (rc_in & 2) rc_temp_pwr++; else						// SDA (Pin12)
		if (rc_temp_pwr) { rc_input_pwr=rc_temp_pwr; rc_temp_pwr=0; }
}
void servo_init(void)
{
	//DDRA |= 16;				// E_INT1 als Ausgang
	//DDRC |= 3;				// SCL und SDA als Ausgang
	DDRA |= 3;				// ADC0(PortA0) und ADC1(PortA1) als Ausgang

	TCCR0 =  (0 << WGM00) | (1 << WGM01);					// CTC-Mode
	TCCR0 |= (0 << COM00) | (0 << COM01);					// ohne OCR-Pin
	TCCR0 |=	(0 << CS02)  | (1 << CS01) | (0 << CS00);	// prescaler /8
	TIMSK =  (1 << OCIE0); 										// Interrupt ein
	OCR0  = 13; // 100kHz?
	time=0;
}
int main(void)
{
	rblib_init();
	servo_init();
	servo_l=130;
	servo_r=120;
   setMotorDir(FWD,FWD);
	setMotorPWM(130,90);
	setLEDs(0b1001);

	while(0)
	{
	   if (1) {
		writeString("\n\r");
	   writeInteger(rc_input_pwr, 10);
	   writeString("-");
	   writeInteger(rc_input_dir, 10);

	   if (rc_input_pwr > 105)
	   {
			rc_pwr=rc_input_pwr-100;
	   	servo_l=servo_r=rc_input_pwr;
	   	setMotorDir(FWD,FWD);
	   	setMotorPWM(rc_pwr*rc_misch,rc_pwr*rc_misch);
		}
		else if (rc_input_pwr < 95)
		   {
				rc_pwr=100-rc_input_pwr;
		   	servo_l=servo_r=rc_input_pwr;
	   		setMotorDir(BWD,BWD);
	   		setMotorPWM(rc_pwr*rc_misch,rc_pwr*rc_misch);
			}
			else
			{
				servo_l=servo_r=100;
				setMotorPWM(0,0);
			}
	   }

	}

	while(1);
	return 0;
}

Ich verwende nicht die orginalen Libraries, so kann ich freier über die Funktionen des Mega32 verfügen. (Meine abgespeckte Lib ist im Anhang, Verwendung auf eigene Gefahr, weil ohne Überwachungen! Ist ideal beim Anhauchen wenn ein Antrieb spinnt*grins*)

Alternativ könnte man auch die Timer 2 Compare ISR in der RP6BaseLib verwenden, die wird aber nicht ganz so häufig aufgerufen, die Werte für die Servostellung sind dann anders. Und man muss von Hand dafür sorgen, dass der Interrupt überhaupt läuft. Deshalb meine eigene Lib, damit geht das einfacher.

Zum Programm(, das eigentlich die Vorbereitung meines 4-Rad-Antriebs ist):

Die zwei Servos hängen an ADC0/1, das sind die freien ADC-Anschlüsse hinter dem IR-Empfänger auf der RP6-Platine. Hier habe ich zwei 3-polige Stiftleisten (+,-,ADC0/1) angelötet. Blöderweise sind die nicht servokompaktibel (bei den Servos ist + in der Mitte), aber an den Steckern der Servos kann man recht einfach die Adern vertauschen. Die Werte für die Servostellungen werden in uint8_t servo_l/servo_r übergeben, count erzeugt die 20ms-Wiederholung (und time dient als allgemeiner Timer):

if(count>servo_l) PORTA &= ~1; else PORTA |= 1; // ADC0
if(count>servo_r) PORTA &= ~2; else PORTA |= 2; // ADC1
if(count<2000)count++; else { count=0; time++; }

SDA und SCL verwende ich hier zum Einlesen der Signale einer RC-Fernbedienung die am xBus hängt, das wird in der ISR gleich miterledigt. Mit dem Programm steure ich momentan die Motoren und die Servos zusammen an. Denn unnötigen Code kann man natürlich rausschmeissen. Das passiert im unteren Teil der ISR:

rc_in=PINC;
if (rc_in & 1) rc_temp_dir++; else // SCL (Pin10)
if (rc_temp_dir) { rc_input_dir=rc_temp_dir; rc_temp_dir=0; }
if (rc_in & 2) rc_temp_pwr++; else // SDA (Pin12)
if (rc_temp_pwr) { rc_input_pwr=rc_temp_pwr; rc_temp_pwr=0; }

Man kann auch gut die SDA/SCL am xBus zur Ansteuerung der Servos verwenden.

Gruß

mic