/* * 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; }