void calibrateBySideSensor() {
unsigned char ADJ = 15;
unsigned char S = sensor.front_sensor;
unsigned char Left = S & 0b1100000;
unsigned char Right = S & 0b00000011;
if(Left != 0 && Right == 0) {
leftSpeed = SPEED - ADJ;
rightSpeed = SPEED + ADJ;
logchr(0x01);
} else if(Left == 0 && Right != 0) {
leftSpeed = SPEED + ADJ;
rightSpeed = SPEED - ADJ;
logchr(0x02);
} else {
leftSpeed = SPEED;
rightSpeed = SPEED;
logchr(0x03);
}
}
static void proxy_default_message_handler(Channel * c, char ** argv, int argc) {
Proxy * proxy = c->client_data;
Channel * otherc = proxy[proxy->other].c;
char * p;
int i = 0;
assert(c == proxy->c);
assert(argc > 0 && strlen(argv[0]) == 1);
if (proxy[proxy->other].state == ProxyStateDisconnected) return;
if (argv[0][0] == 'C') {
write_stringz(&otherc->out, argv[0]);
/* Prefix token with 'R'emote to distinguish from locally
* generated commands */
write_string(&otherc->out, "R");
i = 1;
}
else if (argv[0][0] == 'R' || argv[0][0] == 'P' || argv[0][0] == 'N') {
if (argv[1][0] != 'R') {
trace(LOG_ALWAYS, "Reply with unexpected token: %s", argv[1]);
exception(ERR_PROTOCOL);
}
argv[1]++;
}
while (i < argc) {
write_stringz(&otherc->out, argv[i]);
i++;
}
p = logbuf;
if (log_mode & LOG_TCFLOG) {
logstr(&p, proxy->other > 0 ? "---> " : "<--- ");
for (i = 0; i < argc; i++) {
logstr(&p, argv[i]);
logchr(&p, ' ');
}
}
/* Copy body of message */
do {
i = read_stream(&c->inp);
if (log_mode & LOG_TCFLOG) {
if (i > ' ' && i < 127) {
/* Printable ASCII */
logchr(&p, i);
}
else if (i == 0) {
logstr(&p, " ");
}
else if (i > 0) {
char buf[40];
snprintf(buf, sizeof buf, "\\x%02x", i);
logstr(&p, buf);
}
else if (i == MARKER_EOM) {
logstr(&p, "<eom>");
}
else if (i == MARKER_EOS) {
logstr(&p, "<eom>");
}
else {
logstr(&p, "<?>");
}
}
write_stream(&otherc->out, i);
}
while (i != MARKER_EOM && i != MARKER_EOS);
flush_stream(&otherc->out);
if (log_mode & LOG_TCFLOG) {
*p = '\0';
trace(LOG_TCFLOG, "%d: %s", proxy->instance, logbuf);
}
}