Code:
/* >> Sicherung 15sep08 1740 ...C1..\m168D_10\m168D_mot_10x80.c
===================================================================================
Target MCU : M168
Target Hardware : m168D
Target cpu-frequ. : In der Quelle wählbar
===================================================================================
Enthaltene Routinen: Alle Motorroutinen
void XTI_0_1_init( void )
ISR(INT0_vect) // hiess mal: ISR(SIG_INTERRUPT0)
ISR(INT1_vect) // hiess mal: ISR(SIG_INTERRUPT1)
void rgl_mo_12(void) // Regelung für Motor 12 (neuer Name: 09jul08)
void rgl_mo_34(void) // Regelung für Motor 34
void TC0PWM_init(void) //Init Timer/Counter0 für Erzeugung des PWM-Signals
void setPWMrechts(uint8_t speed) //Geschwindigkeit rechter Motor
void setPWMlinks(uint8_t speed) //Geschwindigkeit linker Motor
void Mrechtsvor(void) //Motor dreht im mathematisch positiven Drehsinn
void Mrechtszur(void) //Motor 1,2 dreht im Uhrzeigersinn
void Mrechtsstop(void) //Motor aus
void Mlinksvor(void) //Motor dreht im mathematisch positiven Drehsinn
void Mlinkszur(void) //Motor 3,4 dreht im Uhrzeigersinn
void Mlinksstop(void) //Motor aus
===================================================================================
*** Versionsgeschichte:
====================
x80 15sep08 1740 Version wie x77 - für zusätzliches Modul ~kal~
x70 22jul08 1620 Nach erfolgreicher Regelung versuch eines "echten" Laufes
mit Ausweichmanövern etc
....
x342 26jun08 08ff Interrupt für Motoren in TC0PWM_init/~mo1~ auf 1200 Hz stellen
===================================================================================
*** Aufgabenstellung : Software für R2_D03
in einer ersten Ausbaustufe angelehnt an: siehe Original
Original: ...C1..\m168D_10\mm168D_mo1_10x342.h 26jun08 08ff
================================================================================ */
/* ============================================================================== */
/* === Initialisierung fuer EXT_INT0/1 auf Pin 4+5/PD2,3 bei m168 ================
$002 jmp SIG_INTERRUPT0 ; IRQ0 Handler _VECTOR(1)
$004 jmp SIG_INTERRUPT1 ; IRQ1 Handler _VECTOR(2) ================ */
void XTI_0_1_init( void )
{ //Initialisiere beide Interrupts auf rising edge
// d.h. EICRA ISC00,01,10+11 auf 1 (doc,S68)
EICRA |= (1<<ISC11)|(1<<ISC10)|(1<<ISC01)|(1<<ISC00);
EIMSK |= (1<<INT1)|(1<<INT0); // und erlaube diese I´s in EIMSK
// Initialisierung der Zeiten:
Iencdr0 = 0;
Iz_diff0 = 0;
Iz_yseci0 = 0;
Iz_ysecv0 = 0;
}
/* ============================================================================== */
/* ============================================================================== */
/* === Nicht unterbrechbare ISR für EXT_INT0 auf Pin 46/PD2/mega168 =========== */
/* Routine setzt einen Zähler hoch. Der Zähler wird im main ausgelesen
##>> cli/sei setzen ?? <<## und nach 2 (od. 5) hunderstel Sekunden
auf den Speicher WEG_L/_R vorzeichenrichtig aufaddiert und dann zurückgesetzt.
##>> Beim Richtungswechsel (cli/sei) wird der Zähler ausgelesen und genullt,
damit ist eine saubere Wegmessung möglich.
Der zugehörige Motor auf PD7/PB0 = re,li und PB1 Geschwind./PWM */
ISR(INT0_vect) // hiess mal: ISR(SIG_INTERRUPT0)
{ // Zuerst mal sehen, ob das ein Spike ist
uint8_t itest = 16;
Iztmp_0 = Izeit_1 - Iz_yseci0; //Wie lange seit der letzten Flanke
if (Iztmp_0 < itest) //wenn zu kurz (spike!?), dann
{
return; // Routine verlassen
}
else
{ // ansonsten
Iencdr0 ++; //zähle Counter/encoder0 hoch
Ienc0alt = Iencdr0;
Iz_yseci0 = Izeit_1; //Weise Iz_ysec1 dem akt. Timerwert zu
Iz_diff0 = Iz_yseci0-Iz_ysecv0; //Neue Zeit-Differenz1 ausrechnen
Iz_ysecv0 = Iz_yseci0; //der aktuelle Zeitwert wird "Alter"
}
// Bessere Spikeerkennung - dynamisch
itest = Iztmp_0 - 8;
}
/* ============================================================================== */
/* ============================================================================== */
/* === Nicht unterbrechbare ISR für EXT_INT1 auf Pin 17/PD3/mega16(32) ======== */
/* Routine setzt einfach einen Zähler hoch.
Sonst wie ISR für EXT_INT0 für Motor li,re und PWM/Geschw. */
ISR(INT1_vect) // hiess mal: ISR(SIG_INTERRUPT1)
{ // Zuerst mal sehen, ob das ein Spike ist
uint8_t itest = 10;
Iztmp_1 = Izeit_1 - Iz_yseci1; //Wie lange seit der letzten Flanke
if (Iztmp_1 < itest) //wenn zu kurz (spike!?), dann
{
return; // Routine verlassen
}
else
{ // ansonsten
Iencdr1 ++; //zähle Counter/encoder1 hoch
Iz_yseci1 = Izeit_1; //Weise Iz_ysec1 dem akt. Timerwert zu
Iz_diff1 = Iz_yseci1-Iz_ysecv1; //Neue Zeit-Differenz1 ausrechnen
Iz_ysecv1 = Iz_yseci1; //der aktuelle Zeitwert wird "Alter"
}
// Bessere Spikeerkennung - dynamisch
itest = Iztmp_1 - 8;
}
/* ============================================================================== */
/* ============================================================================== */
/* == Initialisierung der PWM zur Motoransteuerung ============================ */
void TC0PWM_init(void) //Init Timer/Counter0 für Erzeugung des PWM-Signals
{
TCCR0A |= (1<<COM0A1)|(1<<COM0B1); //Clear/set OC0A/OC0B on Compare Match,
// doc S102 , OC0A/OC0B ist Port PD6/D5
TCCR0A |= (1<<WGM01)|(1<<WGM00); // Fast PWM, TOP=0xFF=dez255, doc S104
// das ergibt aus 20 MHz mit Prescaler 1/64 1220 Hz
TCCR0B |= (1<<CS01)|(1<<CS00); // Prescaler ist clk/64 doc S106
TIMSK0 &= ~(1<<OCIE0A)|(1<<OCIE0B); // Tmr/Cntr0 Oput CompB Match intrrpt dsabld
TIMSK0 &= ~(1<<TOIE0); // Tmr/Cntr0 Overflow interrupt disabled
OCR0A = 0; // PWM auf Null setzen
OCR0B = 0; // PWM auf Null setzen
Iz_diff0 = 0;
iesum12 = 0;
ie_mot12 = 0;
}
/* ============================================================================== */
void setPWMrechts(uint8_t speed) //Geschwindigkeit rechter Motor
{OCR0A = speed;} // PWM auf PD6
/* ============================================================================== */
void setPWMlinks(uint8_t speed) //Geschwindigkeit linker Motor
{OCR0B = speed;} // PWM auf PD5
/* ============================================================================== */
/* ============================================================================== */
/* === Regelungsroutine für Motor 12 ========================================== */
/* Die gemessene Zeitdifferenz Iz_diff0 wird zur Regelung verwendet
*/
void rgl_mo_12(void) // (Wird) Regelung für Motor 12
{
if (stupsi12 <= 5) // Soll überhaupt gefahren werden?
{
OCR0A = 0;
return;
}
tupsi12 = Iz_diff0; // Übernahme Iz-Wert in Regelung
ndrz12 = stupsi12;
if ( tupsi12 > 100 ) { tupsi12 = 100; } // Eingrenzen
idrz12 = 1000 / tupsi12;
ie_mot12 = ndrz12 - idrz12; // Vergleich => Regelabweichung
iesum12 = iesum12 + ie_mot12;
if (iesum12 < -10) { iesum12 = -10;}
if (iesum12 > 500) { iesum12 = 500;}
iy12 = iesum12/2;
iy12 = iy12 + ie_mot12 + ie_mot12 + ie_mot12 + ie_mot12 + ie_mot12;
// Diese Rechenoperation benötigt in float 0,20 bis 0,25 ms!
if ( iy12 < 0 ) { iy12 = 0;}
if ( iy12 > 255 ) { iy12 = 255;}
OCR0A = iy12; // Ansteuerung der PWM direkt statt "setPWMlinks"
return;
}
/* ============================================================================== */
/* ============================================================================== */
/* === Regelungsroutine für Motor 34 ========================================== */
/* Die gemessene Zeitdifferenz Iz_diff1 wird zur Regelung verwendet
*/
void rgl_mo_34(void) // (Wird) Regelung für Motor 34
{
if (stupsi34 <= 5) // Soll überhaupt gefahren werden?
{
OCR0B = 0;
return;
}
tupsi34 = Iz_diff1; // Übernahme Iz-Wert in Regelung
ndrz34 = stupsi34;
if ( tupsi34 > 100 ) { tupsi34 = 100; } // Eingrenzen
idrz34 = 1000 / tupsi34;
ie_mot34 = ndrz34 - idrz34; // Vergleich => Regelabweichung
iesum34 = iesum34 + ie_mot34;
if (iesum34 < -10) { iesum34 = -10;}
if (iesum34 > 500) { iesum34 = 500;}
iy34 = iesum34/2;
iy34 = iy34 + ie_mot34 + ie_mot34 + ie_mot34 + ie_mot34 + ie_mot34;
if ( iy34 < 0 ) { iy34 = 0;}
if ( iy34 > 255 ) { iy34 = 255;}
OCR0B = iy34; // Ansteuerung der PWM direkt statt "setPWMxxx"
return;
}
/* ============================================================================== */
/* ============================================================================== */
/* Motoransteuerung mit dem L293D, hier werden die Drehrichtungen gesetzt */
/* Vorgesehene/belegte Anschlüsse am ATMega168 für R2D03:
==========
hier, unter anderem, für die Motoransteuerung (Stand 22mai08 11ff) :
(E7) _|-- 3,4 Guz, PD4___6 23___PC0, SFH 5110, IN irDME 1-2 } ´168 n
VCC 7 22 GND
GND 8 21 AREF
XTAL1 PB6___9 20___VCC
XTAL2 PB7 10 19 PB5, Startblink (LED0), 3sec v Tmr0, Mehrzweck
(E6) PWM 1,2 uz+Guz,PD5 11 18 PB4 _|-- 3,4 uz (E4) (hier oder PB3)
(E5) PWM 3,4 uz+Guz,PD6___2 17___PB3, Reserve 2
(E8) _|-- 1,2 uz,PD7 13 16 PB2, Reserve 1
(E9) _|-- 1,2 Guz,PB0 14 15 PB1 SFH 415, OUT (irDME) */
// -----------------------
// Drehrichtungsbefehle für Motor 1,2 = "rechter" Motor
void Mrechtszur(void) //Motor 1,2 dreht im Uhrzeigersinn
{PORTB |= (1<<PB4); PORTD &= ~(1<<PD4);}
// r r r r r r r rechts rechter Motor r r r r r r
void Mrechtsvor(void) //Motor dreht im mathematischer Drehsinn
{PORTB &= ~(1<<PB4); PORTD |= (1<<PD4);}
// r r r r r r r rechts rechter Motor r r r r r r
void Mrechtsstop(void) //Motor aus
{ PORTB &= ~(1<<PB4); PORTD &= ~(1<<PD4);}
// -----------------------
// Drehrichtungsbefehle für Motor 3,4 = "linker" Motor
void Mlinkszur(void) //Motor 3,4 dreht im Uhrzeigersinn
{PORTB |= (1<<PB0); PORTD &= ~(1<<PD7);}
// l l l l l l l links linker Motor l l l l l l l
void Mlinksvor(void) //Motor dreht im mathematischer Drehsinn
{PORTB &= ~(1<<PB0); PORTD |= (1<<PD7);}
// l l l l l l l links linker Motor l l l l l l l
void Mlinksstop(void) //Motor aus
{ PORTB &= ~(1<<PB0); PORTD &= ~(1<<PD7);}
/* ============================================================================== */
/* ============================================================================== */
/* ===== ENDE Subroutinen ================================================== */
/* ============================================================================== */
Ausserdem könntest Du Dir mal eine (funktionierende) Motorregelung ansehen. Aber - wie gesagt - das ist eher für Mussestunden geeignet - *gggg*: nicht zum Lesen *gggg*.
Zitieren
Zitat von juhecomp, Heute um 6:53,
Berechtigungen
Lesezeichen