/* Turns on the spot
Because motors on the actual robot may be upside down, this
handles both left and right turns.
By default with motors being right side up, positive power does
a right turn and negative power does a left turn
*/
int NXT_PivotTurn(int power){
int ret = 0;
ret += _set_output_state(OUT_B,INT_TO_HEX(-power),0x07,0x00,0x00,0x20,0x00);
ret += _set_output_state(OUT_C,INT_TO_HEX(power),0x07,0x00,0x00,0x20,0x00);
if ( ret < 0){
fprintf(stderr, "_set_output_state Failed\n");
return -1;
}
return ret;
}
/* Turns on the specified motors with specified power
Supported ports are:
OUT_A
OUT_B
OUT_C
OUT_BC
Power range is [-100,100]
Negative power makes motors go in reverse.
*/
int NXT_OnFwd(unsigned char port, int power){
int ret = 0;
if ( (port == OUT_A) || (port == OUT_B) || (port == OUT_C) ){
ret += _set_output_state(port,INT_TO_HEX(power), 0x07,0x00,0x00,0x20,0x00);
}
else if (port == OUT_BC){
ret += _set_output_state(OUT_B,INT_TO_HEX(power), 0x07,0x00,0x00,0x20,0x00);
ret += _set_output_state(OUT_C,INT_TO_HEX(power), 0x07,0x00,0x00,0x20,0x00);
}
if ( ret < 0){
fprintf(stderr, "_set_output_state Failed\n");
return -1;
}
return ret;
}
static void get_hex_str(gchar *out, guchar ch)
{
gchar hex;
INT_TO_HEX(hex, ch >> 4);
*out++ = hex;
INT_TO_HEX(hex, ch & 0x0f);
*out++ = hex;
}
/*
* Name: req_write_escape_http
* Description: Buffers and "escapes" data before sending to client.
* as above, but translates as it copies, into a form suitably
* encoded for URLs in HTTP headers.
* Returns: -1 for error, otherwise how much is stored
*/
int req_write_escape_http(request * req, char *msg)
{
char c, *inp, *dest;
int left;
inp = msg;
dest = req->buffer + req->buffer_end;
/* 3 is a guard band, since we don't check the destination pointer
* in the middle of a transfer of up to 3 bytes */
left = BUFFER_SIZE - req->buffer_end - 3;
while ((c = *inp++) && left > 0) {
if (needs_escape((unsigned int) c)) {
*dest++ = '%';
*dest++ = INT_TO_HEX(c >> 4);
*dest++ = INT_TO_HEX(c & 15);
left -= 3;
} else {
char *http_escape_string(char *inp, char *buf, const int len)
{
int max;
char *index;
unsigned char c;
max = len * 3;
if (buf == NULL && max)
buf = malloc(sizeof (unsigned char) * (max + 1));
if (buf == NULL)
return NULL;
index = buf;
while ((c = *inp++)) {
if (needs_escape((unsigned int) c)) {
*index++ = '%';
*index++ = INT_TO_HEX(c >> 4);
*index++ = INT_TO_HEX(c & 15);
} else
void get_request(int server_sock)
{
int fd; /* socket */
struct SOCKADDR remote_addr; /* address */
struct SOCKADDR salocal;
unsigned int remote_addrlen = sizeof (struct SOCKADDR);
request *conn; /* connection */
socklen_t len;
#ifndef INET6
remote_addr.S_FAMILY = (sa_family_t) 0xdead;
#endif
fd = accept(server_sock, (struct sockaddr *) &remote_addr,
&remote_addrlen);
if (fd == -1) {
if (errno != EAGAIN && errno != EWOULDBLOCK) {
/* abnormal error */
WARN("accept");
} else {
/* no requests */
}
pending_requests = 0;
return;
}
if (fd >= FD_SETSIZE) {
log_error("Got fd >= FD_SETSIZE.");
close(fd);
return;
}
#ifdef DEBUGNONINET
/* This shows up due to race conditions in some Linux kernels
when the client closes the socket sometime between
the select() and accept() syscalls.
Code and description by Larry Doolittle <*****@*****.**>
*/
if (remote_addr.sin_family != PF_INET) {
struct sockaddr *bogus = (struct sockaddr *) &remote_addr;
char *ap, ablock[44];
int i;
close(fd);
log_error_time();
for (ap = ablock, i = 0; i < remote_addrlen && i < 14; i++) {
*ap++ = ' ';
*ap++ = INT_TO_HEX((bogus->sa_data[i] >> 4) & 0x0f);
*ap++ = INT_TO_HEX(bogus->sa_data[i] & 0x0f);
}
*ap = '\0';
fprintf(stderr, "non-INET connection attempt: socket %d, "
"sa_family = %hu, sa_data[%d] = %s\n",
fd, bogus->sa_family, remote_addrlen, ablock);
return;
}
#endif
/* XXX Either delete this, or document why it's needed */
/* Pointed out 3-Oct-1999 by Paul Saab <*****@*****.**> */
#ifdef REUSE_EACH_CLIENT_CONNECTION_SOCKET
if ((setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, (void *) &sock_opt,
sizeof (sock_opt))) == -1) {
DIE("setsockopt: unable to set SO_REUSEADDR");
}
#endif
len = sizeof (salocal);
if (getsockname(fd, (struct sockaddr *) &salocal, &len) != 0) {
WARN("getsockname");
close(fd);
return;
}
conn = new_request();
if (!conn) {
close(fd);
return;
}
conn->fd = fd;
conn->status = READ_HEADER;
conn->header_line = conn->client_stream;
conn->time_last = current_time;
conn->kacount = ka_max;
if (ascii_sockaddr
(&salocal, conn->local_ip_addr,
sizeof (conn->local_ip_addr)) == NULL) {
WARN("ascii_sockaddr failed");
close(fd);
enqueue(&request_free, conn);
return;
}
/* nonblocking socket */
if (set_nonblock_fd(conn->fd) == -1) {
WARN("fcntl: unable to set new socket to non-block");
close(fd);
enqueue(&request_free, conn);
return;
}
/* set close on exec to true */
if (fcntl(conn->fd, F_SETFD, 1) == -1) {
WARN("fctnl: unable to set close-on-exec for new socket");
close(fd);
enqueue(&request_free, conn);
return;
}
#ifdef TUNE_SNDBUF
/* Increase buffer size if we have to.
* Only ask the system the buffer size on the first request,
* and assume all subsequent sockets have the same size.
*/
if (system_bufsize == 0) {
len = sizeof (system_bufsize);
if (getsockopt
(conn->fd, SOL_SOCKET, SO_SNDBUF, &system_bufsize, &len) == 0
&& len == sizeof (system_bufsize)) {
;
} else {
WARN("getsockopt(SNDBUF)");
system_bufsize = 1;
}
}
if (system_bufsize < sockbufsize) {
if (setsockopt
(conn->fd, SOL_SOCKET, SO_SNDBUF, (void *) &sockbufsize,
sizeof (sockbufsize)) == -1) {
WARN("setsockopt: unable to set socket buffer size");
#ifdef DIE_ON_ERROR_TUNING_SNDBUF
exit(errno);
#endif /* DIE_ON_ERROR_TUNING_SNDBUF */
}
}
#endif /* TUNE_SNDBUF */
/* for log file and possible use by CGI programs */
if (ascii_sockaddr
(&remote_addr, conn->remote_ip_addr,
sizeof (conn->remote_ip_addr)) == NULL) {
WARN("ascii_sockaddr failed");
close(fd);
enqueue(&request_free, conn);
return;
}
/* for possible use by CGI programs */
conn->remote_port = net_port(&remote_addr);
status.requests++;
#ifdef USE_TCPNODELAY
/* Thanks to Jef Poskanzer <*****@*****.**> for this tweak */
{
int one = 1;
if (setsockopt(conn->fd, IPPROTO_TCP, TCP_NODELAY,
(void *) &one, sizeof (one)) == -1) {
DIE("setsockopt: unable to set TCP_NODELAY");
}
}
#endif
total_connections++;
/* gotta have some breathing room */
if (total_connections > max_connections) {
pending_requests = 0;
#ifndef NO_RATE_LIMIT
/* have to fake an http version */
conn->http_version = HTTP10;
conn->method = M_GET;
send_r_service_unavailable(conn);
conn->status = DONE;
#endif /* NO_RATE_LIMIT */
}
enqueue(&request_ready, conn);
}
