Genaue Motorsteuerung mit HALL Sensor
Hallo zusammen
ich arbeite im Moment an einer X-Y Steuerung mit zwei 12V Getriebemotoren. Ich möchte mit meinem ATMEL mega8 die Umdrehungen der Motoren überwachen und damit kontrollierte Bewegungen z.B 100 Umdrehungen in eine bestimmte Richtung fahren.
Wichtigstes Ziel: Dabei keine Schritte verlieren und das Ziel möglicht genau treffen.
Die Motoren sind jeweils mit einem HALL Sensoren bestückt. Auf der Motorachse ist ein kleinen runden Magneten aufgeklebt, daneben sitzt der Hallsensor Beschriftung: 177 823B. Dieser liefert ein einfaches Rechteck im TTL Pegel.
Da ich die Drehrichtung ja softwareseitig vorgebe, dachte ich mir einfach die Flanken zu zählen.
Ich arbeite mit einem mega8 der über eine Brücke L289 die Motoren ansteuert.
Ich will über die beiden externen interrupts INT0 und INT1 die Flanken zählen und die Motoren mit einem PWM Signal quasi an die gewünschte Position heranfahren.
Zunächst mit nur einem Motor hat das ganz gut geklappt und ich habe bis auf +-2 Umdrehungen immer ganz gut meine Marke getroffen.
Als ich allerdings die zweite Achse in mein Programm aufgenommen habe, ging es mit den Problemen los.
Aus mir bisher unerfindlichen Gründen läuft Achse 1 zwar, aber die Anzahl der gezählten Schritte erscheint mir eher zufällig. Achse 2 über INT 2 läuft dafür unaufhörlich was vermuten lässt, dass gar nichts gezählt wird.
Hat vielleicht jemand Erfahrunge mit so einer Motorsteuerung über einen einfachen HALL Drehgeber? Was ist hier ein sinnvoller Ansatz?
Wie stelle ich sicher, dass keine Schritte verlohren gehen?
Ich programmiere in C++ und würde mich riesig über eine paar Denkanstöße freuen.
Beste Grüße
Sven
Danke für den Hinweis, hier also mein Quellcode
Code:
//---------------------------------------------------------------------------
// Title : DC Motor Steuerung V7.0
// Schaltung : Steuert zwei DC Motoren PAN&TILT über Tasten NORD, SÜD, OST, WEST
// Motoren werden über PWM Ausgänge betrieben, Hall Geber über ext Interrut,
// Anzahl der Schritte über SW vorgebene
//----------------------------------------------------------------------
// Prozessor : ATmega8
// Takt : 3.6864 MHz
// Sprache : C
// Datum : 27.05.2010
// Version : 9.0
// Autor : Sven Thomson
// Programmer : ...
// Port : ...
//---------------------------------------------------------------------------
#define F_CPU 3686400 // Taktfrequenz des myAVR-Boards
#include <avr\io.h> // AVR Register und Konstantendefinitionen
#include <util\delay.h> // Bibliothek mit Warteroutinen
#include <math.h>
#include <inttypes.h>
#include <avr\interrupt.h>
//----------------------------------------------------------------------
//------------------------Globale VARIABLEN-----------------------------
// Varaiablen für PAN-Motor
int PRichtung=0; // PAN Drehrichtung Osten:-1, Stillstand:0, Westen=1
int PZaehler_A=0; // PAN Zählerstand am Anfang
int PKorrektur=0; // PAN Korrekturwert
int PWeg_Kor=0; // PAN korrigierter Weg
// Varaiablen für TILT-Motor
int TRichtung=0; // TILT Drehrichtung Norden:-1, Stillstand:0, Süden=1
int TZaehler_A=0; // TILT Zählerstand am Anfang
int TKorrektur=0; // TILT Korrekturwert
int TWeg_Kor=0; // TILT korrigierter Weg
//durch interrupt veränderbare Varaiablen
volatile int PZaehler=0; // PAN Zähler der HALL Impulse
volatile int TZaehler=0; // TILT Zähler der HALL Impulse
// Der Modifizierer "volatile" teilt dem Compiler mit,
// daß eine Variable möglicherweise von einer
// Hintergrundroutine, einer Interruptroutine
// oder einem I/O-Port verändert wird.
//---------------------------------------------------------------------
//----------------INTERRUPT ROUTINE EXTERN ÜBER INT0-------------------
ISR (INT0_vect) // Interruptroutine für den INT0 - PortD2
{ // hier Hall Sensor Auswertung PAN (weisse Leitung)
PZaehler++; // sobald Flanke am PAN Motor, Zähler erhöhen
}
//----------------INTERRUPT ROUTINE EXTERN ÜBER INT1-------------------
ISR (INT1_vect) // Interruptroutine für den INT1 - PortD3
{ // hier Hall Sensor Auswertung TILT (blaue Leitung)
TZaehler++; // sobald Flanke am TILT Motor, Zähler erhöhen
}
//----------------ALLEGEMIEN DELAY FUNKTION ------------------------------------------------------
void delay_us(unsigned int count)
{
_delay_loop_2(count);
}
//----------------PWM AUSGÄNGE B1, B2 INITIALISIEREN------------------------------------------------------
void initPWM()
{
DDRB=0b00000110; // PWM Ausgänge B1, B2
TCCR1A=0b10100011; // Timer/Counter 1 Control Register A
TCCR1B=0b00000010; // Timer/Counter 1 Control Register B
}
//----------------PORTS INITILAISIEREN ------------------------------------------------------
void initPORTS()
{
DDRB = 0b11111111; // PORT-B = als Ausgang festlegen
DDRC = 0b00000000; // PORT-C = als Eingang festlegen
PORTC = 0b00111111; // PORT-C = PULL-UP
DDRD = 0b00000000; // PORT-D = als Eingang festlegen
PORTD = 0b11111111; // PORT-D = PULL-UP
}
//----------------EXTERE INTERRUPT EINGÄNGE INITIALISIEREN------------------------------------------------------
void initEXISR()
{
GICR = 0b01000000; // für mega8 - Bit 7,6 - Exteren Interrupt einschalten für INT1, INT0 also PORTD3,2
//EIMSK = 0b00000011; // für mega 168 - Bit 1,0 - Exteren Interrupt einschalten für int1, int0 also PORTD3,2
MCUCR = 0b00001010; // Interrupt bei fallende Flanke, int0:PORTD2 - für PAN, int1:PORTD3 - für TILT
}
//----------------UART INITIALISIERUNG------------------------------------------------------
void initUART()
{
UBRRL = 23; //für mega 8 - 9600Baud siehe Baudratentabelle
UCSRB = 8 + 16; //Sender enable, (Empfänger enable)
}
//----------------UART SENDEFUNKTION FÜR 1 ZEICHEN (8 BIT)-------------------
void putChar (char data) // sendet nur ein 8Bit Zeichen
{
//warte bis UDR leer ist UCSRA / USR bei z.B.: 2313
while (!(UCSRA&32));
//sende
UDR=data;
}
//----------------UART SENDEFUNKTION FÜR ZEICHENKETTE------------------------
void putChar(char *buffer)
{
for (int i=0; buffer[i] !=0;i++)
putChar (buffer[i]);
}
//----------------UART SENDEFUNKTION FÜR ZAHL 16BIT------------------------
void uart_uint(uint16_t zahl)
{
unsigned char text[20];
int ziffer;
int i = 0;
do
{
ziffer = zahl % 10; // ziffer ist die erste Ziffer 128->8
zahl = ( zahl - ziffer ) / 10; // zahl ist nun Restziffer(n) 128->12
text[i++] = ziffer + 0x30;
}
while ( zahl != 0 ); // solange Restziffern da sind
while (i > 0)
{
putChar( text[--i] );
}
}
//----------------TASTENAUSWERTUNG für PAN & TILT MIT RICHTUNG------------------------
void TastenSTG()
{
if (!(PINC&0b00000100)) PRichtung=1; // PAN, Bit 2 = 0 -> Drehrichtung WESTEN
if (!(PINC&0b00001000)) PRichtung=-1; // PAN, Bit 3 = 0 -> Drehrichtung OSTEN
if (!(PINC&0b00010000)) TRichtung=1; // TILT, Bit 4 = 0 -> Drehrichtung SÜDEN
if (!(PINC&0b00100000)) TRichtung=-1; // TILT, Bit 5 = 0 -> Drehrichtung NORDEN
}
//------------------------------- MOTORSTEUERUNG---------------------------------
//--------------------------------------------------------------------------------
// ---------------------------- PAN MOTOR --------------------------------
void PanMotorGo(int PDir, int PWeg)
{
while (PDir==1) // PAN Drehrichtung -> WESTEN
{
PDir=0; // setzte Drehrichtung wieder auf null -> damit nur ein Durchlauf
putChar(" PZ_:"); // Kontrollausgabe
uart_uint(PZaehler); // PAN Zählerstand über UART
putChar("B"); // Kontrollausgabe, Zeilenumbruch
PKorrektur=PZaehler-PZaehler_A-PWeg_Kor; // Berechne Korrekturwert für nächsten lauf
PZaehler_A=PZaehler; // merke Zählerstand vor dem Lauf
PWeg_Kor=PWeg-PKorrektur; // Berechne korrigierten Weg
while (PZaehler<PZaehler_A+PWeg_Kor-5) // Laufe solange bis gewünschter Zählerstand-Rampe erreicht
{
OCR1A=1000; // schneller Lauf, mit hohem PWM Wert
PORTB|=0b00000001; // Laufe Drehrichtung WESTEN
}
while (PZaehler<PZaehler_A+PWeg_Kor) // Laufe Rest solange bis Rampe zuende
{
OCR1A=500; // lansamer Lauf, mit reduziertem PWM Wert
PORTB|=0b00000001; // Laufe Drehrichtung WESTEN
}
}
while (PDir==-1) // PAN Drehrichtung -> OSTEN
{
PDir=0; // setzte Drehrichtung wieder auf null -> damit nur ein Durchlauf
putChar(" PZ_:"); // Kontrollausgabe
uart_uint(PZaehler); // PAN Zählerstand über UART
putChar("B"); // Kontrollausgabe, Zeilenumbruch
PKorrektur=PZaehler-PZaehler_A-PWeg_Kor; // Berechne Korrekturwert für nächsten lauf
PZaehler_A=PZaehler; // merke Zählerstand vor dem Lauf
PWeg_Kor=PWeg-PKorrektur; // Berechne korrigierten Weg
while (PZaehler<PZaehler_A+PWeg_Kor-5) // Laufe solange bis gewünschter Zählerstand- Rampe erreicht
{
OCR1A=1000; // schneller Lauf, mit hohem PWM Wert
PORTB|=0b00001000; // Laufe Drehrichtung OSTEN
}
while (PZaehler<PZaehler_A+PWeg_Kor) // Laufe Rest solange bis Rampe zuende
{
OCR1A=500; // lansamer Lauf, mit reduziertem PWM Wert
PORTB|=0b00001000; // Laufe Drehrichtung OSTEN
}
}
PORTB&=0b11110110; // Keine Taste gedrückt -> also STOP, Bit 0&3->0
}
// ---------------------------- TILT MOTOR --------------------------------
void TiltMotorGo(int TDir, int TWeg)
{
while (TDir==1) // TILT Drehrichtung -> SÜDEN
{
TDir=0; // setzte Drehrichtung wieder auf null -> damit nur ein
putChar(" TZ_:"); // Kontrollausgabe
uart_uint(TZaehler); // TILT Zählerstand über UART
putChar("B"); // Kontrollausgabe, Zeilenumbruch
TKorrektur=TZaehler-TZaehler_A-TWeg_Kor; // Berechne Korrekturwert für nächsten lauf
TZaehler_A=TZaehler; // merke Zählerstand vor dem Lauf
TWeg_Kor=TWeg-TKorrektur; // Berechne korrigierten Weg
while (TZaehler<TZaehler_A+TWeg_Kor-5) // Laufe solange bis gewünschter Zählerstand-Rampe erreicht
{
OCR1B=1000; // schneller Lauf, mit hohem PWM Wert
PORTB|=0b000010000; // Laufe Drehrichtung SÜDEN
}
while (TZaehler<TZaehler_A+TWeg_Kor) // Laufe Rest solange bis Rampe zuende
{
OCR1B=500; // lansamer Lauf, mit reduziertem PWM Wert
PORTB|=0b000010000; // Laufe Drehrichtung SÜDEN
}
}
while (TDir==-1) // TILT Drehrichtung -> NORDEN
{
TDir=0; // setzte Drehrichtung wieder auf null -> damit nur ein
putChar(" TZ_:"); // Kontrollausgabe
uart_uint(TZaehler); // TILT Zählerstand über UART
putChar("B"); // Kontrollausgabe, Zeilenumbruch
TKorrektur=TZaehler-TZaehler_A-TWeg_Kor; // Berechne Korrekturwert für nächsten lauf
TZaehler_A=TZaehler; // merke Zählerstand vor dem Lauf
TWeg_Kor=TWeg-TKorrektur; // Berechne korrigierten Weg
while (TZaehler<TZaehler_A+TWeg_Kor-5) // Laufe solange bis gewünschter Zählerstand- Rampe erreicht
{
OCR1B=1000; // schneller Lauf, mit hohem PWM Wert
PORTB|=0b000100000; // Laufe Drehrichtung NORDEN
}
while (TZaehler<TZaehler_A+TWeg_Kor) // Laufe Rest solange bis Rampe zuende
{
OCR1B=500; // lansamer Lauf, mit reduziertem PWM WertM
PORTB|=0b000100000; // Laufe Drehrichtung NORDEN
}
}
PORTB&=0b11001111; // Keine Taste gedrückt -> also STOP, Bit 4&5->0
}
// ********************* HAUPTBPROGRAMMTEIL ******************************
// **********************************************************************
// **********************************************************************
main (void)
{
// ************************ Variablen *******************************
// ********************* Initialisierungen **************************
initPWM(); //Initialisiere PWM Ausgänge
initPORTS(); //Initialisiere die Ports
initEXISR(); //Initialisiere den Exteren Interrupt
initUART(); //Initialisiere UART für Kontrollausgabe
sei(); //Ermögliche Interrupts
// ************************* HAUPTSCHLEIFE *****************************
// **********************************************************************
while (true) // Mainloop
{
TastenSTG(); // Tastenabfrage für Motorstart mit jew. Richtung
PanMotorGo(PRichtung,100); // Fahre PAN Achse in jew. Richtung mit 100 Schritten
TiltMotorGo(TRichtung,100); // Fahre TILT Achse in jew. Richtung mit 100 Schritten
_delay_ms(100); // warte 100 ms bis Motor endgülig steht
// AUSGABE der Variablen über UART zu Kontrollzwecken
if (PRichtung!=0) // Einmalige Kontrollausgabe über UART nach PAN Lauf
{
putChar(" PK:"); // Kontrollausgabe PAN Korrekturwert
uart_uint(PKorrektur);
putChar(" PZ_A:"); // Kontrollausgabe PAN Zählerstand am Anfang
uart_uint(PZaehler_A);
putChar(" PW_K:"); // Kontrollausgabe PAN korrigierter Weg
uart_uint(PWeg_Kor);
putChar(" PZ:"); // Kontrollausgabe PAN Zähler der HALL Impulse
uart_uint(PZaehler);
putChar("B"); // Zeilenumbruch
putChar("B"); // Zeilenumbruch
}
if (TRichtung!=0) // Einmalige Kontrollausgabe über UART nach TILT Lauf
{
putChar(" TK:"); // Kontrollausgabe TILT Korrekturwert
uart_uint(TKorrektur);
putChar(" TZ_A:"); // Kontrollausgabe TILT Zählerstand am Anfang
uart_uint(TZaehler_A);
putChar(" TW_K:"); // Kontrollausgabe TILT korrigierter Weg
uart_uint(TWeg_Kor);
putChar(" TZ:"); // Kontrollausgabe TILT Zähler der HALL Impulse
uart_uint(TZaehler);
putChar("B"); // Zeilenumbruch
putChar("B"); // Zeilenumbruch
}
PRichtung=0; // PAN Drehrichtung wieder auf Null -> Motor Stop
TRichtung=0; // TILT Drehrichtung wieder auf Null -> Motor Stop
}
}
// ************************* ENDE HAUPTSCHLEIFE ************************
// **********************************************************************
