void doInstruction() {
int currentTime = getCurrentTimeInSecond();
if (instructionIndex == 0 && currentTime > 1) {
motionDriverForward(600.0f);
instructionIndex++;
}
else if (instructionIndex == 1 && currentTime > 10) {
motionDriverLeft(1800.0f);
}
else if (instructionIndex == 2 && currentTime > 20) {
motionDriverForward(600.0f);
}
}
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;
}
