/*
* retrieves a token converted from a pp-token;
* ASSUMPTION: signed/unsigned integers are compatible on the host
*/
int (clx_next)(void)
{
lex_t *t;
clx_ppos = clx_cpos;
while (1) {
t = lst_next();
clx_cpos = t->pos;
switch(t->id) {
case -1:
strg_free((arena_t *)t->spell);
break;
case LEX_CREM: /* %= */
case LEX_CBAND: /* &= */
case LEX_CMUL: /* *= */
case LEX_CADD: /* += */
case LEX_CSUB: /* -= */
case LEX_CDIV: /* /= */
case LEX_CRSHFT: /* >>= */
case LEX_CLSHFT: /* <<= */
case LEX_CBXOR: /* ^= */
case LEX_CBOR: /* |= */
case '!':
case '%':
case '&':
case LEX_INCR: /* ++ */
case '(':
case ')':
case '*':
case '+':
case ',':
case '-':
case '.':
case '/':
case LEX_DECR: /* -- */
case LEX_DEREF: /* -> */
case LEX_ANDAND: /* && */
case LEX_OROR: /* || */
case LEX_LEQ: /* <= */
case LEX_EQEQ: /* == */
case LEX_NEQ: /* != */
case LEX_GEQ: /* >= */
case LEX_RSHFT: /* >> */
case LEX_LSHFT: /* << */
case ':':
case ';':
case '<':
case '=':
case '>':
case '?':
case LEX_ELLIPSIS: /* ... */
case ']':
case '[':
case '^':
case '{':
case '|':
case '}':
case '~':
case LEX_EOI:
return t->id;
case LEX_ID:
return id(t);
case LEX_CCON:
{
int w = 0;
ux_t c = clx_ccon(t, &w);
tval.type = (w)? ty_wchartype: ty_inttype;
tval.u.c.v.u = c;
clx_sym = &tval;
}
return t->id;
case LEX_SCON:
{
int w = 0;
sz_t len = scon(t, &w);
tval.type = ty_array((!w)? ty_chartype: ty_wchartype, len, clx_cpos);
tval.u.c.v.p = xcfp(strg_sbuf);
clx_sym = &tval;
}
return t->id;
case LEX_PPNUM:
return ifcon(t);
case LEX_SPACE:
case LEX_NEWLINE:
break;
case LEX_SHARP: /* # */
case LEX_DSHARP: /* ## */
case LEX_UNKNOWN:
{
const char *p = LEX_SPELL(t);
err_dpos(t->pos, (*p == '\\')? ERR_LEX_STRAYBS: ERR_LEX_UNKNOWN, p);
}
break;
}
}
/* assert(!"impossible control flow -- should never reach here");
return t->id; */
}
void MotionController::drive(Vector vector, short rotationSpeed) {
ipcSendingConnection scon("MOTION_CONTROL", "NAVIGATION");
scon.sendData(std::string(vector));
Vector g;
Vector h;
double length_g, length_h;
length_g = g.abs();
length_h = h.abs();
if (vector.getAngle() > (315) || vector.getAngle() <= 45) {
g.set(1, 1);
h.set(-1, 1);
length_g = 1;
length_h = 1;
}
else if (vector.getAngle() > 45 && vector.getAngle() <= 135) {
g.set(1, 1);
h.set(1, -1);
length_g = 1;
length_h = -1;
}
else if (vector.getAngle() > 135 && vector.getAngle() <= 225) {
g.set(-1, -1);
h.set(1, -1);
length_g = -1;
length_h = -1;
}
else if (vector.getAngle() > 225 && vector.getAngle() <= 315) {
g.set(-1, -1);
h.set(-1, 1);
length_g = -1;
length_h = 1;
}
else {
std::cerr << "wrong vector" << std::endl;
debug->send("MotionController::drive --> wrong Vector !!!");
}
g.setLength(cos(abs(vector.getAngle(g, false, false))) * vector.abs());
h.setLength(sin(abs(vector.getAngle(g, false, false))) * vector.abs());
length_g *= g.abs();
length_h *= h.abs();
if (length_g > 255 || length_g < (-255) || length_h > 255 || length_h < (-255)) {
std::cerr << "MotorController::drive --> vector length out of range !!!" << std::endl;
debug->send("MotorController::drive --> vector length out of range !!!");
return; /// Error occured !
}
motorSpeeds[0] = length_h;
motorSpeeds[1] = length_g;
motorSpeeds[2] = length_h;
motorSpeeds[3] = length_g;
//std::cout << "set motor speeds: " << motorSpeeds[0] << "\t" << motorSpeeds[1] << "\t" << motorSpeeds[2] << "\t" << motorSpeeds[3] << std::endl;
/// set all motors to the minimum motor offset and calculate new values for the others
/*
// find smallest value
double smallest_value = 255;
for(int i = 0; i < 4; i++) {
if (abs(motorSpeeds[i]) < smallest_value && abs(motorSpeeds[i]) > 0) smallest_value = motorSpeeds[i];
}
if (smallest_value < MOTOR_MINIMUM_OFFSET) {
double ratio = abs(double(MOTOR_MINIMUM_OFFSET) / smallest_value);
for (int j = 0; j < 4; j++) {
motorSpeeds[j] *= ratio;
}
}*/
/*if (abs(smallest_value) < MOTOR_MINIMUM_OFFSET) {
for (int i = 0; i < 4; i++) {
if (motorSpeeds[i] < 0) motorSpeeds[i] -= abs(MOTOR_MINIMUM_OFFSET - smallest_value);
else motorSpeeds[i] += abs(MOTOR_MINIMUM_OFFSET - smallest_value);
}
}*/
/// calculate motor-values dependend to the turn
/// check here for speed overflows and step down the values if neccessary ...
if (rotationSpeed < 0) {
if (motorSpeeds[0] + rotationSpeed > 255) {
double factor = (motorSpeeds[0] - (abs(rotationSpeed) - 255 + motorSpeeds[0])) / abs(motorSpeeds[0]);
for (int i = 0; i < 4; i++) {
motorSpeeds[i] = double(motorSpeeds[i]) * factor;
}
}
if (motorSpeeds[1] + rotationSpeed > 255) {
double factor = (motorSpeeds[1] - (abs(rotationSpeed) - 255 + motorSpeeds[1])) / abs(motorSpeeds[1]);
for (int i = 0; i < 4; i++) {
motorSpeeds[i] = double(motorSpeeds[i]) * factor;
}
}
if (motorSpeeds[2] - rotationSpeed < (-255)) {
double factor = (motorSpeeds[2] - (abs(rotationSpeed) - 255 + motorSpeeds[2])) / abs(motorSpeeds[2]);
for (int i = 0; i < 4; i++) {
motorSpeeds[i] = double(motorSpeeds[i]) * factor;
}
}
if (motorSpeeds[3] - rotationSpeed < (-255)) {
double factor = (motorSpeeds[3] - (abs(rotationSpeed) - 255 + motorSpeeds[3])) / abs(motorSpeeds[3]);
for (int i = 0; i < 4; i++) {
motorSpeeds[i] = double(motorSpeeds[i]) * factor;
}
}
}
else {
if (motorSpeeds[0] - rotationSpeed < (-255)) {
double factor = (motorSpeeds[0] - (abs(rotationSpeed) - 255 + motorSpeeds[0])) / abs(motorSpeeds[0]);
for (int i = 0; i < 4; i++) {
motorSpeeds[i] = double(motorSpeeds[i]) * factor;
}
}
if (motorSpeeds[1] - rotationSpeed < (-255)) {
double factor = (motorSpeeds[1] - (abs(rotationSpeed) - 255 + motorSpeeds[1])) / abs(motorSpeeds[1]);
for (int i = 0; i < 4; i++) {
motorSpeeds[i] = double(motorSpeeds[i]) * factor;
}
}
if (motorSpeeds[2] + rotationSpeed > 255) {
double factor = (motorSpeeds[2] - (abs(rotationSpeed) - 255 + motorSpeeds[2])) / abs(motorSpeeds[2]);
for (int i = 0; i < 4; i++) {
motorSpeeds[i] = double(motorSpeeds[i]) * factor;
}
}
if (motorSpeeds[3] + rotationSpeed > 255) {
double factor = (motorSpeeds[3] - (abs(rotationSpeed) - 255 + motorSpeeds[3])) / abs(motorSpeeds[3]);
for (int i = 0; i < 4; i++) {
motorSpeeds[i] = double(motorSpeeds[i]) * factor;
}
}
}
motorSpeeds[0] -= rotationSpeed;
motorSpeeds[1] -= rotationSpeed;
motorSpeeds[2] += rotationSpeed;
motorSpeeds[3] += rotationSpeed;
motors->setSpeed(motorSpeeds[0], motorSpeeds[1], motorSpeeds[2], motorSpeeds[3]);
return;
}
