void showWaitingStart(OutputStream* pcOutputStream) {
appendString(pcOutputStream, MESSAGE_TO_PC_RESET);
appendString(getOutputStreamLogger(ALWAYS), "CFG:");
appendStringConfig(getOutputStreamLogger(ALWAYS));
println(getOutputStreamLogger(ALWAYS));
appendString(getOutputStreamLogger(ALWAYS), "Waiting start:");
}
void forceRobotNextStepIfNecessary() {
if (strategyDriverInterruptCounter >= FORCE_ROBOT_NEXT_STEP_COUNTER) {
appendString(getOutputStreamLogger(ERROR), "forceRobotNextStepIfNecessary\n");
strategyDriverInterruptCounter = 0;
robotNextStep();
}
}
BOOL sentStrategyRobotPosition(unsigned char status, unsigned int x, unsigned int y, int angleInDeciDegree) {
OutputStream* debugOutputStream = getOutputStreamLogger(INFO);
appendString(debugOutputStream, "sentStrategyRobotPosition:");
OutputStream* outputStream = getDriverRequestOutputStream();
append(outputStream, COMMAND_STRATEGY_SET_ROBOT_POSITION);
appendHex2(outputStream, status);
appendSeparator(outputStream);
appendHex4(outputStream, x);
appendSeparator(outputStream);
appendHex4(outputStream, y);
appendSeparator(outputStream);
appendHex4(outputStream, angleInDeciDegree);
appendStringAndDec(debugOutputStream, "status=", status);
appendStringAndDec(debugOutputStream, ", x=", x);
appendStringAndDec(debugOutputStream, ", y=", y);
appendStringAndDec(debugOutputStream, ", angle=", angleInDeciDegree);
println(debugOutputStream);
BOOL result = transmitFromDriverRequestBuffer();
return result;
}
void doBeaconCalibration() {
// Update the robot Position (which must normally coincides with calibration Point)
getOpponentRobotPosition();
Point* calibrationPoint = &(beaconSystem.calibrationPoint);
OutputStream* outputStream = getOutputStreamLogger(INFO);
recalibrateServoInitValue(outputStream, getLaser(LASER_INDEX_1), calibrationPoint->x, calibrationPoint->y);
recalibrateServoInitValue(outputStream, getLaser(LASER_INDEX_2), getDistanceBetweenBeacon() - calibrationPoint->x, calibrationPoint->y);
}
BOOL sendStrategyConfiguration(int configuration) {
appendString(getOutputStreamLogger(INFO), "sendStrategyConfiguration\n");
OutputStream* outputStream = getDriverRequestOutputStream();
append(outputStream, COMMAND_STRATEGY_SET_CONFIG);
appendHex4(outputStream, configuration);
BOOL result = transmitFromDriverRequestBuffer();
return result;
}
void setColor(TEAM_COLOR color) {
GameStrategyContext* context = getStrategyContext();
appendStringAndDec(getOutputStreamLogger(INFO), "setColor:", color);
println(getOutputStreamLogger(INFO));
context->color = color;
changeLocationsForColor();
int angle = 675;
if (!isViolet()) {
angle = -angle;
context->robotPosition.y = 2840;
}
else {
context->robotPosition.y = 160;
}
context->robotPosition.x = 160;
context->robotAngle = angle;
printStrategyAllDatas(getOutputStreamLogger(INFO));
}
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);
}
}
BOOL isEnd() {
if (doNotEnd) {
return FALSE;
}
if (!matchStarted) {
appendString(getOutputStreamLogger(ERROR), "You must call startMatch before");
}
BOOL result = currentTimeInSecond >= MATCH_DURATION;
if (result) {
// If END
armDriver2012Up(0);
}
return result;
}
BOOL sendStrategyNextStep() {
appendString(getOutputStreamLogger(INFO), "sendStrategyNextStep\n");
OutputStream* outputStream = getDriverRequestOutputStream();
InputStream* inputStream = getDriverResponseInputStream();
append(outputStream, COMMAND_STRATEGY_NEXT_STEP);
BOOL result = transmitFromDriverRequestBuffer();
if (result) {
int result = readHex2(inputStream);
return result;
}
return FALSE;
}
BOOL sendStrategyOpponentRobotPosition(Point* opponentRobotPosition) {
OutputStream* debugOutputStream = getOutputStreamLogger(INFO);
appendString(debugOutputStream, "sendStrategyOpponentRobotPosition: ");
OutputStream* outputStream = getDriverRequestOutputStream();
append(outputStream, COMMAND_STRATEGY_SET_OPPONENT_ROBOT_POSITION);
appendHex4(outputStream, opponentRobotPosition->x);
appendSeparator(outputStream);
appendHex4(outputStream, opponentRobotPosition->y);
appendStringAndDec(debugOutputStream, " x=", opponentRobotPosition->x);
appendStringAndDec(debugOutputStream, ", y=", opponentRobotPosition->y);
println(debugOutputStream);
BOOL result = transmitFromDriverRequestBuffer();
return result;
}
/**
* Gets the position of the laser by triangulation of the information gived by the both laser.
* If information is not correct, we give a position of (0, 0).
*/
Point* getOpponentRobotPosition() {
Timer* beaconTimer = getTimerByCode(BEACON_TIMER_CODE);
// we want to avoid that position change during compute
lockAndWaitForTimer(beaconTimer);
Laser* laser1 = getLaser(LASER_INDEX_1);
Laser* laser2 = getLaser(LASER_INDEX_2);
// Updates the detected Position of the laser
int detectedPosition1 = updateDetectedPosition(laser1);
int detectedPosition2 = updateDetectedPosition(laser2);
Point* opponentRobotPosition = &(beaconSystem.opponentRobotPosition);
// if there is a detected object (by both laser)
if (detectedPosition1 != 0 && detectedPosition2 != 0) {
// Too obsolete Information
if (getLastDetectionTimeInMillis() > getObsoleteDetectionTimeThreshold()) {
opponentRobotPosition->x = 0;
opponentRobotPosition->y = 0;
OutputStream* outputStream = getOutputStreamLogger(INFO);
appendStringAndDec(outputStream, "LOST OBJECT SINCE=", getLastDetectionTimeInMillis());
println(outputStream);
}
else {
float angleInRadian1 = getAngleInRadian(laser1);
float angleInRadian2 = getAngleInRadian(laser2);
float a1 = tan(angleInRadian1);
float a2 = tan(angleInRadian2);
// Compute the position
fillPositionWithValues(opponentRobotPosition, beaconSystem.distanceBetweenBeacon, a1, a2);
}
} else {
// Reset Robot Position
opponentRobotPosition->x = 0;
opponentRobotPosition->y = 0;
}
unlockTimer(beaconTimer);
return opponentRobotPosition;
}
/**
* Call-back when Data send some notification message (like Position Reached, Position failed ...)
*/
void mainBoardCallbackRawData(const Device* device,
char commandHeader,
InputStream* inputStream) {
// if (header == NOTIFY_MOTION_STATUS || header == COMMAND_NOTIFY_TEST || header == COMMAND_PLIERS_2011_OPEN) {
// MOTOR BOARD notification
if (header == NOTIFY_MOTION_STATUS) {
/*
appendString(getInfoOutputStreamLogger(), "\nNotification : From MOTOR BOARD \n");
// NOTIFY_MOTION_STATUS / COMMAND_NOTIFY_TEST
checkIsChar(inputStream, header);
// status
unsigned char status = readHex2(inputStream);
// separator
checkIsSeparator(inputStream);
// x
unsigned int x = readHex4(inputStream);
// separator
checkIsChar(inputStream, '-');
// y
unsigned int y = readHex4(inputStream);
// separator
checkIsSeparator(inputStream);
// angle in ddeg
unsigned int angle = readHex4(inputStream);
updateRobotPosition(x, y, angle);
setRobotPositionChanged();
// FOR DEBUG AND MOTHER BOARD
OutputStream* outputStream = &(compositePcAndDebugOutputStream.outputStream);
append(outputStream, header);
appendHex2(outputStream, status);
appendSeparator(outputStream);
appendHex4(outputStream, x);
appendSeparator(outputStream);
appendHex4(outputStream, y);
appendSeparator(outputStream);
appendHex4(outputStream, angle);
// ready for next motion instruction Index
setReadyForNextMotion(true);
// Robot finished the trajectory
instructionType = INSTRUCTION_TYPE_NO_MOVE;
*/
}
// STRATEGY BOARD notification message of MOTOR => Must be relayed TO MOTOR
else if (header == COMMAND_MOTION_SPLINE_ABSOLUTE || header == COMMAND_MOTION_SPLINE_RELATIVE) {
/*
appendString(getInfoOutputStreamLogger(), "Notification : Spline : From STRATEGY BOARD : relayed to MOTOR_BOARD \n");
appendStringAndDec(getInfoOutputStreamLogger(), "getDriverResponseBuffer:", getBufferElementsCount(getDriverResponseBuffer()));
// forwardCallbackRawDataTo(inputStream, &debugOutputStream, device, header, DEVICE_MODE_INPUT);
OutputStream* outputStream = &(compositeDriverAndDebugOutputStream.outputStream);
// OutputStream* outputStream = &debugOutputStream;
readHex(inputStream);
// appendString(outputStream, ",header=");
append(outputStream, header);
float x = readHex4(inputStream);
// appendString(outputStream, ",x=");
appendHex4(outputStream, x);
checkIsSeparator(inputStream);
appendSeparator(outputStream);
float y = readHex4(inputStream);
// appendString(outputStream, ",y=");
appendHex4(outputStream, y);
checkIsSeparator(inputStream);
appendSeparator(outputStream);
float angle = readHex4(inputStream);
// appendString(outputStream, ",angle=");
appendHex4(outputStream, angle);
checkIsSeparator(inputStream);
appendSeparator(outputStream);
signed char dist0 = readHex2(inputStream);
// appendString(outputStream, ",dist0=");
appendHex2(outputStream, dist0);
checkIsSeparator(inputStream);
appendSeparator(outputStream);
signed char dist1 = readHex2(inputStream);
// appendString(outputStream, ",dist1=");
appendHex2(outputStream, dist1);
checkIsSeparator(inputStream);
appendSeparator(outputStream);
signed char a = readHex(inputStream);
// appendString(outputStream, ",a=");
appendHex(outputStream, a);
signed char s = readHex(inputStream);
// appendString(outputStream, ",s=");
appendHex(outputStream, s);
if (dist0 < 0) {
appendString(getInfoOutputStreamLogger(), "\nMotion Backward !");
instructionType = INSTRUCTION_TYPE_BACKWARD;
}
else {
appendString(getInfoOutputStreamLogger(), "\nMotion Forward !");
instructionType = INSTRUCTION_TYPE_FORWARD;
}
// forwardCallbackRawDataTo(inputStream, &(compositeDriverAndDebugOutputStream.outputStream), device, header, DEVICE_MODE_INPUT);
transmitFromDriverRequestBuffer();
*/
} else if (header == COMMAND_MOTION_LEFT_IN_DECI_DEGREE || header == COMMAND_MOTION_RIGHT_IN_DECI_DEGREE) {
/*
appendString(getInfoOutputStreamLogger(), "Notification : Rotation : From STRATEGY BOARD : relayed to MOTOR_BOARD \n");
instructionType = INSTRUCTION_TYPE_ROTATION;
forwardCallbackRawDataTo(inputStream, &(compositeDriverAndDebugOutputStream.outputStream), device, header, DEVICE_MODE_INPUT);
transmitFromDriverRequestBuffer();
*/
}
// Mechanical Board notification
else if (header == NOTIFY_INFRARED_DETECTOR_DETECTION) {
/*
appendString(getInfoOutputStreamLogger(), "\nNotification : From MECHANICAL BOARD :\n");
checkIsChar(inputStream, NOTIFY_INFRARED_DETECTOR_DETECTION);
// type
unsigned char type = readHex2(inputStream);
OutputStream* outputStream = &(compositePcAndDebugOutputStream.outputStream);
append(outputStream, NOTIFY_INFRARED_DETECTOR_DETECTION);
appendHex2(outputStream, type);
if (useInfrared) {
// Notify only if we are in a compatible move !
if ((instructionType == INSTRUCTION_TYPE_BACKWARD) && (type == DETECTOR_BACKWARD_INDEX)) {
mustNotifyObstacle = true;
}
else if ((instructionType == INSTRUCTION_TYPE_FORWARD) && (type == DETECTOR_FORWARD_INDEX)) {
mustNotifyObstacle = true;
}
}
*/
}
// Cannot not handle the notification !
else {
appendString(getOutputStreamLogger(ERROR), "\nNotification lost:\n");
copyInputToOutputStream(inputStream, getOutputStreamLogger(ERROR), NULL, COPY_ALL);
println(getOutputStreamLogger(ERROR));
}
}
void pathFIFunction() { fillPathData(&locationF, &locationI, 250, 0, 0, 0, 0, 0, 0); }
addNavigationPath(&pathFIFunction);
void pathIJFunction() { fillPathData(&locationI, &locationJ, 84, 0, 0, 0, 0, 0, 0); }
addNavigationPath(&pathIJFunction);
void pathACFunction() { fillPathData(&locationA, &locationC, 217, 0, 0, 0, 0, 0, 0); }
addNavigationPath(&pathACFunction);
void pathCGFunction() { fillPathData(&locationC, &locationG, 186, 0, 0, 0, 0, 0, 0); }
addNavigationPath(&pathCGFunction);
void pathCHFunction() { fillPathData(&locationC, &locationH, 103, 0, 0, 0, 0, 0, 0); }
addNavigationPath(&pathCHFunction);
void pathHDFunction() { fillPathData(&locationH, &locationD, 183, 0, 0, 0, 0, 0, 0); }
addNavigationPath(&pathHDFunction);
void pathHJFunction() { fillPathData(&locationH, &locationJ, 167, 0, 0, 0, 0, 0, 0); }
addNavigationPath(&pathHJFunction);
void pathEJFunction() { fillPathData(&locationE, &locationJ, 502, 0, 0, 0, 0, 0, 0); }
addNavigationPath(&pathEJFunction);
void pathAEFunction() { fillPathData(&locationA, &locationE, 173, 0, 0, 0, 0, 0, 0); }
addNavigationPath(&pathAEFunction);
printPathList(getOutputStreamLogger(INFO), "Paths definition", getNavigationPathList());
int cost = computeBestPath(&resultLocationList, &locationA, &locationJ);
printLocationList(getOutputStreamLogger(INFO), "Result=", &resultLocationList);
return cost;
}
void showStarted(OutputStream* pcOutputStream) {
// Notify the pc that the match started
appendString(pcOutputStream, MESSAGE_TO_PC_START);
appendStringCRLF(getOutputStreamLogger(ALWAYS), "Go !");
}
