void moveFowardAndRightCell(int moveSpeed, int maxSpeed)
{
targetSpeedX = moveSpeed;
targetSpeedW = 0;
do
{
if (needToDecelerate(distanceLeft, curSpeedX, moveSpeed) < decX)
{
targetSpeedX = maxSpeed;
}
else
{
targetSpeedX = moveSpeed;
}
} while(((encoderCount - oldEncoderCount) < oneCellDistance && LFSensor < LFValue2 && RFSensor < RFValue2)
|| (LFSensor < LFValue1 && LFSensor > LFValue2)
|| (RFSensor < RFValue1 && RFSensor > RFValue2));
oldEncoderCount = encoderCount;
}
/*
* 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;
}
}
}
