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 ;)
Bild hier
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:
Bild hier
Gruß
mic
Zitieren
Berechtigungen
Lesezeichen