void readNextDistance(I2cBusConnection* i2cBusConnection) {
if (!readNextDistanceFlag) {
return;
}
readNextDistanceFlag = false;
unsigned int distance = getSRF02DistanceEndRanging(i2cBusConnection, SONAR_INDEX);
delaymSec(1);
startSRF02Ranging(i2cBusConnection, SONAR_INDEX);
distances[(unsigned int) distanceIndex] = distance;
/*
appendDec(getDebugOutputStreamLogger(), distance);
append(getDebugOutputStreamLogger(), '.');
*/
// Manage index of history
distanceIndex++;
if (distanceIndex > ROBOT_SONAR_DETECTOR_DEVICE_HISTORY_SIZE) {
distanceIndex = 0;
}
if (checkObstacle()) {
// for the main LOOP
obstacle = true;
}
}
void deviceRobotSonarDetectorInit() {
// Timer for detector
addTimer(ROBOT_SONAR_DETECTOR_TIMER_INDEX,
TIME_DIVIDER_16_HERTZ,
deviceRobotSonarDetectorCallbackFunc, "ROBOT SONAR DETECTOR TIMER");
// useSonar = isConfigSet(CONFIG_USE_LASER_MASK);
clearHistory();
startSRF02Ranging(robotSonarDetectorI2cBusConnection, SONAR_INDEX);
delaymSec(65);
getSRF02DistanceEndRanging(robotSonarDetectorI2cBusConnection, SONAR_INDEX);
obstacle = false;
readNextDistanceFlag = false;
}
unsigned int getSRF02Distance(unsigned char sonarIndex) {
startSRF02Ranging(sonarIndex);
delaymSec(100);
unsigned int result = getSRF02DistanceEndRanging(sonarIndex);
return result;
}
unsigned char hasRobotObstacle(void) {
unsigned useSonar = false;
if (useSonar) {
if (rangingIndex == 0) {
// start ranging at the first index, because it takes 65 ms to have value
startSRF02Ranging();
}
rangingIndex++;
// At the end of ranging index, we get the value of distance
if (rangingIndex == RANGING_DELAY) {
rangingIndex = 0;
unsigned int rangingValue = getSRF02DistanceEndRanging();
/*
sendDec(rangingValue);
println();
*/
// Manage history
// we cancel low value, as it is often parasite
if (rangingValue > MIN_DETECTION_DISTANCE_CM) {
distances[distanceIndex++] = rangingValue;
if (distanceIndex >= HISTORY_SIZE) {
distanceIndex = 0;
}
// Check the history of ranging to know
int i = 0;
int n = HISTORY_SIZE;
unsigned count = 0;
for (i = 0; i < n; i++) {
// char notToClosed = (distances[(i + distanceIndex) % n] > MIN_DETECTION_DISTANCE);
// char closed = (distances[(i + distanceIndex) % n] < OBSTACLE_DISTANCE_CM);
// char notToClosed = (distances[i] > MIN_DETECTION_DISTANCE_CM);
char closed = (distances[i] < OBSTACLE_DISTANCE_CM);
// if (notToClosed && closed) {
if (closed) {
count++;
}
}
if (count >= n - 2) {
unsigned lastLaserPosition = getLastLaserServoPosition();
if (lastLaserPosition > SERVO_VALUE_MIN_NOTIFY_OBSTACLE && lastLaserPosition < SERVO_VALUE_MAX_NOTIFY_OBSTACLE) {
// sendHex4(lastLaserPosition);
return OBSTACLE;
}
}
}
}
return NO_OBSTACLE;
}
else {
// we wait time to notify again
unsigned int currentTimeInSecond = getCurrentTimeInSecond();
if (currentTimeInSecond > getLastNotifyObstacleTime() + OBSTACLE_DELAY_BEFORE_NEXT_NOTIFICATION) {
if ((lastServoPositionHit[LEFT_DIRECTION] > 0) && (lastServoPositionHit[RIGHT_DIRECTION] > 0)) {
return OBSTACLE;
}
}
}
return NO_OBSTACLE;
}
unsigned int getSRF02Distance(I2cBusConnection* i2cBusConnection, unsigned char sonarIndex) {
startSRF02Ranging(i2cBusConnection, sonarIndex);
delaymSec(100);
unsigned int result = getSRF02DistanceEndRanging(i2cBusConnection, sonarIndex);
return result;
}