469 lines
12 KiB
C
469 lines
12 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 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
|