void beepAltiVersion (int majorNbr, int minorNbr)
{
int i;
for (i = 0; i < majorNbr; i++)
{
longBeep();
}
for (i = 0; i < minorNbr; i++)
{
shortBeep();
}
}
void shortBeepRepeat( int digit ) {
#ifdef DEBUG
SerialCom.println("shortBeepRepeat: " );
SerialCom.println( digit);
#endif
int i;
for ( i = 0; i < digit; i++ ) {
if ( i > 0 ) {
delay(250);
}
shortBeep();
}
}
void beepAltitude(long altitude)
{
int i;
int nbrLongBeep = 0;
int nbrShortBeep = 0;
// this is the last thing that I need to write, some code to beep the altitude
//altitude is in meters
//find how many digits
if (altitude > 99)
{
// 1 long beep per hundred meter
nbrLongBeep = int(altitude / 100);
//then calculate the number of short beep
nbrShortBeep = (altitude - (nbrLongBeep * 100)) / 10;
}
else
{
nbrLongBeep = 0;
nbrShortBeep = (altitude / 10);
}
if (nbrLongBeep > 0)
for (i = 1; i < nbrLongBeep + 1 ; i++)
{
longBeep();
delay(50);
}
if (nbrShortBeep > 0)
for (i = 1; i < nbrShortBeep + 1 ; i++)
{
shortBeep();
delay(50);
}
delay(5000);
}
/*
* Random movements using pivot turns
*/
void randomMovement(void) {
isWaiting = 0;
isSearching = 1;
isSpeedRunning = 0;
int cellCount = 1; // number of explored cells
int turnCount = 0;
int remainingDist = 0; // positional distance
bool beginCellFlag = 0;
bool quarterCellFlag = 0;
bool halfCellFlag = 0;
bool threeQuarterCellFlag = 0;
bool fullCellFlag = 0;
bool hasFrontWall = 0;
bool hasLeftWall = 0;
bool hasRightWall = 0;
int nextMove = 0;
targetSpeedX = searchSpeed;
while(1) { // run forever
remainingDist = cellCount*cellDistance - encCount;
if (!beginCellFlag && (remainingDist <= cellDistance)) { // run once
beginCellFlag = 1;
useIRSensors = 1;
useGyro = 0;
useSpeedProfile = 1;
// If has front wall or needs to turn, decelerate to 0 within half a cell distance
if (hasFrontWall || nextMove == TURNLEFT || nextMove == TURNRIGHT || nextMove == TURNBACK) {
if(needToDecelerate(remainingDist, (int)mm_to_counts(curSpeedX), (int)mm_to_counts(stopSpeed)) < decX) {
targetSpeedX = searchSpeed;
}
else {
targetSpeedX = stopSpeed;
}
}
else
targetSpeedX = searchSpeed;
}
// Reached quarter cell
if (!quarterCellFlag && (remainingDist <= cellDistance*3/4)) {
quarterCellFlag = 1;
}
if (quarterCellFlag && !threeQuarterCellFlag)
useIRSensors = 1;
// Reached half cell
if (!halfCellFlag && (remainingDist <= cellDistance/2)) { // Run once
halfCellFlag = 1;
// Read wall and set wall flags
if ((LFSensor > frontWallThresholdL) || (RFSensor > frontWallThresholdR))
hasFrontWall = 1;
if (LDSensor > leftWallThreshold)
hasLeftWall = 1;
if (RDSensor > rightWallThreshold)
hasRightWall = 1;
// Store destination cell's wall data
// Decide next movement (search algorithm)
while (1) {
nextMove = (millis() % 4) + 1;
if ((nextMove == GOFORWARD) && (!hasFrontWall))
break;
if ((nextMove == TURNLEFT) && (!hasLeftWall))
break;
if ((nextMove == TURNRIGHT) && (!hasRightWall))
break;
if ((nextMove == TURNBACK) && (hasFrontWall && hasLeftWall && hasRightWall))
break;
}
}
// Reached three quarter cell
if (!threeQuarterCellFlag && (remainingDist <= cellDistance*1/4)) { // run once
threeQuarterCellFlag = 1;
}
if (threeQuarterCellFlag) {
// Check for front wall to turn off for the remaining distance
if (hasFrontWall)
useIRSensors = 0;
}
// Reached full cell
if ((!fullCellFlag && (remainingDist <= 0)) || (LFSensor > 2000) || (RFSensor > 2000)) {
fullCellFlag = 1;
cellCount++;
shortBeep(200, 1000);
// If has front wall, align with front wall
if (hasFrontWall) {
alignFrontWall(LFvalue1, RFvalue1, alignTime); // left, right value
}
// Reached full cell, perform next move
if (nextMove == TURNLEFT) {
pivotTurn(turnLeft90);
turnCount++;
}
else if (nextMove == TURNRIGHT) {
pivotTurn(turnRight90);
turnCount++;
}
else if (nextMove == TURNBACK) {
pivotTurn(turnLeft180);
turnCount++;
}
else if (nextMove == GOFORWARD) {
// Continue moving forward
}
beginCellFlag = 0;
quarterCellFlag = 0;
halfCellFlag = 0;
threeQuarterCellFlag = 0;
fullCellFlag = 0;
hasFrontWall = 0;
hasLeftWall = 0;
hasRightWall = 0;
}
}
}
