747 lines
18 KiB
C
747 lines
18 KiB
C
/*
|
|
* c't-Sim - Robotersimulator fuer den c't-Bot
|
|
*
|
|
* This program is free software; you can redistribute it
|
|
* and/or modify it under the terms of the GNU General
|
|
* Public License as published by the Free Software
|
|
* Foundation; either version 2 of the License, or (at your
|
|
* option) any later version.
|
|
* This program is distributed in the hope that it will be
|
|
* useful, but WITHOUT ANY WARRANTY; without even the implied
|
|
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
|
|
* PURPOSE. See the GNU General Public License for more details.
|
|
* You should have received a copy of the GNU General Public
|
|
* License along with this program; if not, write to the Free
|
|
* Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
|
|
* MA 02111-1307, USA.
|
|
*
|
|
*/
|
|
|
|
/*! @file ct-Bot.c
|
|
* @brief Demo-Hauptprogramm
|
|
* @author Benjamin Benz (bbe@heise.de)
|
|
* @date 26.12.05
|
|
*/
|
|
|
|
#include "ct-Bot.h"
|
|
|
|
#ifdef MCU
|
|
#include <avr/io.h>
|
|
#include <avr/interrupt.h>
|
|
// #include <avr/signal.h>
|
|
#include <avr/wdt.h>
|
|
#include "bot-2-pc.h"
|
|
#endif
|
|
|
|
#ifdef PC
|
|
#include "bot-2-sim.h"
|
|
#include "tcp.h"
|
|
#include "tcp-server.h"
|
|
#include <pthread.h>
|
|
#include <unistd.h>
|
|
#include <stdlib.h>
|
|
#include <time.h>
|
|
#include <sys/time.h>
|
|
#endif
|
|
|
|
#ifdef TWI_AVAILABLE
|
|
#include "TWI_driver.h"
|
|
#endif
|
|
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
|
|
#include "global.h"
|
|
#include "display.h"
|
|
#include "led.h"
|
|
#include "ena.h"
|
|
#include "shift.h"
|
|
#include "delay.h"
|
|
#include "uart.h"
|
|
#include "adc.h"
|
|
#include "timer.h"
|
|
#include "sensor.h"
|
|
#include "log.h"
|
|
|
|
|
|
#include "motor.h"
|
|
#include "sensor-low.h"
|
|
#include "bot-logic/bot-logik.h"
|
|
#include "mouse.h"
|
|
|
|
#include "command.h"
|
|
#include "ir-rc5.h"
|
|
#include "rc5.h"
|
|
#include "timer.h"
|
|
#include "mmc.h"
|
|
#include "map.h"
|
|
#include "mmc-emu.h"
|
|
#include "mini-fat.h"
|
|
|
|
/* Nimmt den Status von MCU(C)SR bevor dieses Register auf 0x00 gesetzt wird */
|
|
#ifdef DISPLAY_SCREEN_RESETINFO
|
|
uint8 reset_flag;
|
|
#endif
|
|
|
|
/* Enthaelt den Start des Heaps und ermoeglicht z.B. die Berechnung des verwendeten RAMs */
|
|
#ifdef MCU
|
|
extern unsigned char __heap_start;
|
|
#endif
|
|
|
|
#ifdef TEST_AVAILABLE_COUNTER
|
|
#include <avr/eeprom.h>
|
|
uint8 resetsEEPROM __attribute__ ((section (".eeprom")))=0;
|
|
uint8 resetInfoEEPROM __attribute__ ((section (".eeprom")));
|
|
uint8 resets;
|
|
#endif
|
|
/*!
|
|
* Der Mikrocontroller und der PC-Simulator brauchen ein paar Einstellungen,
|
|
* bevor wir loslegen koennen.
|
|
*/
|
|
void init(void){
|
|
|
|
#ifdef MCU
|
|
PORTA=0; DDRA=0; //Alles Eingang alles Null
|
|
PORTB=0; DDRB=0;
|
|
PORTC=0; DDRC=0;
|
|
PORTD=0; DDRD=0;
|
|
|
|
// Watchdog aus!
|
|
wdt_disable();
|
|
timer_2_init();
|
|
|
|
// Ist das ein Power on Reset ?
|
|
#ifdef __AVR_ATmega644__
|
|
if ((MCUSR & 1) ==1 ) {
|
|
MCUSR &= ~1; // Bit loeschen
|
|
#else
|
|
if ((MCUCSR & 1) ==1 ) {
|
|
MCUCSR &= ~1; // Bit loeschen
|
|
#endif
|
|
delay(100);
|
|
asm volatile("jmp 0");
|
|
}
|
|
|
|
delay(100);
|
|
#ifdef DISPLAY_SCREEN_RESETINFO
|
|
#ifdef __AVR_ATmega644__
|
|
reset_flag = MCUSR & 0x1F; //Lese Grund fuer Reset und sichere Wert
|
|
MCUSR = 0; //setze Register auf 0x00 (loeschen)
|
|
#else
|
|
reset_flag = MCUCSR & 0x1F; //Lese Grund fuer Reset und sichere Wert
|
|
MCUCSR = 0; //setze Register auf 0x00 (loeschen)
|
|
#endif
|
|
#endif
|
|
#endif
|
|
|
|
#ifdef UART_AVAILABLE
|
|
uart_init();
|
|
#endif
|
|
|
|
#ifdef BOT_2_PC_AVAILABLE
|
|
bot_2_pc_init();
|
|
#endif
|
|
|
|
|
|
#ifdef PC
|
|
bot_2_sim_init();
|
|
#endif
|
|
|
|
#ifdef DISPLAY_AVAILABLE
|
|
display_init();
|
|
display_update=1;
|
|
#endif
|
|
|
|
#ifdef LED_AVAILABLE
|
|
LED_init();
|
|
#endif
|
|
|
|
motor_init();
|
|
bot_sens_init();
|
|
#ifdef BEHAVIOUR_AVAILABLE
|
|
bot_behave_init();
|
|
#endif
|
|
|
|
#ifdef MCU
|
|
#ifdef RC5_AVAILABLE
|
|
ir_init();
|
|
#endif
|
|
#endif
|
|
|
|
#ifdef MMC_AVAILABLE
|
|
mmc_init();
|
|
#endif
|
|
|
|
#ifdef MAUS_AVAILABLE
|
|
maus_sens_init();
|
|
#endif
|
|
|
|
#ifdef MAP_AVAILABLE
|
|
map_init();
|
|
#endif
|
|
|
|
|
|
#ifdef DISPLAY_BEHAVIOUR_AVAILABLE
|
|
behaviour_page = 1;
|
|
#endif
|
|
|
|
#ifdef TWI_AVAILABLE
|
|
Init_TWI();
|
|
Close_TWI();
|
|
#endif
|
|
}
|
|
|
|
#ifdef DISPLAY_AVAILABLE
|
|
|
|
/*!
|
|
* Zeigt ein paar Informationen an
|
|
*/
|
|
void display(void){
|
|
|
|
#ifdef DISPLAY_BEHAVIOUR_AVAILABLE
|
|
/*!
|
|
* Definitionen fuer die Verhaltensanzeige
|
|
*/
|
|
#undef TEST_AVAILABLE_COUNTER
|
|
Behaviour_t *ptr = behaviour;
|
|
int8 colcounter = 0;
|
|
int8 linecounter = 0;
|
|
int8 firstcol = 0;
|
|
#endif
|
|
#ifdef MCU
|
|
#ifndef LOG_DISPLAY_AVAILABLE
|
|
#ifdef DISPLAY_SCREENS_AVAILABLE
|
|
uint16 frei; // enthaelt im Screen 5 den freien RAM-Speicher
|
|
#endif // DISPLAY_SCREENS_AVAILABLE
|
|
#endif
|
|
#endif
|
|
#ifdef TEST_AVAILABLE_COUNTER
|
|
static int counter=0;
|
|
#endif
|
|
if (display_update >0)
|
|
#ifdef DISPLAY_SCREENS_AVAILABLE
|
|
switch (display_screen) {
|
|
case 0:
|
|
#endif
|
|
display_cursor(1,1);
|
|
display_printf("P=%03X %03X D=%03d %03d ",sensLDRL,sensLDRR,sensDistL,sensDistR);
|
|
|
|
display_cursor(2,1);
|
|
display_printf("B=%03X %03X L=%03X %03X ",sensBorderL,sensBorderR,sensLineL,sensLineR);
|
|
|
|
display_cursor(3,1);
|
|
display_printf("R=%2d %2d F=%d K=%d T=%d ",sensEncL % 10,sensEncR %10,sensError,sensDoor,sensTrans);
|
|
|
|
display_cursor(4,1);
|
|
#ifdef RC5_AVAILABLE
|
|
#ifdef MAUS_AVAILABLE
|
|
display_printf("I=%04X M=%05d %05d",RC5_Code,sensMouseX,sensMouseY);
|
|
#else
|
|
display_printf("I=%04X",RC5_Code);
|
|
#endif
|
|
#else
|
|
#ifdef MAUS_AVAILABLE
|
|
display_printf("M=%05d %05d",sensMouseX,sensMouseY);
|
|
#endif
|
|
#endif
|
|
#ifdef DISPLAY_SCREENS_AVAILABLE
|
|
break;
|
|
case 1:
|
|
#ifdef TIME_AVAILABLE
|
|
display_cursor(1,1);
|
|
display_printf("Zeit: %04d:%03d", timer_get_s(), timer_get_ms());
|
|
#endif
|
|
|
|
#ifdef BEHAVIOUR_AVAILABLE
|
|
display_cursor(2,1);
|
|
display_printf("TS=%+4d %+4d",target_speed_l,target_speed_r);
|
|
#endif
|
|
#ifdef SRF10_AVAILABLE
|
|
display_cursor(2,15);
|
|
display_printf("US%+4d",sensSRF10);
|
|
#endif
|
|
|
|
display_cursor(3,1);
|
|
display_printf("RC=%+4d %+4d",sensEncL,sensEncR);
|
|
|
|
display_cursor(4,1);
|
|
display_printf("Speed= %04d",(int16)v_center);
|
|
break;
|
|
|
|
case 2:
|
|
display_cursor(1,1);
|
|
|
|
#ifdef DISPLAY_BEHAVIOUR_AVAILABLE
|
|
/*!
|
|
* zeilenweise Anzeige der Verhalten
|
|
*/
|
|
display_printf("Verhalten (Pri/Akt)%d",behaviour_page);
|
|
|
|
colcounter = 0; linecounter = 2;
|
|
/* je nach Seitenwahl die ersten Saetze ueberlesen bis richtige Seite */
|
|
firstcol = (behaviour_page -1)*6;
|
|
|
|
/*!
|
|
* max. 3 Zeilen mit 6 Verhalten anzeigbar wegen Ueberschrift
|
|
* Seitensteuerung bei mehr Verhalten
|
|
*/
|
|
while((ptr != NULL)&& (linecounter<5)) {
|
|
|
|
if ((ptr->priority >= PRIO_VISIBLE_MIN) &&(ptr->priority <= PRIO_VISIBLE_MAX)) {
|
|
if (colcounter >= firstcol) {
|
|
display_cursor(linecounter,((colcounter % 2)* 12)+1);
|
|
#ifdef DISPLAY_DYNAMIC_BEHAVIOUR_AVAILABLE
|
|
display_printf(" %3d,%2d",ptr->priority,ptr->active);
|
|
#else
|
|
display_printf(" %3d,%2d",ptr->priority,ptr->active_new);
|
|
#endif
|
|
colcounter++;
|
|
|
|
/* bei colcounter 0 neue Zeile */
|
|
if (colcounter % 2 == 0)
|
|
linecounter++;
|
|
|
|
}
|
|
else
|
|
colcounter ++;
|
|
}
|
|
ptr = ptr->next;
|
|
}
|
|
|
|
#endif
|
|
#ifdef DISPLAY_ODOMETRIC_INFO
|
|
/* Zeige Positions- und Geschwindigkeitsdaten */
|
|
display_cursor(1,1);
|
|
display_printf("heading: %3d ",(int16)heading);
|
|
display_cursor(2,1);
|
|
display_printf("x: %3d y: %3d ",(int16)x_pos,(int16)y_pos);
|
|
display_cursor(3,1);
|
|
display_printf("v_l: %3d v_r: %3d ",(int16)v_left,(int16)v_right);
|
|
#ifdef MEASURE_MOUSE_AVAILABLE
|
|
display_cursor(4,1);
|
|
display_printf("squal: %3d v_c: %3d",maus_get_squal(),(int16)v_mou_center);
|
|
#endif
|
|
#endif
|
|
|
|
|
|
#ifdef TEST_AVAILABLE_COUNTER
|
|
display_printf("Screen 3");
|
|
display_cursor(2,1);
|
|
display_printf("count %d",counter++);
|
|
|
|
display_cursor(3,1);
|
|
display_printf("Reset-Counter %d",resets);
|
|
#endif
|
|
break;
|
|
|
|
case 3:
|
|
#ifdef DISPLAY_SCREEN_RESETINFO
|
|
display_cursor(1,1);
|
|
/* Zeige den Grund fuer Resets an */
|
|
display_printf("MCU(C)SR - Register");
|
|
|
|
display_cursor(2,1);
|
|
display_printf("PORF :%d WDRF :%d",binary(reset_flag,0),binary(reset_flag,3));
|
|
|
|
display_cursor(3,1);
|
|
display_printf("EXTRF:%d JTRF :%d",binary(reset_flag,1),binary(reset_flag,4));
|
|
|
|
display_cursor(4,1);
|
|
display_printf("BORF :%d",binary(reset_flag,2));
|
|
#endif
|
|
#ifdef DISPLAY_MMC_INFO
|
|
#ifdef MMC_INFO_AVAILABLE
|
|
{
|
|
uint32 size = 0;
|
|
uint8 csd[16];
|
|
static uint8 mmc_state= 0xFF;
|
|
|
|
|
|
uint8 dummy= mmc_init();
|
|
// hat sich was geaendert?
|
|
if (dummy!=mmc_state) {
|
|
mmc_state=dummy;
|
|
|
|
uint8 i;
|
|
for (i=0;i<16;i++)
|
|
csd[i]=0;
|
|
|
|
display_cursor(1,1);
|
|
if (mmc_state !=0){
|
|
display_printf("MMC not init (%d) ",mmc_state);
|
|
}else {
|
|
size=mmc_get_size();
|
|
mmc_read_csd(csd);
|
|
display_printf("MMC= %4d MByte ",size >> 20);
|
|
}
|
|
#ifndef MMC_WRITE_TEST_AVAILABLE
|
|
display_cursor(3,1);
|
|
for (i=0;i<16;i++){
|
|
if (i == 8) display_cursor(4,1);
|
|
if (i%2 == 0) display_printf(" ");
|
|
display_printf("%02x",csd[i]);
|
|
}
|
|
#endif // MMC_WRITE_TEST_AVAILABLE
|
|
}
|
|
#ifdef MMC_WRITE_TEST_AVAILABLE
|
|
if (mmc_state == 0){
|
|
static uint16 time = 0;
|
|
if (TIMER_GET_TICKCOUNT_16-time > MS_TO_TICKS(200)){
|
|
time = TIMER_GET_TICKCOUNT_16;
|
|
uint8 result = mmc_test();
|
|
if (result != 0){
|
|
display_cursor(3,1);
|
|
display_printf("mmc_test()=%u :( ", result);
|
|
}
|
|
}
|
|
}
|
|
#endif // MMC_WRITE_TEST_AVAILABLE
|
|
}
|
|
#else
|
|
#ifdef MMC_VM_AVAILABLE
|
|
#ifdef PC
|
|
display_cursor(3,1);
|
|
display_printf("mmc_emu_test() = %u ", mmc_emu_test());
|
|
#endif // PC
|
|
#endif // MMC_VM_AVAILABLE
|
|
#endif // MMC_INFO_AVAILABLE
|
|
#endif
|
|
|
|
break;
|
|
|
|
case 4:
|
|
/* Ausgabe des freien RAMs */
|
|
#ifdef MCU
|
|
#ifndef LOG_DISPLAY_AVAILABLE
|
|
frei = SP - (uint16) &__heap_start;
|
|
display_cursor(1,1);
|
|
display_printf("free RAM: %4d ",frei);
|
|
#endif
|
|
#endif
|
|
|
|
break;
|
|
|
|
}
|
|
#endif
|
|
}
|
|
#endif
|
|
|
|
#ifdef TEST_AVAILABLE
|
|
/*! Zeigt den internen Status der Sensoren mit den LEDs an */
|
|
void show_sensors(void){
|
|
uint8 led=0x00;
|
|
led_t * status = (led_t *)&led;
|
|
#ifdef TEST_AVAILABLE_ANALOG
|
|
(*status).rechts = (sensDistR >> 9) & 0x01;
|
|
(*status).links = (sensDistL >> 9) & 0x01;
|
|
(*status).rot = (sensLineL >> 9) & 0x01;
|
|
(*status).orange = (sensLineR >> 9) & 0x01;
|
|
(*status).gelb = (sensLDRL >> 9) & 0x01;
|
|
(*status).gruen = (sensLDRR >> 9) & 0x01;
|
|
(*status).tuerkis = (sensBorderL >> 9) & 0x01;
|
|
(*status).weiss = (sensBorderR >> 9) & 0x01;
|
|
#endif
|
|
#ifdef TEST_AVAILABLE_DIGITAL
|
|
(*status).rechts = sensEncR & 0x01;
|
|
(*status).links = sensEncL & 0x01;
|
|
(*status).rot = sensTrans & 0x01;
|
|
(*status).orange = sensError & 0x01;
|
|
(*status).gelb = sensDoor & 0x01;
|
|
#ifdef MAUS_AVAILABLE
|
|
(*status).gruen = (sensMouseDX >>1) & 0x01;
|
|
(*status).tuerkis = (sensMouseDY >>1) & 0x01;
|
|
#endif
|
|
#ifdef RC5_AVAILABLE
|
|
(*status).weiss = RC5_Code & 0x01;
|
|
#endif
|
|
#endif
|
|
|
|
LED_set(led);
|
|
}
|
|
#endif
|
|
|
|
#ifdef PC
|
|
/*!
|
|
* Zeigt Informationen zu den moeglichen Kommandozeilenargumenten an.
|
|
* Das Programm wird nach Anzeige des Hilfetextes per exit() beendet.
|
|
*/
|
|
void usage(void){
|
|
puts("USAGE: ct-Bot [-t host] [-T] [-h] [-s] [-M from] [-c FILE ID SIZE]");
|
|
puts("\t-t\tHostname oder IP Adresse zu der Verbunden werden soll");
|
|
puts("\t-T\tTestClient");
|
|
puts("\t-s\tServermodus");
|
|
puts("\t-M from\tKonvertiert eine Bot-map in eine PGM-Datei");
|
|
puts("\t-c \tErzeugt eine Mini-Fat-Datei fuer den Bot.");
|
|
puts("\t FILE\tDateiname");
|
|
puts("\t ID \tDie ID aus ASCII-Zeichen");
|
|
puts("\t SIZE\tDie Nutzgroesse der Datei in KByte");
|
|
puts("\t-h\tZeigt diese Hilfe an");
|
|
exit(1);
|
|
}
|
|
|
|
#endif
|
|
|
|
#ifdef MCU
|
|
/*!
|
|
* Hauptprogramm des Bots. Diese Schleife kuemmert sich um seine Steuerung.
|
|
*/
|
|
int main (void){
|
|
|
|
#endif
|
|
|
|
#ifdef PC
|
|
|
|
/*!
|
|
* Hauptprogramm des Bots. Diese Schleife kuemmert sich um seine Steuerung.
|
|
*/
|
|
int main (int argc, char *argv[]){
|
|
//Zum debuggen der Zeiten:
|
|
#ifdef DEBUG_TIMES
|
|
struct timeval start, stop;
|
|
#endif
|
|
|
|
|
|
int ch;
|
|
int start_server = 0; /*!< Wird auf 1 gesetzt, falls -s angegeben wurde */
|
|
int start_test_client =0; /*!< Wird auf 1 gesetzt, falls -T angegeben wurde */
|
|
char *hostname = NULL; /*!< Speichert den per -t uebergebenen Hostnamen zwischen */
|
|
|
|
int convert =0; /*!< Wird auf 1 gesetzt, wenn die Karte konvertiert werden soll */
|
|
int create =0; /*!< Wird auf 1 gesetzt, wenn eine neue Datei fuer Bot-mini-fat erzeugt werden soll */
|
|
char *from = NULL; /*!< Speichert den per -M uebergebenen Quellnamen zwischen */
|
|
|
|
|
|
// Die Kommandozeilenargumente komplett verarbeiten
|
|
while ((ch = getopt(argc, argv, "hsTt:M:c:")) != -1) {
|
|
switch (ch) {
|
|
case 's':
|
|
// Servermodus [-s] wird verlangt
|
|
start_server = 1;
|
|
break;
|
|
case 'T':
|
|
start_test_client=1;
|
|
break;
|
|
case 't':
|
|
// Hostname, auf dem ct-Sim laeuft wurde
|
|
// uebergeben. Der String wird in hostname
|
|
// gesichert.
|
|
{
|
|
const int len = strlen(optarg);
|
|
hostname = malloc(len + 1);
|
|
if (NULL == hostname)
|
|
exit(1);
|
|
strcpy(hostname, optarg);
|
|
}
|
|
break;
|
|
case 'M':
|
|
// Dateiname, fuer die Map wurde
|
|
// uebergeben. Der String wird in from
|
|
// gesichert.
|
|
{
|
|
int len = strlen(optarg);
|
|
from = malloc(len + 1);
|
|
if (NULL == from)
|
|
exit(1);
|
|
strcpy(from, optarg);
|
|
|
|
convert=1;
|
|
}
|
|
break;
|
|
case 'c':
|
|
// Datei fuer den Bot (mini-fat soll erzeugt werden
|
|
// Der Name wird in hostname gesichert.
|
|
{
|
|
int len = strlen(optarg);
|
|
from = malloc(len + 1);
|
|
if (NULL == from)
|
|
exit(1);
|
|
strcpy(from, optarg);
|
|
|
|
create=1;
|
|
}
|
|
break;
|
|
case 'h':
|
|
default:
|
|
// -h oder falscher Parameter, Usage anzeigen
|
|
usage();
|
|
}
|
|
}
|
|
argc -= optind;
|
|
argv += optind;
|
|
|
|
if (start_server != 0) { // Soll der TCP-Server gestartet werden?
|
|
printf("ARGV[0]= %s\n",argv[0]);
|
|
tcp_server_init();
|
|
tcp_server_run(100);
|
|
} else {
|
|
#ifdef MAP_AVAILABLE
|
|
/* Karte in pgm konvertieren */
|
|
if (convert !=0) {
|
|
|
|
|
|
printf("Konvertiere Karte %s in PGM %s\n",from,"map.pgm");
|
|
read_map(from);
|
|
map_to_pgm("map.pgm");
|
|
exit(0);
|
|
}
|
|
|
|
#endif // MAP_AVAILABLE
|
|
|
|
if (create !=0) {
|
|
printf("optind= %d argc=%d\n",optind, argc);
|
|
|
|
if (argc != 2){
|
|
usage();
|
|
exit(1);
|
|
}
|
|
|
|
char * id;
|
|
id = malloc(strlen(argv[0]));
|
|
strcpy(id,argv[0]);
|
|
|
|
char * s;
|
|
s = malloc(strlen(argv[1]));
|
|
strcpy(s,argv[1]);
|
|
|
|
int size = atoi(s);
|
|
|
|
create_mini_fat_file(from,id,size);
|
|
|
|
printf("Erstelle eine Mini-Fat-Datei (%s) mit %d kByte fuer den Bot. ID=%s \n",from,size,id);
|
|
// read_map(from);
|
|
// map_to_pgm("map.pgm");
|
|
exit(0);
|
|
}
|
|
|
|
|
|
printf("c't-Bot\n");
|
|
if (hostname)
|
|
// Hostname wurde per Kommandozeile uebergeben
|
|
tcp_hostname = hostname;
|
|
else {
|
|
// Der Zielhost wird per default durch das Macro IP definiert und
|
|
// tcp_hostname mit einer Kopie des Strings initialisiert.
|
|
tcp_hostname = malloc(strlen(IP) + 1);
|
|
if (NULL == tcp_hostname)
|
|
exit(1);
|
|
strcpy(tcp_hostname, IP);
|
|
}
|
|
|
|
if (start_test_client !=0) {
|
|
tcp_test_client_init();
|
|
tcp_test_client_run(100);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
#endif
|
|
#ifdef TEST_AVAILABLE_MOTOR
|
|
uint16 calls=0; /*!< Im Testfall zaehle die Durchlaeufe */
|
|
#endif
|
|
|
|
init();
|
|
|
|
|
|
#ifdef WELCOME_AVAILABLE
|
|
display_cursor(1,1);
|
|
display_printf("c't-Roboter");
|
|
LED_set(0x00);
|
|
#ifdef LOG_AVAILABLE
|
|
LOG_DEBUG(("Hallo Welt!"));
|
|
#endif
|
|
#endif
|
|
|
|
#ifdef TEST_AVAILABLE_COUNTER
|
|
display_screen=2;
|
|
|
|
resets=eeprom_read_byte(&resetsEEPROM)+1;
|
|
eeprom_write_byte(&resetsEEPROM,resets);
|
|
/* Lege den Grund für jeden Reset im EEPROM ab */
|
|
eeprom_write_byte(&resetInfoEEPROM+resets,reset_flag);
|
|
#endif
|
|
/*! Hauptschleife des Bot */
|
|
|
|
for(;;){
|
|
#ifdef PC
|
|
receive_until_Frame(CMD_DONE);
|
|
#ifdef DEBUG_TIMES
|
|
//Zum debuggen der Zeiten:
|
|
GETTIMEOFDAY(&start, NULL);
|
|
int t1=(start.tv_sec - stop.tv_sec)*1000000 + start.tv_usec - stop.tv_usec;
|
|
printf("Done-Token (%d) in nach %d usec ",received_command.data_l,t1);
|
|
#endif
|
|
#endif
|
|
|
|
|
|
#ifdef MCU
|
|
bot_sens_isr();
|
|
#endif
|
|
#ifdef TEST_AVAILABLE
|
|
show_sensors();
|
|
#endif
|
|
|
|
// Testprogramm, dass den Bot erst links, dann rechtsrum dreht
|
|
#ifdef TEST_AVAILABLE_MOTOR
|
|
calls++;
|
|
if (calls == 1)
|
|
motor_set(BOT_SPEED_SLOW,-BOT_SPEED_SLOW);
|
|
else if (calls == 501)
|
|
motor_set(-BOT_SPEED_SLOW,BOT_SPEED_SLOW);
|
|
else if (calls== 1001)
|
|
motor_set(BOT_SPEED_STOP,BOT_SPEED_STOP);
|
|
else
|
|
#endif
|
|
// hier drin steckt der Verhaltenscode
|
|
#ifdef BEHAVIOUR_AVAILABLE
|
|
if (sensors_initialized ==1 )
|
|
bot_behave();
|
|
#ifdef LOG_AVAILABLE
|
|
//else
|
|
//LOG_DEBUG(("sens not init"));
|
|
#endif
|
|
#endif
|
|
|
|
#ifdef MCU
|
|
#ifdef BOT_2_PC_AVAILABLE
|
|
// static int16 lastTimeCom =0;
|
|
|
|
bot_2_pc_inform(); // Den PC ueber Sensorern und aktuatoren informieren
|
|
bot_2_pc_listen(); // Kommandos vom PC empfangen
|
|
|
|
// if (timer_get_s() != lastTimeCom) { // sollte genau 1x pro Sekunde zutreffen
|
|
// lastTimeCom = timer_get_s();
|
|
|
|
// }
|
|
#endif
|
|
#endif
|
|
|
|
#ifdef LOG_AVAILABLE
|
|
//LOG_DEBUG(("BOT TIME %d s", timer_get_s()));
|
|
#endif
|
|
|
|
// Alles Anzeigen
|
|
#ifdef DISPLAY_AVAILABLE
|
|
display();
|
|
#endif
|
|
#ifdef MCU
|
|
// delay(10);
|
|
#endif
|
|
|
|
#ifdef PC
|
|
command_write(CMD_DONE, SUB_CMD_NORM ,(int16*)&simultime,0,0);
|
|
|
|
flushSendBuffer();
|
|
//Zum debuggen der Zeiten:
|
|
#ifdef DEBUG_TIMES
|
|
GETTIMEOFDAY(&stop, NULL);
|
|
int t2=(stop.tv_sec - start.tv_sec)*1000000 +stop.tv_usec - start.tv_usec;
|
|
printf("Done-Token (%d) out after %d usec\n",simultime,t2);
|
|
#endif
|
|
#endif
|
|
|
|
}
|
|
|
|
/*! Falls wir das je erreichen sollten ;-) */
|
|
return 1;
|
|
}
|