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