BOOL notifyRobotPositionIfNecessary() {
// Don't notify if the servo does not move
if (!beaconSystem.enabled) {
return FALSE;
}
Timer* beaconTimer = getTimerByCode(BEACON_TIMER_CODE);
// delay is exceed => must notify
if (beaconTimer->time > beaconSystem.lastNotifyTime + beaconSystem.notifyTimeDelay) {
// for the next time, mark it
beaconSystem.lastNotifyTime = beaconTimer->time;
Point* point = getOpponentRobotPosition();
notifyRobotPosition(point);
return TRUE;
}
return FALSE;
}
/**
* 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;
}
bool mustPidBeRecomputed(void) {
Timer* timer = getTimerByCode(TIMER_PID_CODE);
unsigned long pidTime = timer->time;
// Example 1 :
// pidTime = 687, lastPidTime = 2 ==> pidTime > 624
// lastPidTime will be : (687 / 312) + 1 = 3 (because we use integer division, and not float division)
// We will recompute when pidTime > 3 * 312 => if pidTime > 936
//
// Example 2 :
// pidTime = 465, lastPidTime = 3 ==> pidTime < 936
//
// Example 3 :
// pidTime = 1723, lastPidTime = 2 => pidTIme > 624
// lastPidTime will be 5 (we will not have at any time lastPidTime = 3 or lastPidTime = 4)
if (pidTime > PID_UPDATE_MOTOR_TIMER_INTERVAL * lastPidTime) {
unsigned long interval = PID_UPDATE_MOTOR_TIMER_INTERVAL;
lastPidTime = (pidTime / interval) + 1;
return true;
}
return false;
}
void stopLaserBeacon() {
// Shutdown timer
Timer* beaconTimer = getTimerByCode(BEACON_TIMER_CODE);
stopTimer(beaconTimer);
}
void deviceTimerHandleRawData(char commandHeader, InputStream* inputStream, OutputStream* outputStream) {
if (commandHeader == COMMAND_TIMER_LIST) {
printTimerList(getInfoOutputStreamLogger(), getTimerList());
ackCommand(outputStream, TIMER_DEVICE_HEADER, COMMAND_TIMER_LIST);
}
else if (commandHeader == COMMAND_TIMER_COUNT) {
ackCommand(outputStream, TIMER_DEVICE_HEADER, COMMAND_TIMER_COUNT);
unsigned timerCount = getTimerCount();
appendHex2(outputStream, timerCount);
}
else if (commandHeader == COMMAND_TIMER_READ) {
ackCommand(outputStream, TIMER_DEVICE_HEADER, COMMAND_TIMER_READ);
unsigned char timerIndex = readHex2(inputStream);
Timer* timer = getTimerByIndex(timerIndex);
appendHex2(outputStream, timerIndex);
appendSeparator(outputStream);
appendHex2(outputStream, timer->timerCode);
appendSeparator(outputStream);
appendHex4(outputStream, timer->timeDiviser);
appendSeparator(outputStream);
appendHex4(outputStream, timer->timeInternalCounter);
appendSeparator(outputStream);
appendHex6(outputStream, timer->time);
appendSeparator(outputStream);
appendHex6(outputStream, timer->markTime);
appendSeparator(outputStream);
appendBool(outputStream, timer->enabled);
}
// Enable / Tisable
else if (commandHeader == COMMAND_TIMER_ENABLE_DISABLE) {
unsigned char timerIndex = readHex2(inputStream);
Timer* timer = getTimerByIndex(timerIndex);
checkIsSeparator(inputStream);
unsigned enableAsChar = readHex(inputStream);
bool enabled = enableAsChar != 0;
timer->enabled = enabled;
ackCommand(outputStream, TIMER_DEVICE_HEADER, COMMAND_TIMER_ENABLE_DISABLE);
}
// Mark
else if (commandHeader == COMMAND_TIMER_MARK) {
unsigned char timerIndex = readHex2(inputStream);
Timer* timer = getTimerByIndex(timerIndex);
unsigned long time = markTimer(timer);
ackCommand(outputStream, TIMER_DEVICE_HEADER, COMMAND_TIMER_MARK);
appendHex6(outputStream, time);
}
else if (commandHeader == COMMAND_TIMER_TIME_SINCE_LAST_MARK) {
unsigned char timerIndex = readHex2(inputStream);
Timer* timer = getTimerByIndex(timerIndex);
unsigned long value = getTimeSinceLastMark(timer);
ackCommand(outputStream, TIMER_DEVICE_HEADER, COMMAND_TIMER_TIME_SINCE_LAST_MARK);
appendHex6(outputStream, value);
}
else if (commandHeader == COMMAND_TIMER_TIMEOUT) {
unsigned char timerIndex = readHex2(inputStream);
checkIsSeparator(inputStream);
unsigned long timeToCheck = (unsigned long) readHex6(inputStream);
Timer* timer = getTimerByIndex(timerIndex);
bool value = timeout(timer, timeToCheck);
ackCommand(outputStream, TIMER_DEVICE_HEADER, COMMAND_TIMER_TIMEOUT);
appendHex2(outputStream, timerIndex);
appendSeparator(outputStream);
appendBool(outputStream, value);
}
// Demo
else if (commandHeader == COMMAND_TIMER_DEMO) {
Timer* timer = getTimerByCode(DEMO_TIMER_INDEX);
if (timer == NULL) {
timer = addTimerDemo();
}
// Timer could be null when adding the timerDemo because of limit, we don't want any crash !
if (timer != NULL) {
unsigned enableAsChar = readHex(inputStream);
bool enabled = enableAsChar != 0;
timer->enabled = enabled;
}
ackCommand(outputStream, TIMER_DEVICE_HEADER, COMMAND_TIMER_DEMO);
}
}
void clearPidTime(void) {
Timer* timer = getTimerByCode(TIMER_PID_CODE);
timer->time = 0;
lastPidTime = 0;
}
float getPidTimeInSecond(void) {
Timer* timer = getTimerByCode(TIMER_PID_CODE);
float result = (float)timer->time / (float) TIME_DIVIDER_1_HERTZ;
return result;
}
