Sie befinden sich hier: start » electronics » projects » legorobot » software

Seitenleiste

Community

Tutorials

Einstieg

Nativ

Web-Programmierung

Fachthemen

mehr...

->

Inhaltsverzeichnis

Software

Roboter

Der Roboter muss die empfangenen Signale auswerten und danach den Motor und den Servo steuern. Zusätzlich muss er die Distanzen der Sensoren auslesen und auswerten, um nicht gegen Wände zu fahren. Als Steuerprotokoll wird 3B-8(3-Block 8-Bit) verwendet, der Code für das Funkmodul hier:http://www.mikrocontroller.net/articles/RFM12: 

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <avr/eeprom.h>
#include <stdlib.h>
#include "global.h"
#include "rf01.h"
#include <util/delay.h>
 
#define LED_PORT PORTC
#define LED_RED PC3
#define LED_GREEN PC4
#define LED_RED_ON() LED_PORT |= (1 << LED_RED)
#define LED_GREEN_ON() LED_PORT |= (1 << LED_GREEN)
#define LED_GREEN_OFF() LED_PORT &= ~(1 << LED_GREEN)
#define LED_RED_OFF() LED_PORT &= ~(1 << LED_RED)
 
#define MOTOR1_A PD5
#define MOTOR1_B PD6
#define MOTOR2_A PD7
#define MOTOR2_B PB0
#define MOTOR_PORT PORTD
#define MOTOR_PORTB PORTB
 
#define SERVOPIN PC2
#define SERVOPORT PORTC
#define DDRSERVO DDRC
 
#define US_SENSOR_1 1
#define US_SENSOR_2 2
#define US_READY 0
#define US_SENSOR_PORT PORTD
#define US_SENSOR1_TRIG PD0
#define US_SENSOR2_TRIG PD3
#define US_SENSOR1_ECHO PD1
#define US_SENSOR2_ECHO PD4
 
volatile unsigned char servopos;
uint8_t irDistRef = 0;		//IR-Distance sensor reference
volatile uint8_t irDistVal = 0;	//IR-Distance sensor value
uint8_t usSensor1Dist = 0;
uint8_t usSensor2Dist = 0;
volatile uint8_t next_usSensor1Dist = 0;
volatile uint8_t next_usSensor2Dist = 0;
volatile uint8_t actualSensor = 0;
uint8_t usDistRef = 0;		//US-Distance sensors reference
 
void servo_init(void)
{
	TIMSK|=(1<<OCIE2);
	TCCR2 |= (1<<WGM21) | (1<<CS20);	//Prescale=1, CTC mode
	OCR2 = F_CPU/100000;			//alle 10µS ein IRQ
	DDRSERVO |= (1 << SERVOPIN);
}
 
ISR(TIMER2_COMP_vect)
{
	cli();
	static int count;
	if(count > servopos) SERVOPORT &= ~(1 << SERVOPIN);
	  else SERVOPORT |= (1 << SERVOPIN);
	if(count < 2000) count++; // Die Impulse sollten alle 20ms gesendet werden! 6.2.11 mic
	  else count=0;
	sei();
};
 
enum Richtung
{
	halt = 0,
	vorwaerts,
	rueckwaerts,
	stopp
}direction;
 
void Delay_ms(unsigned char amS)
{
	_delay_ms(amS);
}
 
 
//Setzten der Richtung und Geschwindigkeit des ersten Motors
void Motor1(uint8_t speed, enum Richtung richtung)
{
	//if(irDistVal > irDistRef/* || usSensor1Dist < usDistRef || usSensor1Dist < usDistRef*/)
		//richtung = (richtung == rueckwaerts)?(richtung):(halt);	//Nur rückwärts zulassen
	switch(richtung)
	{
	case halt:
		PORTD &= ~((1 << MOTOR1_A) | (1 << MOTOR1_B));
		break;
	case vorwaerts:
		PORTD &= ~((1 << MOTOR1_A) | (1 << MOTOR1_B));
		PORTD |= (1 << MOTOR1_A);
		OCR1A = speed;
		break;
	case rueckwaerts:
		PORTD &= ~((1 << MOTOR1_A) | (1 << MOTOR1_B));
		PORTD |= (1 << MOTOR1_B);
		OCR1A = speed;
		break;
	case stopp:
		PORTD |= ((1 << MOTOR1_A) | (1 << MOTOR1_B));
		break;
	}
}
 
