/** 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; }