Hallo

Wirklich zufällig hängt das Servo das meinen Sharp schwenkt auch an X3 :)

Code:
// Nibobee Motor- und Odometrytest                                    18.11.09 mic

#include <nibobee/iodefs.h>
#include <nibobee/delay.h>
#include <nibobee/analog.h>
#include <nibobee/odometry.h>
#include <nibobee/led.h>
#include <nibobee/sens.h>
#include <stdlib.h>			// C standard functions (e.g. itoa...)
uint16_t nibobee_initialization; // ???

#define LineLEDs_on		PORTB &=~(1<<PB4)
#define LineLEDs_off		PORTB |= (1<<PB4)
#define DIR_R 7
#define DIR_L 6
#define PWM_L 5
#define PWM_R 4

void ir_init(void);
void writeChar(uint8_t data );
void writeString(char *string);
void writeInteger(int16_t number, uint8_t base);
void motorDir(uint8_t dir_left, uint8_t dir_right);
void motorSpeed(uint8_t speed_left, uint8_t speed_right);

uint8_t p=0, servo=0;

int main(void) {

	uint8_t c;
	uint16_t echo[16];
	uint8_t servopos=0;
	int8_t schwenk=1;
	

	led_init();
	sens_init();
	analog_init();
	odometry_init();
	odometry_reset();
	ir_init();
	led_set(LED_L_YE,1);


	PORTD |=(1<<PWM_L)|(1<<PWM_R); // Pullups einschalten damit der Pegel high bleibt
	DDRD |=(1<<DIR_L)|(1<<DIR_R)|(1<<PWM_L)|(1<<PWM_R);

	writeString("\n\n\r");
	writeString("Sharp-Test mit Schwenkservo\n\r");
	writeString("21.11.09 mic\n\n\r");
	delay(2000);
	servo=108;

	while(1)
	{
	   servo=35+servopos*10;
	   delay(100);
	   echo[14-servopos]=analog_getValue(ANALOG_EXT3);
	   if((servopos==0) || (servopos==14))
		{
			for(c=0; c<15; c++)
			{
				writeInteger(echo[c], 10);
			   writeString("-");
			}
			writeString("\n\r");
	   }
	   servopos+=schwenk;
	   if(!servopos) schwenk=1; else if(servopos==14) schwenk=-1;
	}
	return 0;
}

// Die ISR erzeugt die 36kHz-Trägerfrequenz und "lauscht" gleichzeitig am TXD-Pin
// um die seriellen Daten auf die Trägerfrequenz zu mischen. Zusätzlich erzeugt
// sie die Impulse für das Servo an PC3. Letztlich dekrementiert sie p solange
// bis es 0 ist

ISR (TIMER2_COMP_vect)
{
	static uint8_t status=0;
   if(PIND & 2) // Der TSOP invertiert die empfangenen Daten!
	   LineLEDs_off;
	else
		if(status) {LineLEDs_on; status=0;} else {LineLEDs_off; status=1;}

	static uint16_t count=1;
	if(count>servo) PORTC &= ~(1<<PC3); else PORTC |= (1<<PC3);
	if(count<1440) count++; else {count=1; if(p) p--;};
}
void ir_init(void)
{
 	#define UBRR_BAUD_2400 390       // Baudrate auf 2400 setzen
   UBRRH = UBRR_BAUD_2400 >> 8;
	UBRRL = (uint8_t) UBRR_BAUD_2400;

	UCSRC = (1<<URSEL)|(0<<USBS)|(3<<UCSZ0); // 8, none, 1
  	UCSRB |= (1<<TXEN); // Senden enablen

	TCCR2 = (1 << WGM21) | (1 << CS20); // CTC-Mode, no prescaling, no OC2-PIN!
	OCR2  = 208; // 36kHz @15MHz
	TIMSK |= (1 << OCIE2);

	DDRB |= (1<<PB4); // LineLED ist ein Ausgang
	LineLEDs_on; 		// LineLED schaltet gegen GND!
	DDRD |= 2;  	 	// TXD Ausgang und low
	PORTD &= ~2;
	sei();
}
void writeChar(uint8_t data ) // Code aus ATMega16-Datenblatt
{
	while ( !( UCSRA & (1<<UDRE)) );
	UDR = data;
}
void writeString(char *string)
{
	while(*string)
		writeChar(*string++);
}
void writeInteger(int16_t number, uint8_t base)
{
	char buffer[17];
	itoa(number, &buffer[0], base);
	writeString(&buffer[0]);
}
void motorDir(uint8_t dir_left, uint8_t dir_right)
{
	if(dir_left)  PORTD |= (1<<DIR_L); else PORTD &= ~(1<<DIR_L);
	if(dir_right) PORTD &= ~(1<<DIR_R); else PORTD |= (1<<DIR_R);
}
void motorSpeed(uint8_t speed_left, uint8_t speed_right)
{
	if(speed_left)  PORTD &= ~(1<<PWM_L); else PORTD |= (1<<PWM_L);
	if(speed_right) PORTD &= ~(1<<PWM_R); else PORTD |= (1<<PWM_R);
}
(ungeputzer Arbeitscode)

Ein 36kHz-Takt wird mit Timer2 erzeugt:
Code:
	TCCR2 = (1 << WGM21) | (1 << CS20); // CTC-Mode, no prescaling, no OC2-PIN!
	OCR2  = 208; // 36kHz @15MHz
	TIMSK |= (1 << OCIE2);
In der ISR wird dann das Signal ausgegeben:
Code:
ISR (TIMER2_COMP_vect)
{
...
	static uint16_t count=1;
	if(count>servo) PORTC &= ~(1<<PC3); else PORTC |= (1<<PC3);
	if(count<1440) count++; else {count=1; if(p) p--;};
}
Und schließlich kann man dann die Servoposition in der Variablen servo übergeben. Mitte ist ca. 108, 0 bedeutet kein Impuls senden. Das sollte direkt so funktionieren, mein Sharp hängt auch an X3 ;)

Externes Bild anzeigen   
http://www.youtube.com/watch?v=G836SWiYA_c

Servostecker mußte man für ADC0/1 beim RP6 auch "Umbauen".
Warum die Hersteller das Servopinout nicht übernehmen ist mir schleierhaft:
Externes Bild anzeigen   

Gruß

mic