//Setzen der Richtung und Geschwidnigkeit des zweiten Motors
void Motor2(uint8_t speed, enum Richtung richtung)
{
	//if(irDistVal > irDistRef/* || usSensor1Dist < usDistRef || usSensor1Dist < usDistRef*/)
	//	richtung = (richtung == rueckwaerts)?(richtung):(halt);	//Nur rückwärts zulassen
	switch(richtung)
	{
	case halt:
		PORTD &= ~(1 << MOTOR2_A);
		PORTB &= ~(1 << MOTOR2_B);
		break;
	case vorwaerts:
		PORTD |= (1 << MOTOR2_A);
		PORTB &= ~(1 << MOTOR2_B);
		OCR1A = speed;
		break;
	case rueckwaerts:
		PORTD &= ~((1 << MOTOR2_A));
		PORTB |= (1 << MOTOR2_B);
		OCR1A = speed;
		break;
	case stopp:
		PORTD |= (1 << MOTOR2_A);
		PORTB |= (1 << MOTOR2_B);
		break;
	}
}
 
//Setze den Servo auf den Angegebenen Winkel
void SetServo(int8_t value)
{
	servopos = value;
}
 
//ADC conversion result
ISR(ADC_vect)
{
	irDistVal = ADCH;
};
 
void SelfTest(void)
{
	Motor1(255, direction = vorwaerts);
	Delay_ms(100);
	Motor1(255, direction = rueckwaerts);
	Delay_ms(100);
	Motor1(0, direction = halt);
	Delay_ms(100);
	Motor2(255, direction = vorwaerts);
	Delay_ms(100);
	Motor2(255, direction = rueckwaerts);
	Delay_ms(100);
	Motor2(0, direction = halt);
	SetServo(100);
	Delay_ms(100);
	SetServo(128);
}
 
//Execute a command recived
void ExecuteCmd(uint8_t cmd, uint8_t data)
{
	switch(cmd)
	{
	case 0:	//Nothing
		return;
		break;
	case 1:	//self-test
		SelfTest();
		break;
	case 2:		//forward
		Motor1(data, direction = vorwaerts);
		Motor2(data, direction = vorwaerts);
		break;
	case 3:		//backward
		Motor1(data, direction = rueckwaerts);
		Motor2(data, direction = rueckwaerts);
		break;
	case 4:		//servo angle
		SetServo(data);
		break;
	case 5:		//Motor 1 forward
		Motor1(data, direction = vorwaerts);
		break;
	case 6:		//Motor 1 backward
		Motor1(data, direction = rueckwaerts);
		break;
	case 7:		//Motor 2 forward
		Motor2(data, direction = vorwaerts);
		break;
	case 8:		//Motor 2 backward
		Motor2(data, direction = rueckwaerts);
		break;
	case 9:		//halt
		Motor1(data, direction = halt);
		Motor2(data, direction = halt);
		break;
	case 10:	//stopp
		Motor1(data, direction = stopp);
		Motor2(data, direction = stopp);
		break;
	case 11:
		irDistRef = data;
		break;
	case 12:
		usDistRef = data;
		break;
	}
}
 
unsigned char data[32];
 
