Procedure and description for returning home from room without fire (guangmaoda fire suppression robot)

Sensor connection
Infrared sensor connection
Front infrared: Number 9
Left infrared: Number 15
Left 45 degree angle infrared: Number 10
Right 45-degree infrared: Number 8
Right infrared: Number 14
Flame sensor connection
Left flame: simulation 3
Medium flame: Simulation 5
Right flame: Simulation 4
(Bottom) grayscale sensor: analog 2
Voice Control Sensor: analog 6

Program description

# Define p 120 // defines the return value of the flame detected by the flame Sensor
Int k, j, I = 0, n, B = 1, c = 1; // program control variable, do not need to change
Int m = I;
Int pro, end = 1;

Void main () // main program
{
While (analog (6)> 100) // enable voice control
{
}

While (! (Analog (2)> 100) // exit the white superstart Area
{
Motor (0, 80 );
Motor (1, 80 );
}

Pro = start_process (test (); // start the ground mark detection process

While (1) // fire extinguishing and maze program Switching
{
If (analog (3 ){
Fire ();
}
Else // no flame found, entering the maze subroutine
{
Migong ();
}
}
}

Void migong () // maze subroutine
{
If (digital (8) = 0 II digital (9) = 0) // if an obstacle is detected in front or right 45 degrees infrared, turn left
{
Motor (0,-70); // adjust the power parameters according to the situation.
Motor (1, 70 );
}
Else if (digital (14) = 0 & digital (9) = 1 & digital (8) = 1) // if only the right side has obstacles, go straight
{
Motor (0,100 );
Motor (1,100 );
}
Else // if no obstacle exists, turn right
{
Motor (0,100 );
Motor (1,-100 );

Motor (1, 10 );
Motor (0, 90 );
}
If (I> 2) & (I> m) // if the number of flags exceeds 2 and the number of flags changes (for rooms 1, 2, and 3)
{
Stop ();
While (analog (3)> 150 & analog (5)> 150 & analog (4)> 150) // if no flame is detected
{
Motor (0, 70); // right
Motor (1,-70 );
If (digital (14) = 0) // The right infrared detects the obstacle and stops rotating.
Break;
}
M = I + 1; // change the comparison variable of the logo line
}
If (I = 1 & B & (analog (3)> 150 & analog (5)> 150 & analog (4)> 150 )) // Room 4, with 1st lines detected and no flame
{
While (digital (9) = 0 II digital (8) = 0 II digital (14) = 0) // any right hand infrared sensor has obstacles and right turn
{
Motor (0, 70 );
Motor (1,-70 );
}
While (digital (9) = 1) // the sensor in front has no obstacle, right forward
{
Motor (0,100 );
Motor (1, 70 );
}
B = 0; // control variable
}
}

Void fire () // fire suppression subroutine
{
If (I> 4 II I <8) & c) // if you are in Room 2
{
While (analog (3 ){
Motor (0,-60 );
Motor (1, 60 );
}
Motor (); // force a straight row
Motor (); // It is mainly used to solve the wall hit when the right-hand principle of Room 2 is used for fire suppression.
Sleep (0.6 );
C = 0;
}
While (analog (5) <10 & analog (2) <60) // if the flame distance is appropriate and the ground marking line is detected, extinguish the fire
{
Motor (0,-10); // return a paragraph
Motor (1,-10 );
Sleep (0.15 );
Stop ();
Motor (2,100); // extinguish
Sleep (2.000 );
Stop ();
Motor (0,-70); // after the fire is cleared, move back
Motor (1,-100 );
Sleep (0.2 );

Kill_process (pro); // turn off the ground mark detection process
Select (); // enter the home subroutine to select a subroutine
}
While (analog (3)-analog (4)> 3) // adjust to the right. If the adjustment is frequent, modify the last 3
{
Motor (0, 50 );
Motor (1,-50 );
}
While (analog (4)-analog (3)> 3) // adjust to the left, same as above
{
Motor (0,-50 );
Motor (1, 50 );
}
Motor (); // direct
Motor (1, 40 );
}

Void test () // subprogram of ground mark Detection
{
While (1)
{
K = analog (2 );
If (k <30 II k> 150) // 30 and 150 are the marks detected by the bottom sensor and the black ground respectively.
J = k; // value. Modify the value according to the actual test value.
If (j <30) & (n> 150) // if changed, it is also changed here
{
I ++;
Beep ();
}
N = j;
}
}

Void select () // return program selection subroutine
{
If (I <3) // if the number of lines on the ground is less than 3 (in Room 4)
Gohome_4 (); // enter the sub-Program for returning home in Room 4, the same below
Else if (I <5)
Gohome_3 ();
Else if (I <8)
Gohome_2 ();
Else
Gohome_1 ();
}

Void gohome_4 () // subprogram for returning home in Room 4
{
Start_process (endstop (); // start the end (start) point to stop the program, the same below
Mg_right (); // return home with the right hand rule
Stop (); // stop
Sleep (100.0 );
}

Void gohome_3 () // subroutine for returning home in Room 3
{
Start_process (endstop ());
Mg_left (); // The left-hand rule is returned.
Stop ();
Sleep (100.0 );
}

Void gohome_2 () // special home program for Room 2
{
Motor (); // turn right after the fire is turned off
Motor (1,-60 );
Sleep (0.2 );
Reset_system_time (); // reset the system time
While (mseconds () <2100L) // run the left-hand rule to walk the maze for 2.1 seconds (the length of time is the corridor that can walk out of the door of the room and less than Room 3)
{
While (digital (9) = 0 II digital (10) = 0)
{
Motor (0, 60 );
Motor (1,-50 );
}
While (digital (15) = 0 & digital (9) = 1 & digital (10) = 1)
{
Motor (0, 50 );
Motor (1, 50 );
}
While (digital (15) = 1 & digital (9) = 1 & digital (10) = 1)
{
Motor (0,-80 );
Motor (1, 80 );

Motor (0, 45 );
Motor (1, 80 );
}
}
Stop ();
Sleep (0.1 );
While (digital (9) = 0) // if there are obstacles in front, turn right (adjust posture)
{
Motor (0, 60 );
Motor (1,-60 );
}
While (digital (9) = 1) // if there are no obstacles in front, the left and right hand rules go forward and basically go straight to the other end (next to Room 3)
{
If (digital (8) = 0) // There is an obstacle at 45 degrees on the right, turn left
{
Motor (0,-50 );
Motor (1, 50 );
}
If (digital (14) = 0 & digital (8) = 1) // There are obstacles on the right side and no obstacles on the right 45 degrees corner, go straight
{
Motor (0, 50 );
Motor (1, 50 );
}
If (digital (10) = 0) // There are obstacles at 45 degrees on the left, right turn
{
Motor (0, 50 );
Motor (1,-50 );
}
If (digital (15) = 0 & digital (10) = 1) // There are obstacles on the left and no obstacles at 45 degrees on the left, go straight
{
Motor (0, 50 );
Motor (1, 50 );
}
Motor (); // direct
Motor (1, 50 );
}
Start_process (endstop (); // when there is an obstacle in front of it, exit the loop and enable the end point to stop the process.
While (end) // The left-hand rule follows the maze
{
If (digital (9) = 0 II digital (10) = 0)
{
Motor (0, 60 );
Motor (1,-50 );
}
Else if (digital (15) = 0 & digital (9) = 1 & digital (10) = 1)
{
Motor (0, 70 );
Motor (1, 70 );
}
Else
{
Motor (0,-80 );
Motor (1, 80 );

Motor (0, 40 );
Motor (1,100 );
}
}
Motor (0,-100); // end brake
Motor (1,-100 );
Sleep (0.1 );
Stop (); // stop
Sleep (100.0 );
}

Void gohome_1 () // go home from Room 1
{
Start_process (endstop (); // enable the end point to stop the process.
Mg_right (); // The right hand rule follows the maze
Stop ();
Sleep (100.0 );
}

Void mg_right () // The right-hand rule follows the maze subroutine
{
While (end)
{
If (digital (8) = 0 II digital (9) = 0)
{
Motor (0,-70 );
Motor (1, 70 );
}
Else if (digital (14) = 0 & digital (9) = 1 & digital (8) = 1)
{
Motor (0,100 );
Motor (1,100 );
}
Else
{
Motor (0,100 );
Motor (1,-100 );

Motor (1, 20 );
Motor (0,100 );
}
}
}

Void mg_left () // The left-hand rule follows the maze subroutine
{
While (end)
{
If (digital (9) = 0 II digital (10) = 0)
{
Motor (0, 60 );
Motor (1,-50 );
}
Else if (digital (15) = 0 & digital (9) = 1 & digital (10) = 1)
{
Motor (0, 70 );
Motor (1, 70 );
}
Else
{
Motor (0,-100 );
Motor (1,100 );

Motor (0, 30 );
Motor (1,100 );
}
}
}

Void endstop () // End stop process subroutine
{
While (1)
{
K = analog (2); // check once
Sleep (0.02); // interval of 0.02 seconds before detection // The time can be appropriately shortened or extended
If (k <50 & amp; analog (2) <50) // if both are white, the end is reached and the end is stopped.
{
End = 0;
Stop ();
Sleep (1.0 );
}
}
}

// End all programs