/** Adjust Bot velocity according to current white line values.
* If white line sensors indicate checkpoint hit, flag (*checkpointHit) will be
* set. */
STATUS whiteLineVelocityAdjust(BOOL *checkpointHit /**< Pointer to flag
denoting whether checkpoint is hit. Value of the flag is always modified. */
) {
UINT idx;
UINT lBaseVel, rBaseVel;
BYTE lVel, rVel;
const UINT velReduction = 0; /* Used for braking, currently not supported */
STATUS ret;
ASSERT(checkpointHit != NULL);
ret = readWhiteLineSensors(&idx);
ASSERT(ret == STATUS_OK);
lBaseVel = BOT_VELOCITY;
rBaseVel = BOT_VELOCITY + VEL_ADJUST;
ret = motorVelocityGet(&lVel, &rVel);
ASSERT(ret == STATUS_OK);
*checkpointHit = FALSE;
switch(idx) {
case 0: *checkpointHit = TRUE;
break;/* Checkpoint */
case 1: lVel = MAX(BOT_VELOCITY - VEL_DEVIATION, lVel - NOTCH - velReduction);
rVel = MIN(BOT_VELOCITY + VEL_DEVIATION, rVel + NOTCH - velReduction);
break;
case 2: break; /* impossible - ignore */
case 3: lVel = MAX(BOT_VELOCITY - VEL_DEVIATION, lVel - 2*NOTCH - velReduction);
rVel = MIN(BOT_VELOCITY + VEL_DEVIATION, rVel + 2*NOTCH - velReduction);
break;
case 4: lVel = MIN(BOT_VELOCITY + VEL_DEVIATION, lVel + NOTCH - velReduction);
rVel = MAX(BOT_VELOCITY - VEL_DEVIATION, rVel - NOTCH - velReduction);
break;
case 5: lVel = lBaseVel;
rVel = rBaseVel;
break;
case 6: lVel = MIN(BOT_VELOCITY + VEL_DEVIATION, lVel + 2*NOTCH - velReduction);
rVel = MAX(BOT_VELOCITY - VEL_DEVIATION, rVel - 2*NOTCH - velReduction);
break;
case 7: lVel = lBaseVel;
rVel = rBaseVel;
break;
default: ASSERT(0);
}
motorVelocitySet(lVel, rVel);
return STATUS_OK;
}
/** Rotate the Bot in place by specified angle and direction.
* Note that angle of rotation is restricted to multiple of 90 degrees.
*/
STATUS rotateBot(MotorDirection dir, UINT angle) {
RotationAutomataState state;
UINT val, rVel, lVel, turn;
STATUS ret;
ASSERT(angle == 90 || angle == 180 || angle == 270);
ASSERT(dir == LEFT || dir == RIGHT);
/* Start the Bot */
lVel = BOT_ROTATION_SPEED;
rVel = BOT_ROTATION_SPEED + ROT_VEL_ADJUST;
motorVelocitySet(lVel, rVel);
motorDirectionSet(dir);
for(turn = 0; turn < angle/90; turn ++) {
/* Bot may be standing on background */
ret = readWhiteLineSensors(&val);
ASSERT(ret == STATUS_OK);
if(val == 7) {_delay_ms(800);}
state = RAS_START;
while(state != RAS_STOP) {
ret = readWhiteLineSensors(&val);
ASSERT(ret == STATUS_OK);
switch(state) {
case RAS_START: if(val == 7) {state = RAS_SEARCH;}
break;
case RAS_SEARCH: if(val == 5) {state = RAS_LOCKED;}
break;
case RAS_LOCKED: if(val != 5) {state = RAS_STOP;}
break;
case RAS_STOP:
break;
default: ASSERT(0);
}
/* Delay */
//_delay_ms(DELAY_COUNT);
}
}
motorDirectionSet(STOP);
_delay_ms(MOTOR_SAFETY_DELAY);
return STATUS_OK;
}
/** Move foward by specified distance (in millimeters) along white line.
* Parameter `stop' indicates whether Bot should halt after specified distance
* is covered.
*/
STATUS moveForwardFollwingLineByDistance(ULINT distInMm, /**< Distance in
millimeters */
BOOL stop /**< Halts the Bot if TRUE, continues the current motion otherwise
*/
) {
ULINT posCount;
BOOL checkPoint;
UINT lVel, rVel;
STATUS ret;
motorLeftPositionEncoderInterruptConfig(INTR_OFF); /* Mask interrupt */
motorRightPositionEncoderInterruptConfig(INTR_OFF); /* Mask interrupt */
/* Configure distance */
posCount = getPosCountForDistance(distInMm);
lPosCount = rPosCount = 0; /* Reset position counters */
motorLeftPositionEncoderInterruptConfig(INTR_ON); /* Unmask interrupt */
/* Start the Bot */
lVel = BOT_VELOCITY;
rVel = BOT_VELOCITY + VEL_ADJUST;
motorVelocitySet(lVel, rVel);
motorDirectionSet(FORWARD);
while(lPosCount < posCount) {
ret = whiteLineVelocityAdjust(&checkPoint);
ASSERT(ret == STATUS_OK);
/* Delay */
// _delay_ms(DELAY_COUNT);
}
motorLeftPositionEncoderInterruptConfig(INTR_OFF); /* Mask interrupt */
motorRightPositionEncoderInterruptConfig(INTR_OFF); /* Mask interrupt */
if(stop == TRUE){
motorDirectionSet(STOP);
_delay_ms(MOTOR_SAFETY_DELAY);
}
return STATUS_OK;
}
/** Move foward by specified number of checkpoints along white line.
* Parameter `stop' indicates whether Bot should halt after specified distance
* is covered.
*/
STATUS moveForwardFollwingLineByCheckpoint(UINT chkpts, /**< No. of checkpoints
by which Bot moves. */
BOOL stop /**< Halts the Bot if TRUE, continues the current motion otherwise
*/
) {
BOOL checkPoint, ignoreCheckPoint;
UINT lVel, rVel;
STATUS ret;
/* Start the Bot */
lVel = BOT_VELOCITY;
rVel = BOT_VELOCITY + VEL_ADJUST;
ignoreCheckPoint = FALSE;
motorVelocitySet(lVel, rVel);
motorDirectionSet(FORWARD);
while(chkpts > 0) {
ret = whiteLineVelocityAdjust(&checkPoint);
ASSERT(ret == STATUS_OK);
if(checkPoint == TRUE) {
if(ignoreCheckPoint == FALSE) {
ignoreCheckPoint = TRUE;
chkpts --;
}
}
else {
ignoreCheckPoint = FALSE;
}
/* Delay */
//_delay_ms(DELAY_COUNT);
}
moveForwardFollwingLineByDistance(75, stop);
return STATUS_OK;
}
int main() {
MotorDirection dirs[] = {FORWARD, BACKWARD, RIGHT, LEFT,
SOFT_RIGHT, SOFT_LEFT, SOFT_RIGHT2, SOFT_LEFT2, STOP};
char strDirs[9][16] = {"FORWARD", "BACKWARD", "RIGHT", "LEFT",
"SOFT_RIGHT", "SOFT_LEFT", "SOFT_RIGHT2", "SOFT_LEFT2", "STOP"};
int idx;
initMotor();
initLcd();
#if 0
/* Test #1: Check direction */
motorVelocitySet(255, 255);
for(idx = 0; idx < (sizeof(dirs)/sizeof(dirs[0])); idx ++) {
lcdClear();
lcdCursor(1,1);
lcdString(strDirs[idx]);
_delay_ms(DELAY_COUNT*2);
motorDirectionSet(dirs[idx]);
/* Delay for some time */
_delay_ms(DELAY_COUNT);
motorDirectionSet(STOP);
}
/* Test #2: Check speed */
lcdClear();
lcdCursor(1,1);
lcdString("Speed : ");
lcdCursor(2,1);
lcdString("L 100, R 100");
_delay_ms(DELAY_COUNT*2);
motorVelocitySet(100, 100);
motorDirectionSet(FORWARD);
/* Delay for some time */
_delay_ms(DELAY_COUNT);
motorDirectionSet(STOP);
/* Test #3: Check individual wheel speed (L < R) */
lcdClear();
lcdCursor(1,1);
lcdString("Speed : ");
lcdCursor(2,1);
lcdString("L 100, R 255");
_delay_ms(DELAY_COUNT*2);
motorVelocitySet(100, 255); /* Left turn expected */
motorDirectionSet(FORWARD);
/* Delay for some time */
_delay_ms(DELAY_COUNT);
motorDirectionSet(STOP);
/* Test #4: Check individual wheel speed (L > R) */
lcdClear();
lcdCursor(1,1);
lcdString("Speed : ");
lcdCursor(2,1);
lcdString("L 255, R 100");
_delay_ms(DELAY_COUNT*2);
motorVelocitySet(255, 100); /* Right turn expected */
motorDirectionSet(FORWARD);
/* Delay for some time */
_delay_ms(DELAY_COUNT);
motorDirectionSet(STOP);
/* Test #5: Check left position encoder */
motorLeftPositionEncoderInit(leftPosEncoderIsr);
lPosCount = 100;
motorVelocitySet(255, 255);
motorDirectionSet(FORWARD);
#endif
/* Test #6: Check orientation */
motorLeftPositionEncoderInit(leftPosEncoderIsr);
lPosCount = 23;
motorVelocitySet(150, 150);
motorDirectionSet(LEFT);
while(1);
return 0;
}