int main(void)
{
	DDRD |= (1 << PD5) | (1 << PD6) | (1 << PD7);
	DDRB |= (1 << PB0) | (1 << PB1);
	DDRC |= (1 << LED_RED) | (1 << LED_GREEN);
	//Set Pins for US-Sensor
	DDRD |= (1 << US_SENSOR1_TRIG) | (1 << US_SENSOR2_TRIG);
	DDRD &= ~((1 << US_SENSOR1_ECHO) | (1 << US_SENSOR2_ECHO));
	uint8_t state = 1;
	LED_RED_ON();
	LED_GREEN_ON();
	sei();
	ADMUX |= (1 << REFS0) | (1 << MUX2) | (1 << MUX0) | (1 << ADLAR);	//AVcc, ADC5, Left-Adjust
	ADCSRA |= (1 << ADIE) | (1 << ADPS2) | (1 << ADFR) | (1 << ADEN) | (1 << ADSC);		//Interrupt enable, Prescaler = 16, freerunning mode
 
	/*TCCR1B =(1 << WGM12) | (1 << CS01) | (1 << CS00);		//CTC, Prescaler = 8;
	OCR1A = 116;	//alle 58µs*/
	//Timer wird hier anderweitig gebruacht
	PORTB &= ~(1 << PB1);
	TCCR1A = (1 << COM1A1) | (1 << WGM10);
	TCCR1B = (1 << WGM12) | (1 << CS12) | (1 << CS10);
	OCR1A = 0;	//Speed auf 0
 
	servo_init();
	SetServo(80);
	Delay_ms(00);
	SetServo(180);
	Delay_ms(500);
	SetServo(100);
 
	SetServo(125);
 
	rf01_init();					// ein paar Register setzen (z.B. CLK auf 10MHz)
	rf01_setfreq(RF01FREQ(433.92));	// Sende/Empfangsfrequenz auf 433,92MHz einstellen
	rf01_setbandwidth(4);			// 200kHz Bandbreite
	rf01_setreceiver(2,4);			// -6dB Verstärkung, DRSSI threshold: -79dBm 
	rf01_setbaud(19200);			// 19200 Baud
 
	LED_GREEN_OFF();
	while(1)
	{
		LED_GREEN_OFF();
		rf01_rxdata(data, 12);
		LED_GREEN_ON();
		for(int i = 0; i < 10; i++)
		{
			if(data[i] != 0) continue;
			ExecuteCmd(data[i+1], data[i+2]);
		}
		//USStartMeasurment(state + 1);	//Abwechselnd beide Sensoren abfragen		//Wir Verzichten mal hier drauf
		state = 1-state;
	}
	return 0;
}

Funk Sender

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <avr/eeprom.h>
#include <stdlib.h>
#include "global.h"
#include "rf02.h"
 
#define F_CPU 16000000UL
#include <util/delay.h>
 
#define LED_PORT PORTB
#define LED_RED PB0
#define LED_RED_ON() LED_PORT |= (1 << LED_RED)
#define LED_RED_OFF() LED_PORT &= ~(1 << LED_RED)
 
//Empfangsbuffer
volatile uint8_t txBuffer[12];
volatile uint8_t position = 0;
volatile uint8_t transmit = 0;
 
void uart_putc(unsigned char c)
{
	while (!(UCSR0A & (1<<UDRE0)));
	UDR0 = c;                      /* sende Zeichen */
}
 
ISR(USART_RX_vect)
{
	LED_RED_ON();
	uint8_t data = UDR0;
	if(position == 12)	//3 Datensiganle zum weiterleiten
	{
		transmit = 1;
		position = 0;
		LED_RED_OFF();
		return;
	}
	txBuffer[position] = data;
	position++;
	LED_RED_OFF();
}
 
void UARTSend(uint8_t *buffer, uint8_t length)
{
	for(; length > 0;length--)
	{
		while (!(UCSR0A & (1<<UDRE0)));
		UDR0 = *(buffer++);
	}
}
 
void uart_puts (char *s)
{
  while (*s)
  {   /* so lange *s != '\0' also ungleich dem "String-Endezeichen" */
      uart_putc(*s);
      s++;
  }
}
void Delay_ms(unsigned char amS)
{
	_delay_ms(amS);
}
int main(void)
{
	DDRD = (1 << PD1);
	DDRB |= (1 << LED_RED);
	UBRR0H = 0;
	UBRR0L = 51;		//19200 baud
	UCSR0B = (1 << RXCIE0) | (1 << RXEN0) | (1 << TXEN0);		//Interrupt enable, Recieve enable, Transmit Enable
	sei();
 
	uart_puts("Start");
 
	rf02_init();					// ein paar Register setzen (z.B. CLK auf 10MHz)
	rf02_setfreq(RF02FREQ(433.92));	// Sende/Empfangsfrequenz auf 433,92MHz einstellen
	rf02_setpower(4);				// -12dBm Ausgangangsleistung
	rf02_setmodfreq(3);				// 120kHz Frequenzshift
	rf02_setbaud(19200);			// 19200 Baud
	while(1)
	{
		if(transmit == 0) continue;
		uart_putc('s');
		LED_RED_ON();
		cli();
		rf02_txdata(txBuffer,12);
		sei();
		transmit = 0;
		uart_putc('x');
		LED_RED_OFF();
	}
	return 0;
}

Windows Software

Die Windows-Sofwtare ist etwas komplizierter, da sie in C# geschrieben ist, eine grafische Oberfläche besitzt und den TinkerForge Joystick zur Steuerung des Roboters nutzt. FIXME: Code/Programm veröffentlichen