diff options
Diffstat (limited to 'source/ct-Bot/ct-Bot.c')
| -rw-r--r-- | source/ct-Bot/ct-Bot.c | 747 |
1 files changed, 747 insertions, 0 deletions
diff --git a/source/ct-Bot/ct-Bot.c b/source/ct-Bot/ct-Bot.c new file mode 100644 index 0000000..0cc077c --- /dev/null +++ b/source/ct-Bot/ct-Bot.c @@ -0,0 +1,747 @@ +/* + * 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; +} |