Hallo
Nun habe ich mich endlich auf die Odometry gestürzt und diese auch gleich etwas gepimpt. Mechanische Änderung sind je 4 zusätzliche Löcher in den Codescheibenritzeln, softwaremäßig habe ich odometry.c der Lib ersetzt.
Durch die zusätzlichen Löcher sind die Abstände der Löcher in den Coderitzeln gleichmäßiger, deshalb werden die Odometryzähler nun bei erkanntem Pinchange des entsprechenden Eingangs erhöht. Das ergibt mit 16 Zählimpulsen pro Coderadumdrehung eine deutliche Verbesserung gegenüber den vier Impulsen der orginalen Odometry. Die Impulse werden nur aufsteigend und drehrichtungsunabhängig gezählt. Gleichzeitig wird die Differenz der Seiten seit dem letzen Zählerreset gebildet. Die neuen Funktionen dafür sind:
int odometry_reset(void); // Zähler zurücksetzen
int odometry_get_left(void); // linken Zähler auslesen
int odometry_get_right(void); // rechten Zähler auslesen
int odometry_get_difference(void); // Zählerdifferenz auslesen, positiv bedeutet links>rechts
Desweiteren habe ich eine (in der Library nicht verwendete) Overflow-ISR am Timer1 eingeklinkt. Diese wird mit 15MHz/1022= ca. 15kHz aufgerufen und bildet die Basis für einfache Zeit- und Geschwindigkeitsfunktionen:
void Sleep(uint8_t); // Pause in 1/15000 Sekunden
void Msleep(uint16_t); // Pause in 1/1000 Sekunden
void delay(uint16_t); // ohne delay.h, verwendet Msleep()
uint32_t gettime(void); // Millisekunden seit dem Systemstart (als 32bit-Wert)
int speed_get_left(void); // 1000 minus Anzahl der Overflow-Ticks zwischen zwei positiven Flanken links
int speed_get_right(void); // dito rechts
Maxwerte für die Geschwindigkeitsmessung mit Startwert 1000 sind bei meiner Bee ca. 900 (also 100 Ticks zwischen den Löchern), bei orginalen Coderitzeln muss man die Werte selbst ermitteln. Mein aktueller Arbeitscode dazu:
Code:
// nibobee Odometry- und Motorfunktionen 24.1.01 mic
// Modifizierte Odometry mit 8 Löchern in den Ritzeln und Pinchange-Interrupt
// ergibt 16 Flanken pro Umdrehung des Coderitzels.
// Zusätzlich zur Library ist für den Timer1 eine Overflow-ISR eingeklingt die
// mit ca. 15kHz die Zeitbasis für Sleep(), Msleep() und gettime() liefert.
// Außerdem misst diese ISR die Anzahl der Timer1-Overflows zwischen zwei positiven
// Flanken der Odometrysensoren als umgekehrtes Mass für die Geschwindigkeit.
#include <nibobee/iodefs.h>
#include <nibobee/led.h>
#include <nibobee/motpwm.h>
// Variblen
volatile uint8_t count15kHz;
volatile uint32_t count_ms; // 32bit-Zähler!
volatile uint16_t odometry_left=0, odometry_right=0;
volatile int16_t odometry_difference=0;
volatile uint16_t speed_left=0, speed_right=0, speed_left_tmp=1000, speed_right_tmp=1000;
int16_t motor_pwm_left, motor_pwm_right;
uint32_t stopwatch1, stopwatch2;
// Funktionen
void Sleep(uint8_t pause); // ca. 1/15000 Pause blockierend
void Msleep(uint16_t pause); // ca. 1/1000 Pause blockierend
inline void delay(uint16_t pause) { Msleep(pause); }
uint32_t gettime(void); // Millisekunden seit Systemstart als 32bit-Wert
void odometry_reset(void); // Odometryzähler zurücksetzen
int odometry_get_left(void); // Odometryzähler auslesen
int odometry_get_right(void);
int odometry_get_difference(void); // Zählerunterschied seit dem letzten odo_reset
int speed_get_left(void); // Geschwindigkeit auslesen
int speed_get_right(void);
#include "lcd_lib.c" // Hier muss man die eigene LCD-Lib einbinden
int main(void)
{
led_init();
motpwm_init();
TIMSK |= (1 << TOIE1); // Die Overflow-ISR misst die Geschwindigkeit der Antriebe
enable_interrupts();
lcd_init();
lcd_writeString(" Nibobee mit LCD");
led_set(0,1);
Msleep(2000); // wait4programmer
lcd_writeStringAt(0,3,"Bitte Taste druecken");
while(!lcd_getkeys()) {PORTB ^= (1<<PB0); Msleep(100);}
led_set(0,0);
// Odometry: enable int0 and int1 on pinchange
MCUCR = (1<<ISC10) | (1<<ISC00); // Interrupt bei Pinchange
GICR |= (1<<INT1) | (1<<INT0); // Int0 und Int1 freigeben
lcd_cls();
lcd_writeString(" Odometrywerte");
lcd_writeStringAt(0,2,"Sp/PWM Li:");
lcd_writeStringAt(0,3,"Sp/PWM Re:");
motor_pwm_left=motor_pwm_right=300;
stopwatch1=stopwatch2=gettime();
while(1)
{
odometry_reset();
while((odometry_get_left()+odometry_get_right()) < 998) // Sollposition 1000
{
motpwm_setLeft(motor_pwm_left); motpwm_setRight(motor_pwm_right);
if((odometry_get_left()+odometry_get_right()) < 900) // Verzögerungspunkt
{
if(gettime() > stopwatch1)
{
stopwatch1=gettime()+20; // alle 20 Millisekunden ausführen
if(odometry_difference > 0)
{
motor_pwm_left--;
motor_pwm_right++;
}
else
{
motor_pwm_left++;
motor_pwm_right--;
}
}
}
else
{
motor_pwm_left=motor_pwm_right=200; // Einfahrgeschwindigkeit
}
if(gettime() > stopwatch2)
{
stopwatch2=gettime()+300;
lcd_writeIntegerAt(11,2,speed_get_left(), 10);
lcd_writeString("/");
lcd_writeInteger(motor_pwm_left, 10);
lcd_writeString(" ");
lcd_writeIntegerAt(11,3,speed_get_right(), 10);
lcd_writeString("/");
lcd_writeInteger(motor_pwm_right, 10);
lcd_writeString(" ");
}
}
motpwm_stop();
Msleep(200);
lcd_writeStringAt(0,1,"L ");
lcd_writeInteger(odometry_get_left(), 10);
lcd_writeString(" R ");
lcd_writeInteger(odometry_get_right(), 10);
lcd_writeString(" S ");
lcd_writeInteger(odometry_get_difference(), 10);
lcd_writeString(" ");
motor_pwm_left=motor_pwm_right=300;
while(!lcd_getkeys()) {PORTB ^= (1<<PB0); Msleep(100);}
}
return(0);
}
ISR(INT0_vect)
{
// static uint8_t flag=0; // nur jedes zweite Loch zählen für orginale bee
odometry_left++;
odometry_difference++;
if(PIND & (1<<PD2)) // if(flag++)
{
speed_left=(speed_left+speed_left_tmp)/2; // unterschiedliche Lochabstände ausgleichen
speed_left_tmp=1000;
// flag=0;
}
PORTB ^= (1<<PB0); // Kontrolle mit LED0
}
ISR(INT1_vect)
{
//static uint8_t flag=0;
odometry_right++;
odometry_difference--;
if(PIND & (1<<PD3)) // if(flag++)
{
speed_right=(speed_right+speed_right_tmp)/2;
speed_right_tmp=1000;
// flag=0;
}
PORTB ^= (1<<PB3); // Kontrolle mit LED3
}
ISR(TIMER1_OVF_vect)
{
static uint8_t count_ms_temp=15;
if(speed_left_tmp) speed_left_tmp--;
if(speed_right_tmp) speed_right_tmp--;
//PORTB ^= (1<<PB2); // Kontrolle mit LED2
if(count15kHz) count15kHz--;
if(!count_ms_temp--) {count_ms++; count_ms_temp=15;}
}
void Sleep(uint8_t pause) // ca. 1/15000 Pause blockierend
{
count15kHz=pause;
while(count15kHz);
}
void Msleep(uint16_t pause) // ca. 1/1000 Pause blockierend
{
while(pause--) Sleep(15);
}
uint32_t gettime(void) // Millisekunden seit Systemstart als 32bit-Wert
{
uint32_t temp32;
cli();
temp32=count_ms;
sei();
return(temp32);
}
void odometry_reset(void)
{
cli();
odometry_left=odometry_right=odometry_difference=0;
sei();
}
int odometry_get_left(void)
{
uint16_t temp16;
cli();
temp16=odometry_left;
sei();
return(temp16);
}
int odometry_get_right(void)
{
uint16_t temp16;
cli();
temp16=odometry_right;
sei();
return(temp16);
}
int odometry_get_difference(void)
{
uint16_t temp16;
cli();
temp16=odometry_difference;
sei();
return(temp16);
}
int speed_get_left(void)
{
uint16_t temp16;
cli();
temp16=speed_left;
sei();
return(temp16);
}
int speed_get_right(void)
{
uint16_t temp16;
cli();
temp16=speed_right;
sei();
return(temp16);
}
Soweit, so gut, das funktioniert alles zufriedenstellend. Aber wie kann ich nun die Geschwindigkeiten der Motoren regeln? Alle meine Anläufe die diversen Reglergrößen anzupassen scheiterten bisher kläglich. Selbst einen popeligen P-Regler kann ich nicht umsetzen, weil ich nicht weiß, wie ich die Abtastzeit und die Stellgröße (möglichst ohne float) an meine bee anpassen kann. Hier finde ich den Einstieg nicht, vielleicht kann mir das mal jemand erklären.
Gruß
mic
Zitat von Tuxi-Halle
Hallo m.a.r.v.i.n,
Berechtigungen
Zitieren
Lesezeichen