Genaue Motorsteuerung mit HALL Sensor

**sthomson** · 27.05.2010, 19:58

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 ************************		
// **********************************************************************

Thema: Genaue Motorsteuerung mit HALL Sensor

Themen-Optionen

Thema durchsuchen

Anzeige

Baum-Darstellung

Danke für den Hinweis, hier also mein Quellcode

Lesezeichen

Lesezeichen

Berechtigungen