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");
}
