Software (actually, just the main procedure):
IgORE2 PIC18F452
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
// Main procedure
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
void main()
{
// Initialize the hardware subsystems
initPorts();
initPWM();
initADC();
initI2C();
initRTI();
initLCD();
// Initialize variables
output_high(STATUS_LED); // STATUS LED is OFF
heartbeat = 50; // 50 * 20 ms = 1 second
leftUVtronCount = 0; // UVtrons
rightUVtronCount = 0;
prevLeftEncoder = 0; // Wheel encoders
prevRightEncoder = 0;
leftEncoderCount = 0;
rightEncoderCount = 0;
enable_interrupts(GLOBAL); // Enable interrupts
enable_interrupts(INT_TIMER0); // Realtime interrupt
enable_interrupts(INT_RB); // UVtrons and wheel encoders interrupt
// Select the mode of operation
if (input(SWITCH1) == 0) // Calibrate mode
{
displayMode = input(SWITCH2);
switch (displayMode)
{
case 1:
clearLCD(); // 012345678901234567890123
printf(writeLcdData, "Line Dets | PIR | UVtron");
do
{
leftDetector = sampleADC(LF_LINE_DETECTOR);
rightDetector = sampleADC(RT_LINE_DETECTOR);
leftUVtronCount = 0;
rightUVtronCount = 0;
delay_ms(1000);
pirReading = sampleADC(ELTEC);
setLine(2);
printf(writeLcdData, "%LX %LX %LX %X %LX", leftDetector, rightDetector, pirReading, leftUVtronCount, rightUVtronCount);
delay_ms(1000);
} while(TRUE);
break;
case 0:
clearLCD(); // 012345678901234567890123
printf(writeLcdData, "Left Forward Right");
do
{
controlServo(LEFT);
i2cSRF08WriteByte(SRF08_ADDR, COMMAND_REG, DIST_IN_CM);
delay_ms(100);
wordTemp2 = i2cSRF08ReadWord(SRF08_ADDR, ECHO_1);
controlServo(FORWARD);
i2cSRF08WriteByte(SRF08_ADDR, COMMAND_REG, DIST_IN_CM);
delay_ms(100);
wordTemp3 = i2cSRF08ReadWord(SRF08_ADDR, ECHO_1);
controlServo(RIGHT);
i2cSRF08WriteByte(SRF08_ADDR, COMMAND_REG, DIST_IN_CM);
delay_ms(100);
wordTemp4 = i2cSRF08ReadWord(SRF08_ADDR, ECHO_1);
setLine(2);
printf(writeLcdData, "%LX %LX %LX", wordTemp2, wordTemp3, wordTemp4);
delay_ms(500);
} while(TRUE);
break;
}
}
// End of select the mode of operation
// Let the search begin . . .
//
// Follow the left wall until it disappears
//
controlServo(LEFT);
i2cSRF08WriteByte(SRF08_ADDR, RANGE_REG, MAX_RANGE_47CM);
i2cSRF08WriteByte(SRF08_ADDR, GAIN_REG, GAIN_47CM);
i2cSRF08WriteByte(SRF08_ADDR, COMMAND_REG, DIST_IN_CM);
delay_ms(20);
desiredDistance = i2cSRF08ReadWord(SRF08_ADDR, ECHO_1);
accelerate(FORWARD, 70);
followWallDisappear(LEFT);
//
// Turn the corner into room #1
//
rightEncoderCount = 0;
setMotors(FORWARD, 45, FORWARD, 60);
delay_ms(10);
setMotors(FORWARD, 23, FORWARD, 51);
do {} while (rightEncoderCount < 1850);
leftUVtronCount = 0;
rightUVtronCount = 0;
do {} while (rightEncoderCount < 3800);
//
// If Uvtrons = 0 while going around the corner, then look into near corner
//
if (leftUVtronCount == 0)
{
rightUVtronCount = 0;
rightEncoderCount = 0;
setMotors(FORWARD, 40, FORWARD, 40);
do {} while (rightEncoderCount < 1000);
stop();
rotate(CW, 60);
delay_ms(300);
near = FALSE;
if (leftUVtronCount != 0)
{
near = TRUE;
extinguishCandleInRoom(1);
}
}
//
// else detected UV in room 1
//
else
{
setMotors(FORWARD, 45, FORWARD, 40); // Move into room #1
rightEncoderCount = 0;
pirReading = sampleADC(ELTEC);
foundIt = FALSE;
do // Look for candle while moving into room 1
{
ADCON0 = ADCON0 | 0b00000100;
while (goDone);
pirReading = ADRESH * 256 + ADRESL;
if (pirReading > 0x0280)
foundIt = TRUE;
} while((!foundIt) & (rightEncoderCount < 2800));
stop();
if (foundIt) // If found canlde while moving in . . .
{
rotate(CCW, 65);
moveCloseToObject(18, 25); // Move close enough to candle
tooCloseToWall(); // Is there a wall too close on either side?
done = FALSE;
wiggleBum(); // Point propeller at candle and blow it out
}
else
extinguishCandleInRoom(1);
}
//
// Candle is not in room 1
//
rotate(CW, 120);
lfThreshold = sampleADC(LF_LINE_DETECTOR) - THRESHOLD_INCREMENT;
rtThreshold = sampleADC(RT_LINE_DETECTOR) - THRESHOLD_INCREMENT;
squareToLine(); // Look for white line and square to it
//
// Move out of room #1
//
accelerate(FORWARD, 50);
rightEncoderCount = 0;
setMotors(FORWARD, 12, FORWARD, 25);
delay_ms(100);
setMotors(FORWARD, 18, FORWARD, 37);
delay_ms(100);
setMotors(FORWARD, 25, FORWARD, 50);
do {} while (rightEncoderCount < 1600);
setMotors(FORWARD, 35, FORWARD, 35);
delay_ms(50);
leftUVtronCount = 0;
leftEncoderCount = 0;
setMotors(FORWARD, 25, FORWARD, 12);
delay_ms(100);
setMotors(FORWARD, 37, FORWARD, 18);
delay_ms(100);
setMotors(FORWARD, 50, FORWARD, 25);
do {} while (leftEncoderCount < 1400);
near = FALSE;
if (leftUVtronCount != 0)
near = TRUE; // candle is in near end of room 2
//
// Move into room #2
//
setMotors(FORWARD, 40, FORWARD, 40);
rightEncoderCount = 0;
do {} while(rightEncoderCount < 600);
stop();
if (near)
extinguishCandleInRoom(2);
else
{
if (rotateWhileSearch(CW))
extinguishCandleInRoom(2);
}
//
// Candle is not in room 2
//
lfThreshold = sampleADC(LF_LINE_DETECTOR) - THRESHOLD_INCREMENT;
rtThreshold = sampleADC(RT_LINE_DETECTOR) - THRESHOLD_INCREMENT;
squareToLine(); // Look for white line and square to it
//
// Move out of room #2
//
accelerate(FORWARD, 40);
delay_ms(150);
rightEncoderCount = 0;
setMotors(FORWARD, 27, FORWARD, 60);
do {} while (rightEncoderCount < 1750);
setMotors(FORWARD, 70, FORWARD, 70);
//
// Follow left wall until it disappears
//
controlServo(LEFT);
i2cSRF08WriteByte(SRF08_ADDR, RANGE_REG, MAX_RANGE_47CM);
i2cSRF08WriteByte(SRF08_ADDR, GAIN_REG, GAIN_47CM);
i2cSRF08WriteByte(SRF08_ADDR, COMMAND_REG, DIST_IN_CM);
delay_ms(20);
desiredDistance = i2cSRF08ReadWord(SRF08_ADDR, ECHO_1);
followWallDisappear(LEFT);
//
// Follow right wall until it disappears
//
controlServo(RIGHT);
i2cSRF08WriteByte(SRF08_ADDR, RANGE_REG, MAX_RANGE_47CM);
i2cSRF08WriteByte(SRF08_ADDR, GAIN_REG, GAIN_47CM);
i2cSRF08WriteByte(SRF08_ADDR, COMMAND_REG, DIST_IN_CM);
delay_ms(20);
desiredDistance = i2cSRF08ReadWord(SRF08_ADDR, ECHO_1);
followWallDisappear(RIGHT);
//
// Turn the corner into room #3
//
leftEncoderCount = 0;
setMotors(FORWARD, 50, FORWARD, 40);
delay_ms(10);
setMotors(FORWARD, 40, FORWARD, 20);
do {} while (leftEncoderCount < 1900);
setMotors(FORWARD, 35, FORWARD, 35);
//
// Move into room #3
//
rightEncoderCount = 0;
do {} while(rightEncoderCount < 1000);
stop();
if (rotateWhileSearch(CW))
extinguishCandleInRoom(3);
//
// Candle is not in room 4
//
lfThreshold = sampleADC(LF_LINE_DETECTOR) - THRESHOLD_INCREMENT;
rtThreshold = sampleADC(RT_LINE_DETECTOR) - THRESHOLD_INCREMENT;
squareToLine(); // Look for white line and square to it
//
// Move out of room #3
//
accelerate(FORWARD, 50);
rightEncoderCount = 0;
setMotors(FORWARD, 27, FORWARD, 60);
do {} while (rightEncoderCount < 1700);
setMotors(FORWARD, 50, FORWARD, 50);
i2cSRF08WriteByte(SRF08_ADDR, RANGE_REG, MAX_RANGE_47CM);
i2cSRF08WriteByte(SRF08_ADDR, GAIN_REG, GAIN_47CM);
i2cSRF08WriteByte(SRF08_ADDR, COMMAND_REG, DIST_IN_CM);
delay_ms(20);
desiredDistance = i2cSRF08ReadWord(SRF08_ADDR, ECHO_1);
followWallDisappear(RIGHT);
leftEncoderCount = 0;
setMotors(FORWARD, 40, FORWARD, 20);
do {} while (leftEncoderCount < 1700);
setMotors(FORWARD, 50, FORWARD, 50);
i2cSRF08WriteByte(SRF08_ADDR, RANGE_REG, MAX_RANGE_47CM);
i2cSRF08WriteByte(SRF08_ADDR, GAIN_REG, GAIN_47CM);
i2cSRF08WriteByte(SRF08_ADDR, COMMAND_REG, DIST_IN_CM);
delay_ms(20);
desiredDistance = i2cSRF08ReadWord(SRF08_ADDR, ECHO_1);
followWallDisappear(RIGHT);
leftEncoderCount = 0;
setMotors(FORWARD, 40, FORWARD, 20);
do {} while (leftEncoderCount < 1800);
setMotors(FORWARD, 35, FORWARD, 35);
//
// Move into room #4
//
rightEncoderCount = 0;
do {} while(rightEncoderCount < 500);
rotate(CW, 45);
moveCloseToObject(35, 25);
stop();
extinguishCandleInRoom(4);
stop();
do {} while(TRUE);
}
// *********************************************************
//
// End of main()
//
// *********************************************************