summaryrefslogtreecommitdiffstats
path: root/source/ct-Bot/command.c
diff options
context:
space:
mode:
Diffstat (limited to 'source/ct-Bot/command.c')
-rw-r--r--source/ct-Bot/command.c469
1 files changed, 469 insertions, 0 deletions
diff --git a/source/ct-Bot/command.c b/source/ct-Bot/command.c
new file mode 100644
index 0000000..a1993dd
--- /dev/null
+++ b/source/ct-Bot/command.c
@@ -0,0 +1,469 @@
+/*
+ * 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 command.c
+ * @brief Kommando-Management
+ * @author Benjamin Benz (bbe@heise.de)
+ * @date 20.12.05
+*/
+
+#include "ct-Bot.h"
+
+#include "led.h"
+#include "log.h"
+
+#include "uart.h"
+#include "adc.h"
+#include "timer.h"
+#include "mouse.h"
+
+#include "command.h"
+#include "display.h"
+
+#include "sensor.h"
+#include "motor.h"
+#include "rc5.h"
+#include "ir-rc5.h"
+#include "bot-logic/bot-logik.h"
+#include "bot-2-pc.h"
+
+#include <stdio.h>
+#include <string.h>
+
+#ifdef PC
+ #include "tcp.h"
+ #include <pthread.h>
+#endif
+
+#define COMMAND_TIMEOUT 10 /*!< Anzahl an ms, die maximal auf fehlende Daten gewartet wird */
+
+#ifdef COMMAND_AVAILABLE
+
+#define RCVBUFSIZE (sizeof(command_t)*2) /*!< Groesse des Empfangspuffers */
+
+command_t received_command; /*!< Puffer fuer Kommandos */
+
+#ifdef PC
+ // Auf dword alignment bestehen, wird fuer MacOS X benoetigt
+ pthread_mutex_t command_mutex __attribute__ ((aligned (4)))
+ = PTHREAD_MUTEX_INITIALIZER;
+#endif
+
+/*!
+ * Liest ein Kommando ein, ist blockierend!
+ * Greift auf low_read() zurueck
+ * Achtung, die Payload wird nicht mitgelesen!!!
+ * @see low_read()
+ */
+int8 command_read(void){
+ int bytesRcvd;
+ int start=0; // start des Kommandos
+ int i;
+ command_t * command; // Pointer zum Casten der empfangegen Daten
+ char * ptr; // Nur zu Hilfszwecken
+ char buffer[RCVBUFSIZE]; // Buffer
+ #if BYTE_ORDER == BIG_ENDIAN
+ uint16 store; //Puffer für die Endian-Konvertierung
+ #endif
+
+ uint16 old_ticks; // alte Systemzeit
+
+ buffer[0]=0; // Sicherheitshalber mit sauberem Puffer anfangen
+
+ // Daten holen, maximal soviele, wie ein Kommando lang ist
+ bytesRcvd=low_read(buffer,sizeof(command_t));
+
+ // LOG_DEBUG(("%d/%d read/av",bytesRcvd,tmp));
+ // LOG_DEBUG(("%x %x %x",buffer[0],buffer[1],buffer[2]));
+
+ // Suche nach dem Beginn des Frames
+ while ((start<bytesRcvd)&&(buffer[start] != CMD_STARTCODE)) {
+// printf("\nStartzeichen nicht am Anfang des Puffers! (%d)\n",start);
+// printf(".");
+ start++;
+ }
+
+ // Wenn keine STARTCODE gefunden ==> Daten verwerfen
+ if (buffer[start] != CMD_STARTCODE){
+ // LOG_DEBUG(("start not found"));
+ return -1;
+ }
+
+ // LOG_DEBUG(("Start @%d",start));
+
+ // haben wir noch genug Platz im Puffer, um das Packet ferig zu lesen?
+ if ((RCVBUFSIZE-start) < sizeof(command_t)){
+ // LOG_DEBUG(("not enough space"));
+// printf("not enough space");
+ return -1; // nein? ==> verwerfen
+ }
+
+ i=sizeof(command_t) - (bytesRcvd-start)-1;
+
+
+ if (i> 0) { // Fehlen noch Daten ?
+ LOG_DEBUG(("command.c: Start @ %d es fehlen %d bytes ",start,i));
+ // Systemzeit erfassen
+ old_ticks = TIMER_GET_TICKCOUNT_16;
+
+ // So lange Daten lesen, bis das Packet vollstaendig ist, oder der Timeout zuschlaegt
+ while (i > 0){
+ // Wenn der Timeout ueberschritten ist
+ if (TIMER_GET_TICKCOUNT_16-old_ticks > MS_TO_TICKS(COMMAND_TIMEOUT))
+ return -1; // ==> Abbruch
+ // LOG_DEBUG(("%d bytes missing",i));
+ i= low_read(buffer+bytesRcvd,i);
+ // LOG_DEBUG(("%d read",i));
+ bytesRcvd+=i;
+ i=sizeof(command_t) - (bytesRcvd-start);
+ }
+ }
+
+ // LOG_DEBUG(("%d/%d read/start",bytesRcvd,start));
+ // LOG_DEBUG(("%x %x %x",buffer[start],buffer[start+1],buffer[start+2]));
+
+ // Cast in command_t
+ command= (command_t *) ( buffer +start);
+
+ // LOG_DEBUG(("start: %x ",command->startCode));
+ // command_display(command);
+
+ // validate (startcode ist bereits ok, sonst waeren wir nicht hier )
+ if (command->CRC==CMD_STOPCODE){
+ // LOG_DEBUG(("Command is valid"));
+ // Transfer
+ #ifdef PC
+ command_lock(); // on PC make storage threadsafe
+ #endif
+ ptr = (char *) &received_command;
+ for (i=0; i<sizeof(command_t);i++){
+ *ptr=buffer[i+start];
+ ptr++;
+ }
+ #if BYTE_ORDER == BIG_ENDIAN
+ /* Umwandeln der 16 bit Werte in Big Endian */
+ store = received_command.data_l;
+ received_command.data_l = store << 8;
+ received_command.data_l |= (store >> 8) & 0xff;
+
+ store = received_command.data_r;
+ received_command.data_r = store << 8;
+ received_command.data_r |= (store >> 8) & 0xff;
+
+ store = received_command.seq;
+ received_command.seq = store << 8;
+ received_command.seq |= (store >> 8) & 0xff;
+
+ /* "Umdrehen" des Bitfields */
+ store = received_command.request.subcommand;
+ received_command.request.subcommand = store << 1;
+ received_command.request.direction = store >> 7;
+ #endif
+ #ifdef PC
+ command_unlock(); // on PC make storage threadsafe
+ #endif
+
+ return 0;
+ } else { // Command not valid
+ // LOG_DEBUG(("Invalid Command:"));
+ // LOG_DEBUG(("%x %x %x",command->startCode,command->request.command,command->CRC));
+ return -1;
+ }
+}
+
+static uint16 count=1; /*!< Zaehler fuer Paket-Sequenznummer*/
+
+/*!
+ * Uebertraegt ein Kommando und wartet nicht auf eine Antwort
+ * @param command Kennung zum Command
+ * @param subcommand Kennung des Subcommand
+ * @param data_l Daten fuer den linken Kanal
+ * @param data_r Daten fuer den rechten Kanal
+ * @param payload Anzahl der Bytes, die diesem Kommando als Payload folgen
+ */
+void command_write(uint8 command, uint8 subcommand, int16* data_l,int16* data_r,uint8 payload){
+ command_t cmd;
+
+
+ cmd.startCode=CMD_STARTCODE;
+ cmd.request.direction=DIR_REQUEST; // Anfrage
+ cmd.request.command= command;
+ cmd.request.subcommand= subcommand;
+
+ cmd.payload=payload;
+ if (data_l != NULL)
+ cmd.data_l = *data_l;
+ else
+ cmd.data_l = 0;
+
+ if (data_r != NULL)
+ cmd.data_r = *data_r;
+ else
+ cmd.data_r = 0;
+
+ cmd.seq=count++;
+ cmd.CRC=CMD_STOPCODE;
+
+ low_write(&cmd);
+}
+
+
+/*!
+ * Uebertraegt ein Kommando und wartet nicht auf eine Antwort
+ * @param command Kennung zum Command
+ * @param subcommand Kennung des Subcommand
+ * @param data_l Daten fuer den linken Kanal
+ * @param data_r Daten fuer den rechten Kanal
+ */
+//void command_write(uint8 command, uint8 subcommand, int16* data_l,int16* data_r){
+//}
+
+/*!
+ * Gibt dem Simulator Daten mit Anhang und wartet nicht auf Antwort
+ * @param command Kennung zum Command
+ * @param subcommand Kennung des Subcommand
+ * @param data_l Daten fuer den linken Kanal
+ * @param data_r Daten fuer den rechten Kanal
+ * @param payload Anzahl der Bytes im Anhang
+ * @param data Datenanhang an das eigentliche Command
+ */
+void command_write_rawdata(uint8 command, uint8 subcommand, int16* data_l, int16* data_r, uint8 payload, uint8* data){
+ command_write(command, subcommand, data_l, data_r,payload);
+ low_write_data(data, payload);
+}
+
+
+/*!
+ * Gibt dem Simulator Daten mit String-Anhang und wartet nicht auf Antwort
+ * @param command Kennung zum Command
+ * @param subcommand Kennung des Subcommand
+ * @param data_l Daten fuer den linken Kanal
+ * @param data_r Daten fuer den rechten Kanal
+ * @param data Datenanhang an das eigentliche Command
+ */
+void command_write_data(uint8 command, uint8 subcommand, int16* data_l, int16* data_r, const char* data){
+ size_t len;
+ uint8 payload;
+
+ if (data != NULL) {
+ len = strlen(data);
+ if (len > MAX_PAYLOAD) {
+ payload = MAX_PAYLOAD;
+ } else {
+ payload = len;
+ }
+ } else {
+ payload = 0;
+ }
+
+ command_write(command, subcommand, data_l, data_r,payload);
+ low_write_data((uint8 *)data, payload);
+}
+
+#ifdef MAUS_AVAILABLE
+ /*!
+ * Uebertraegt ein Bild vom Maussensor an den PC
+ */
+ void transmit_mouse_picture(void){
+ int16 dummy,i;
+
+ int16 pixel;
+ uint8 data;
+ maus_image_prepare();
+
+ for (i=0; i<6; i++) {
+ dummy= i*54 +1;
+ command_write(CMD_SENS_MOUSE_PICTURE, SUB_CMD_NORM, &dummy , &dummy,54);
+ for (pixel=0; pixel <54; pixel++){
+ data= maus_image_read();
+ low_write_data((uint8 *)&data,1);
+ }
+ }
+
+ }
+#endif
+
+
+/*!
+ * Wertet das Kommando im Puffer aus
+ * return 1, wenn Kommando schon bearbeitet wurde, 0 sonst
+ */
+int command_evaluate(void){
+ uint8 analyzed = 1;
+
+ #ifdef LOG_AVAILABLE
+ // command_display(&received_command);
+ #endif // LOG_AVAILABLE
+
+ switch (received_command.request.command) {
+ #ifdef IR_AVAILABLE
+ case CMD_SENS_RC5:
+ ir_data=received_command.data_l;
+ break;
+ #endif
+ case CMD_AKT_LED: // LED-Steuerung
+ LED_set(received_command.data_l & 255);
+ break;
+
+ // Einige Kommandos ergeben nur fuer reale Bots Sinn
+ case CMD_WELCOME:
+ #ifdef MCU
+ command_write(CMD_WELCOME, SUB_WELCOME_REAL,0,0,0);
+ #else
+ command_write(CMD_WELCOME, SUB_WELCOME_SIM,0,0,0);
+ #endif
+ break;
+
+
+ #ifdef MAUS_AVAILABLE
+ case CMD_SENS_MOUSE_PICTURE: // PC fragt nach dem Bild
+ transmit_mouse_picture();
+ break;
+ #endif
+
+ #ifdef BEHAVIOUR_REMOTECALL_AVAILABLE
+ case CMD_REMOTE_CALL:
+ switch (received_command.request.subcommand) {
+ case SUB_REMOTE_CALL_LIST:
+ remote_call_list();
+ break;
+ case SUB_REMOTE_CALL_ORDER:
+ {
+ uint8 buffer[REMOTE_CALL_BUFFER_SIZE];
+ low_read(buffer,received_command.payload);
+ bot_remotecall_from_command((uint8 *)&buffer);
+
+ break;
+ }
+ default:
+ LOG_DEBUG(("unbekanntes Subkommando: %c",received_command.request.subcommand));
+ break;
+ }
+ break;
+ #endif
+
+
+ // Einige Kommandos ergeben nur fuer simulierte Bots Sinn
+ #ifdef PC
+ case CMD_SENS_IR:
+ sensor_abstand(received_command.data_l,received_command.data_r);
+ break;
+ case CMD_SENS_ENC:
+ sensEncL+=received_command.data_l;
+ sensEncR+=received_command.data_r;
+ break;
+ case CMD_SENS_BORDER:
+ sensBorderL=received_command.data_l;
+ sensBorderR=received_command.data_r;
+ break;
+ case CMD_SENS_LINE:
+ sensLineL=received_command.data_l;
+ sensLineR=received_command.data_r;
+ break;
+ case CMD_SENS_LDR:
+ sensLDRL=received_command.data_l;
+ sensLDRR=received_command.data_r;
+ break;
+ case CMD_SENS_TRANS:
+ sensTrans=(char)received_command.data_l;
+ break;
+ #ifdef MAUS_AVAILABLE
+ case CMD_SENS_MOUSE:
+ sensMouseDX=received_command.data_l;
+ sensMouseDY=received_command.data_r;
+ break;
+ #endif
+ case CMD_SENS_ERROR:
+ sensError=(char)received_command.data_l;
+ sensor_update(); /* Error ist der letzte uebertragene Sensorwert, danach koennen wir uns um allgemeine updates kümmern*/
+ break;
+ case CMD_DONE:
+ simultime=received_command.data_l;
+ system_time_isr(); /* Einmal pro Update-Zyklus aktualisieren wir die Systemzeit */
+ // printf("X-Frame for Simultime = %d received ",simultime);
+ break;
+ #endif
+ default:
+ analyzed=0; // Command was not analysed yet
+ break;
+ }
+ return analyzed;
+}
+
+
+
+#ifdef LOG_AVAILABLE
+/*!
+ * Gibt ein Kommando auf dem Bildschirm aus
+ */
+ void command_display(command_t * command){
+ #ifdef PC
+/* printf("START= %d\nDIR= %d CMD= %d SUBCMD= %d\nPayload= %d\nDATA = %d %d\nSeq= %d\nCRC= %d",
+ (*command).startCode,
+ (*command).request.direction,
+ (*command).request.command,
+ (*command).request.subcommand,
+ (*command).payload,
+ (*command).data_l,
+ (*command).data_r,
+ (*command).seq,
+ (*command).CRC);
+*/
+ LOG_DEBUG(("CMD: %c\tData L: %d\tSeq: %d\n",
+ (*command).request.command,
+ (*command).data_l,
+ (*command).seq));
+ #else
+ LOG_DEBUG(("%x %x %x",
+ (*command).request.command,
+ (*command).data_l,
+ (*command).seq));
+
+/* char* raw= (char*)command;
+ unsigned char i=0;
+ char hex[6];
+
+ display_cursor(4,1);
+ display_string("0x");
+ to_hex(*raw,hex);
+ display_string(hex);
+ display_string(" ");
+ raw++;
+
+ for (i=1; i<sizeof(command_t)-1; i++){
+ to_hex(*raw++,hex);
+ display_string(hex);
+ }
+
+ display_string(" ");
+ to_hex(*raw,hex);
+ display_string(hex);
+
+ sprintf(hex,"%5d",(*command).seq);
+ display_string(" ");
+ display_string(hex);
+*/
+ #ifdef WIN32
+ printf("\n");
+ #endif
+
+ #endif
+ }
+#endif
+#endif
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-06 04:22:14 +0200