void deviceTrajectoryHandleRawData(char header, InputStream* inputStream, OutputStream* outputStream) { if (header == COMMAND_GET_ABSOLUTE_POSITION) { appendAck(outputStream); updateTrajectoryWithNoThreshold(); append(outputStream, COMMAND_GET_ABSOLUTE_POSITION); notifyAbsolutePositionWithoutHeader(outputStream); } else if (header == COMMAND_DEBUG_GET_ABSOLUTE_POSITION) { appendAck(outputStream); updateTrajectoryWithNoThreshold(); append(outputStream, COMMAND_DEBUG_GET_ABSOLUTE_POSITION); OutputStream* debugOutputStream = getOutputStreamLogger(ALWAYS); printPosition(debugOutputStream); } else if (header == COMMAND_SET_ABSOLUTE_POSITION) { long newX = readHex4(inputStream); inputStream->readChar(inputStream); long newY = readHex4(inputStream); inputStream->readChar(inputStream); long newAngle = readHex4(inputStream); appendAck(outputStream); OutputStream* debugOutputStream = getDebugOutputStreamLogger(); appendStringAndDec(debugOutputStream, "newX=", newX); appendStringAndDec(debugOutputStream, ",newY=", newY); appendStringAndDec(debugOutputStream, ",newAngle=", newAngle); float fX = (float) newX; float fY = (float) newY; float fAngle = PI_DIVIDE_1800 * (float) newAngle; appendStringAndDecf(debugOutputStream, "fX=", fX); appendStringAndDecf(debugOutputStream, ",fY=", fY); appendStringAndDecf(debugOutputStream, ",fAngle=", fAngle); setPosition(fX, fY, fAngle); append(outputStream, COMMAND_SET_ABSOLUTE_POSITION); } }
void printBeaconSystemConfiguration(OutputStream* outputStream) { Point* opponentPoint = getOpponentRobotPosition(); Laser* laser1 = getLaser(LASER_INDEX_1); Laser* laser2 = getLaser(LASER_INDEX_2); printLaserStruct(outputStream, laser1); println(outputStream); printLaserStruct(outputStream, laser2); appendCRLF(outputStream); // Last Detection Time appendStringAndDecf(outputStream, "lastDetectionTime=", getLastDetectionTimeInMillis()); println(outputStream); // Obsolete Detection Time Threshold appendStringAndDecf(outputStream, "obsoleteDetectionTimeThreshold=", getObsoleteDetectionTimeThreshold()); println(outputStream); // Notify Time Delay appendStringAndDecf(outputStream, "notifyTimeDelay=", getNotifyTimeDelay()); println(outputStream); // Opponent Position appendString(outputStream, "opponentPosition:"); printPoint(outputStream, opponentPoint, " mm"); // Distance between beacon appendStringAndDecf(outputStream, "distanceBetweenBeacon=", getDistanceBetweenBeacon()); appendString(outputStream, " mm"); println(outputStream); // Calibration Point appendString(outputStream, "calibrationPoint:"); printPoint(outputStream, &(beaconSystem.calibrationPoint), " mm"); // Opponent Position appendString(outputStream, "opponentPosition:"); printPoint(outputStream, opponentPoint, " mm"); }
void printGameTarget(OutputStream* outputStream, GameTarget* target, BOOL includeItems) { appendString(outputStream, "target:"); appendKeyAndName(outputStream, "name=", target->name); appendStringAndDecf(outputStream, ", gain=", target->gain); appendStringAndDec(outputStream, ", status=", target->status); // TODO : Point println(outputStream); if (includeItems) { printGameTargetActionList(outputStream, &(target->actionList)); } }
void printPosition(OutputStream* outputStream) { // clearScreen(); appendCRLF(outputStream); Position* p = getPosition(); appendStringAndDec(outputStream, "left=", getCoderValue(CODER_LEFT)); appendStringAndDec(outputStream, " | right=", getCoderValue(CODER_RIGHT)); println(outputStream); printPoint(outputStream, &(p->pos), " mm"); appendStringAndAngleInDeg(outputStream, "ang:", p->orientation); appendStringAndAngleInDeg(outputStream, "\r\nang init:", p->initialOrientation); appendStringAndDecf(outputStream, "\r\nlastLeft:", lastLeft); appendString(outputStream, " pulse"); appendStringAndDecf(outputStream, "\r\nlastRight:", lastRight); appendString(outputStream, " pulse"); appendStringAndAngleInDeg(outputStream, "\r\nlastAng:", lastAngle); println(outputStream); }
void writeBSplineDefinition(OutputStream* outputStream, BSplineCurve* bSplineCurve) { writeBSplineControlPoints(outputStream, bSplineCurve, 1.0f); float curveLength = bSplineCurve->curveLength; appendStringAndDecf(outputStream, "\ncurve.length=", curveLength); appendString(outputStream, " mm\n"); /* appendString(outputStream, "lastPointData:\n"); writeBSplinePointData(outputStream, &(bSplineCurve->lastPointData)); appendString(outputStream, "tempPointData:\n"); writeBSplinePointData(outputStream, &(bSplineCurve->tempPointData)); */ appendStringAndDec(outputStream, "acc Factor:", bSplineCurve->accelerationFactor); println(outputStream); appendStringAndDec(outputStream, "Speed Factor:", bSplineCurve->speedFactor); println(outputStream); }
void printGameStrategyContext(OutputStream* outputStream, GameStrategyContext* context) { appendString(outputStream, "GameStrategyContext\n"); appendString(outputStream, "\tstrategy.name="); // gameStrategy if (context->gameStrategy != NULL) { appendString(outputStream, context->gameStrategy->name); } else { appendString(outputStream, "NULL"); } // elapsedMatchTime appendStringAndDecf(outputStream, "\n\telapsedMatchTime=", context->elapsedMatchTime); println(outputStream); // Robot Position appendString(outputStream, "\trobotPosition="); printPoint(outputStream, &(context->robotPosition), ""); appendStringAndDec(outputStream, "\n\trobotAngle (ddeg)=", context->robotAngle); // nearestLocation appendString(outputStream, "\n\tnearestLocation="); if (context->nearestLocation != NULL) { printLocation(outputStream, context->nearestLocation); } else { appendString(outputStream, "NULL"); } appendStringAndDec(outputStream, "\ntimeSinceLastCollision=", context->timeSinceLastCollision); // Robot Position appendString(outputStream, "\n\topponentRobotPosition="); printPoint(outputStream, &(context->opponentRobotPosition), ""); // Obstacle Position appendString(outputStream, "\n\tlastObstaclePosition="); printPoint(outputStream, &(context->lastObstaclePosition), ""); // current Target appendString(outputStream, "\n\tcurrentTarget="); if (context->currentTarget != NULL) { printGameTarget(outputStream, context->currentTarget, false); } else { appendString(outputStream, "NULL"); } // currentTrajectory if (&(context->currentTrajectory) != NULL) { printLocationList(outputStream, "\n\tcurrentTrajectory:", &(context->currentTrajectory)); } else { appendString(outputStream, "\n\tcurrentTrajectory=NULL"); } appendStringAndDec(outputStream, "\n\tteamColor=", context->color); appendStringAndDec(outputStream, "\n\tstrategyIndex=", context->strategyIndex); appendStringAndDec(outputStream, "\n\tmaxTargetToHandle=", context->maxTargetToHandle); // appendStringAndDec(outputStream, "\n\tmustDoNextStep=", context->mustDoNextStep); appendStringAndDec(outputStream, "\n\thasMoreNextSteps=", context->hasMoreNextSteps); println(outputStream); }
void bSplineMotionUCompute(void) { PidMotion* pidMotion = getPidMotion(); PidComputationValues* computationValues = &(pidMotion->computationValues); PidMotionDefinition* motionDefinition = &(pidMotion->currentMotionDefinition); BSplineCurve* curve = &(motionDefinition->curve); float pidTime = computationValues->pidTime; MotionInstruction* thetaInst = &(motionDefinition->inst[THETA]); float normalPosition = computeNormalPosition(thetaInst, pidTime); // Computes the time of the bSpline in [0.00, 1.00] float bSplineTime = computeBSplineTimeAtDistance(curve, normalPosition); Point normalPoint; // Computes the normal Point where the robot must be computeBSplinePoint(curve, bSplineTime, &normalPoint); // Convert normalPoint into mm space RobotKinematics* robotKinematics = getRobotKinematics(); float wheelAverageLength = robotKinematics->wheelAverageLengthForOnePulse; normalPoint.x = normalPoint.x * wheelAverageLength; normalPoint.y = normalPoint.y * wheelAverageLength; Position* robotPosition = getPosition(); Point robotPoint = robotPosition->pos; // GET PID unsigned pidIndex = getIndexOfPid(THETA, thetaInst->pidType); unsigned char rollingTestMode = getRollingTestMode(); PidParameter* pidParameter = getPidParameter(pidIndex, rollingTestMode); // ALPHA PidMotionError* alphaMotionError = &(computationValues->errors[ALPHA]); float normalAlpha = computeBSplineOrientationWithDerivative(curve, bSplineTime); float realAlpha = robotPosition->orientation; // backward management if (curve->backward) { realAlpha += PI; } float alphaError = (normalAlpha - realAlpha); // restriction to [-PI, PI] alphaError = mod2PI(alphaError); // Convert angleError into pulse equivalent float wheelsDistanceFromCenter = getWheelsDistanceFromCenter(robotKinematics); float alphaPulseError = (-wheelsDistanceFromCenter * alphaError) / wheelAverageLength; // THETA PidMotionError* thetaMotionError = &(computationValues->errors[THETA]); // thetaError must be in Pulse and not in MM float thetaError = distanceBetweenPoints(&robotPoint, &normalPoint) / wheelAverageLength; float thetaAngle = angleOfVector(&robotPoint, &normalPoint); if (curve->backward) { thetaAngle += PI; } float alphaAndThetaDiff = thetaAngle - normalAlpha; // restriction to [-PI, PI] alphaAndThetaDiff = mod2PI(alphaAndThetaDiff); float cosAlphaAndThetaDiff = cosf(alphaAndThetaDiff); float thetaErrorWithCos = thetaError * cosAlphaAndThetaDiff; float normalSpeed = computeNormalSpeed(thetaInst, pidTime); float thetaU = computePidCorrection(thetaMotionError, pidParameter, normalSpeed, thetaErrorWithCos); PidCurrentValues* thetaCurrentValues = &(computationValues->currentValues[THETA]); thetaCurrentValues->u = thetaU; // ALPHA CORRECTION alphaPulseError *= 5.0f; float alphaCorrection = -0.00050f * normalSpeed * thetaError * (alphaAndThetaDiff); // float alphaCorrection = 0.0f; alphaPulseError += alphaCorrection; float alphaU = computePidCorrection(alphaMotionError, pidParameter, 0, alphaPulseError); PidCurrentValues* alphaCurrentValues = &(computationValues->currentValues[ALPHA]); alphaCurrentValues->u = alphaU; // LOG OutputStream* out = getDebugOutputStreamLogger(); // appendStringAndDecf(out, "pt=", pidTime); appendStringAndDecf(out, ",t=", bSplineTime); // Normal Position appendStringAndDecf(out, ",nx=", normalPoint.x); appendStringAndDecf(out, ",ny=", normalPoint.y); appendStringAndDecf(out, ",na=", normalAlpha); // Real position appendStringAndDecf(out, ",rx=", robotPoint.x); appendStringAndDecf(out, ",ry=", robotPoint.y); appendStringAndDecf(out, ",ra=", realAlpha); // ALPHA appendStringAndDecf(out, ",ta=", thetaAngle); appendStringAndDecf(out, ",ae=", alphaError); //appendStringAndDecf(out, ",atdiff=", alphaAndThetaDiff); //appendStringAndDecf(out, ",catdiff=", cosAlphaAndThetaDiff); appendStringAndDecf(out, ",au=", alphaU); appendStringAndDecf(out, ",ac=", alphaCorrection); // THETA // appendString(out, ",np="); // appendDecf(out, normalPosition); appendStringAndDecf(out, ",te=", thetaError); appendStringAndDecf(out, ",tec=", thetaErrorWithCos); appendStringAndDecf(out, ",tu=", thetaU); appendCRLF(out); }
void debugTrajectoryVariables(char* valueName1, float value1, char* valueName2, float value2) { OutputStream* outputStream = getDebugOutputStreamLogger(); appendStringAndDecf(outputStream, valueName1, value1); appendStringAndDecf(outputStream, valueName2, value2); appendCRLF(outputStream); }
void writeBSplineDefinitionPoint(OutputStream* outputStream, Point* point, char* pointName, float factor) { appendString(outputStream, pointName); appendStringAndDecf(outputStream, "(x=", point->x * factor); appendStringAndDecf(outputStream, ",y=", point->y * factor); appendString(outputStream, ")\n"); }