1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
|
/*
* 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 bot-2-sim.c
* @brief Verbindung c't-Bot zu c't-Sim
* @author Benjamin Benz (bbe@heise.de)
* @date 26.12.05
*/
#include "ct-Bot.h"
#ifdef PC
#include <stdio.h> // for printf() and fprintf()
#include <stdlib.h> // for atoi() and exit()
#include <string.h> // for strlen()
#include <sys/time.h>
#include <pthread.h>
#include <time.h>
#include "bot-2-sim.h"
#include "tcp.h"
#include "command.h"
#include "display.h"
#include "sensor.h"
#include "bot-logic/bot-logik.h"
#include "motor.h"
#include "command.h"
/* Linux with glibc:
* _REENTRANT to grab thread-safe libraries
* _POSIX_SOURCE to get POSIX semantics
*/
#ifdef __linux__
# define _REENTRANT
//# define _POSIX_SOURCE
#endif
/* Hack for LinuxThreads */
#ifdef __linux__
# define _P __P
#endif
#define low_init tcp_init /*!< Low-Funktion zum Initialisieren*/
pthread_t simThread; /*!< Simuliert den Bot */
pthread_t bot_2_sim_Thread; /*!< Thread sammelt Sensordaten, uebertraegt Motor-Daten */
pthread_cond_t command_cond = PTHREAD_COND_INITIALIZER; /*!< Schuetzt das Kommando */
pthread_mutex_t command_cond_mutex = PTHREAD_MUTEX_INITIALIZER; /*!< Schuetzt das Kommando */
void signal_command_available(void);
int wait_for_command(int timeout_s);
#ifdef WIN32
/* These are winbase.h definitions, but to avoid including
tons of Windows related stuff, it is reprinted here */
typedef struct _FILETIME {
unsigned long dwLowDateTime;
unsigned long dwHighDateTime;
} FILETIME;
void __stdcall GetSystemTimeAsFileTime(FILETIME*);
void gettimeofday_win(struct timeval* p, void* tz /* IGNORED */){
union {
long long ns100; // time since 1 Jan 1601 in 100ns units
FILETIME ft;
} _now;
GetSystemTimeAsFileTime( &(_now.ft) );
p->tv_usec=(long)((_now.ns100 / 10LL) % 1000000LL );
p->tv_sec= (long)((_now.ns100-(116444736000000000LL))/10000000LL);
return;
}
#endif
/*!
* Dieser Thread nimmt die Daten vom PC entgegen
*/
void *bot_2_sim_rcv_isr(void * arg){
#ifdef DISPLAY_AVAILABLE
display_cursor(11,1);
#endif
printf("bot_2_sim_rcv_isr() comming up\n");
for (;;){
// only write if noone reads command
if (command_read()!=0)
printf("Error reading command\n"); // read a command
else {
// command_display(&received_command); // show it
if (command_evaluate() ==0) // use data transfered
signal_command_available(); // tell anyone waiting
}
}
return 0;
}
/*!
* Schleife, die Kommandis empfaengt und bearbeitet, bis ein Kommando vom Typ Frame kommt
* @param frame Kommando zum abbruch
*/
int8 receive_until_Frame(int8 frame){
int8 result=0;
for(;;){
result=command_read();
if (result!=0){
printf("Error reading command\n"); // read a command
return result;
} else {
command_evaluate();
}
if (received_command.request.command == frame)
return 0;
}
}
/*!
* Ein wenig Initialisierung kann nicht schaden
*/
void bot_2_sim_init(void){
low_init();
// if (pthread_create(&bot_2_sim_Thread, // thread struct
// NULL, // default thread attributes
// bot_2_sim_rcv_isr, // start routine
// NULL)) { // arg to routine
// printf("Thread Creation failed");
// exit(1);
// }
int j;
int16 null=0;
for(j=0;j<5;j++)
command_write(CMD_WELCOME, SUB_WELCOME_SIM ,&null,&null,0);
flushSendBuffer();
}
int not_answered_error=1; /*!< Wurde ein Packet beantwortet */
/*!
* Wartet auf die Antwort des PC
* @param timeout_s Wartezeit in Sekunden
* @return 0, wenn Ok
*/
int wait_for_command(int timeout_s){
struct timespec ts;
struct timeval tp;
int result=0;
pthread_mutex_lock(&command_cond_mutex);
GETTIMEOFDAY(&tp, NULL);
// Convert from timeval to timespec
ts.tv_sec = tp.tv_sec;
ts.tv_nsec = tp.tv_usec * 1000;
ts.tv_sec += timeout_s;
result= pthread_cond_timedwait(&command_cond, &command_cond_mutex, &ts);
pthread_mutex_unlock(&command_cond_mutex);
return result;
}
/*!
* Wartet auf die Antwort des PC
* Achtung blockierend ohne Timeout
* @param command Das kommando auf das gewartet wird.
* @return 0, wenn Ok
*/
int wait_for_special_command(uint8 command){
int result=0;
while (1){
result= wait_for_command(1000);
if ((result==0) && (received_command.request.command == command))
return result;
}
}
/*!
* Benachrichtigt wartende Threads ueber eingetroffene Kommandos
*/
void signal_command_available(void){
pthread_mutex_lock(&command_cond_mutex);
pthread_cond_signal(&command_cond);
pthread_mutex_unlock(&command_cond_mutex);
}
/*!
* Schickt einen Thread in die Warteposition
* @param timeout_us Wartezeit in Mikrosekunden
*/
void wait_for_time(long timeout_us){
pthread_cond_t cond = PTHREAD_COND_INITIALIZER;
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
struct timespec ts;
struct timeval tp;
pthread_mutex_lock(&mutex);
GETTIMEOFDAY(&tp, NULL);
// Convert from timeval to timespec
tp.tv_usec += (timeout_us % 1000000);
tp.tv_sec += (timeout_us / 1000000);
ts.tv_sec = tp.tv_sec+ (tp.tv_usec/1000000);
ts.tv_nsec = (tp.tv_usec % 1000000)* 1000;
pthread_cond_timedwait(&cond, &mutex, &ts);
pthread_mutex_unlock(&mutex);
}
#endif
|
