Intelligent
Rescue Robot
Tutorial
Maze
Line
Ball
Robot parts Introduction
POP-BOT32i Rover (top side)
Gripper-X
consist of : �2 servo motors
Gripper mechanism
POP-32i Controller Board
Left motor
Right motor
Back
Front
POP-BOT32i Rover (bottom side)
ZX-03 light reflector (Left)
ZX-03 light reflector (Right)
DC motor gearbox (Left)
DC motor gearbox (Right)
Caster wheel
POP-BOT32i Rover (front & back side)
ZX-SONAR1M
Ultrasonic sensor
ZX-SWITCH01
Switch sensor (Left)
ZX-SWITCH01
Switch sensor (Right)
Back
Front
Wiring
Instructions
Wiring instructions
Battery 6-9Vdc
ZX-SWITCH01
Switch sensor (Left)
ZX-Switch01
Switch Sensor (Right)
DC motor (Right)
USB-C port
ZX-03 sensor (Left)
ZX-03 sensor (Right)
ZX-SONAR1M
Ultrasonic sensor
GRIPPER
SHOULDER
DC motor (Left)
P22
P23
P0
P1
P16
SERVO1
SERVO2
MOTOR2
MOTOR1
Getting Started with
Arduino IDE
Getting started with �the Arduino IDE
Arduino IDE or Arduino Integrated Development Environment contains a text editor for writing code, a message area, a text console, a toolbar with buttons for common functions and a series of menus. It connects to the Arduino hardware to upload programs and communicate with them.
the latest versions of Arduino IDE.
Download the Arduino IDE
Go to https://www.arduino.cc/en/software and download the latest Arduino IDE.
1
2
3
4
5
Install the Arduino IDE
Double-click
Double-click the .exe installation file to start the installation.
2
1
3
4
6
5
Overview the Arduino IDE
Verify/Upload
Select Board & Port
Open
Serial Plotter
Open
Serial Monitor
Search
Debugger
Library Manager
Board Manager
Sketchbook
Overview the Arduino IDE
POP-32i board installation for Arduino IDE
(1) In the Arduino IDE, go to File -> Preferences.
(2) In the Additional Boards Manager URLs input field, type:
https://github.com/INEXdev/ArduinoSTM32/raw/main/package_inex_stm32_index.json
2.1
2.2
2.3
Finish
(3) Go to Boards Manager icon, paste “inex” in the search bar and click INSTALL button.
3.2
3.1
3.3
Download and installation�the STM32 Cube Programmer
(1) Go to https://www.st.com/en/development-tools/stm32cubeprog.html and download the software.
Before downloading, you must register with the website first.
(2) Double-click the ZIP file. The new folder will open to display the extracted files.
(3) Double-click the .exe installation file to start the software installation.
1
2
3
4
5
7
6
8
9
10
11
12
(4) Next is USB driver installation.
Finish
13
14
How to upload code to POP-32i board
How to upload code
(1) Connect the USB cable to the top USB port of the SWD module on the POP-32i board to the computer.
USB-C cable
POP-32i Board
Connect to this port
(2) Apply the supply voltage �to the board and turn on the POWER switch.
(3) At the main window of the Arduino IDE program, select the menu
Tools > Board:xxx > INEX_STM32 > POP-32.
(4) Select the code uploading method by selecting the menu
Tools > Upload method:xxx > STM32CubeProgrammer (SWD).
(5) Type the code according to the picture to test the operation, then click the Upload button. When the program upload is completed, the program status window will display the compilation status.
Status Window
Testing Code
Upload Button
2
1
3
(6) The POP-32i board is running.
Competition Field
Competition Field
Stage 3
Ball Collecting
Stage 1
Maze Solving
Stage 2
Line Following
All three stages are in the same area and continuous.
Maze Solving
Stage 1
Maze solveing : Maze information
About the field’s walls or Maze are made of the 28cm x 20cm x 2cm plaswood sheets. The wall side must be holed with self-tapping screws to insert into the aluminum profile rails. The screw positions should be measured 5cm down from the top rod precisely in the middle of the plaswood sheet as shown in the picture above. Make four holes and tighten the self-tapping screws through. The leaving a gap is about 3mm.
Maze solveing : Maze information
Use the double-sided cloth tape to attach between the edge below the aluminum profile or plaswood with the floor as shown in the photo above.
Maze solveing : Maze information
Let the robot moves following the direction shown in the picture above.
See the actual field like this.
#include <POP32.h>
void setup() {
// put your setup code here, to run once:
}
void loop() {
// put your main code here, to run repeatedly:
oled.text(1,0,"RAW:%d ", analog(8));
oled.text(3,0,"DISTANCE:%d CM ", analog(8) * 100 / 4095);
oled.show();
}
Use an ultrasonic sensor to detect the wall by measuring and keeping the distance between its sensor and the wall and ensure the robot is almost at the center of the pathway during measuring to affect the next step. If the robot is far or near the wall, it has completely spin left or right. While the robot is moving, it may crash, slip, slide, or strike the wall.
Listing 1-1 : Wall Detection
Save as “SONARRead.ino”
#include <POP32.h>
void setup() {
// put your setup code here, to run once:
}
void loop() {
// put your main code here, to run repeatedly:
oled.text(2, 0, "L:%d ", analog(0));
oled.text(4, 0, "R:%d ", analog(1));
oled.show();
}
Listing 1-2 : Line Detection
Use a light reflection sensor to detect the black line. Keep the black line value and the white surface value.
Save as “IFRRead.ino”
#define SONAR_READ_CM (analog(5) * 100) / 4095
// Read analog raw data and convert it to centimeter.
#define SPD 45 // Determine speed value. (0 to 100)
#define kpsonar 7 // Determine threshold between ultrasonic sensor and wall.
#define kpspin_t 350 // Determine spin left or right time.
#define kpifr 1500 // Determine threshold of black line detection.
(1-1) Header determination
Save as “IRQMazeSolving.ino”
Next step: This below code is finalized. We have separated the sections to describe each part operation.
Description: We have defined some values as macros value.
void fdSONAR(int cm) {
// Use fd2 function
fd2(SPD - 2, SPD);
while (SONAR_READ_CM > cm);
ao();
sound(500, 300);
}
(1-2) fdSONAR function :
“Move forward, then stop temporarily when it detects the wall.”
Description: Use a determined independent left-speed motor and right-speed motor function to move precisely straightforward. In this example, we had tested the robot moving tilted right, so we had to decrease the left-speed motor to make the robot straightforward.
The robot continues moving when it is far from the wall until the robot is near the wall then stops temporarily.
STOP
void slMAZE(int sl_t) {
sl(SPD);delay(300);
ao();delay(300);
}
(1-3) Spin Time function
Description: Set a spinning left time and temporarily stop time.
void srMAZE(int sl_t) {
sr(SPD);delay(300);
ao();delay(300);
}
Description: Set a spinning right time and temporarily stop time.
void bkSW(int freq, int ao_t) {
while (1) {
if (!in(22) && !in(23)) {
bk(SPD);
sound(500, 300);
ao();
delay(300);
break;
} else if (in(22) && !in(23)) {
sl(SPD);
} else if (!in(22) && in(23)) {
sr(SPD);
} else {
bk(SPD);
}
}
}
(1-4) bkSW function :
Move backward, touch the wall and set it to be straight by 2-Switch”.
Straight backward
Left button is pressed on the wall, let it spin right.
Right button is pressed on the wall, let it spin left.
Straight backward
temporarily when the both switches were pressed then make sound.
void stopline(int ifr) {
fd2(SPD - 2, SPD);
while (analog(2) > ifr);
ao(); sound(500, 300);
}
(1-5) Stop on the line function
Description: It is the similar as code (1-2) but uses the light reflect sensor to detect the black lines instead of an ultrasonic sensor.
The robot will move continue when it is on the white surface. When it detects the black line or area, it stops.
STOP
void setup() {}
void loop() {
waitSW_OK_bmp();
fdSONAR(kpsonar);
srMAZE(kpspin_t);
bkSW();
fdSONAR(kpsonar);
slMAZE(kpspin_t);
bkSW();
fdSONAR(kpsonar);
slMAZE(kpspin_t);
bkSW();
(1-6) Main code
fdSONAR(kpsonar);
slMAZE(kpspin_t);
bkSW();
fdSONAR(kpsonar);
srMAZE(kpspin_t);
bkSW();
fdSONAR(kpsonar);
srMAZE(kpspin_t);
bkSW();
stopline(kpifr);
}
Maze Solving Demo Video
Source Code
Maze Solving Demonstration Video
Source code
Line Following
Stage 2
Basic coding for Line Following Mission - 1
ZX-03 sensor (Left)
ZX-03 sensor (Right)
Sensors reading
bottom side
The robot has two light reflection sensors; ZX-03 (left and right). These sensors detect whether they are over a black line or a white surface. When over the black line, the sensor gives a low value and over white or light-colored, it gives a high value.
Making a simple field for a line following robot mission
Materials :
Listing 2-1 : Sensor reading
#include <POP32.h>
void setup() {
// put your setup code here, to run once:
}
void loop() {
// put your main code here, to run repeatedly:
oled.text(2, 0, "L:%d ", analog(0));
oled.text(4, 0, "R:%d ", analog(1));
oled.show();
}
Results:
Black Line : Sensor values may range between 200 to 700.
White Surface : Sensor values may be greater than 2,000
Listing 2-2 : Determine the threshold value
Thershold Value (ref) = (sensor’s value of Black Line + sensor’s value of White Surface) /2
After collecting data, choose a threshold value that separates the black line and white surfaces. The threshold should be in the middle of the recorded ranges.
Example
Description: If the values of both sensors have similar reference values, the same reference value can be used.
Listing 2-3 : Movement function
void Motor(int L, int R, int T) {
motor(1, L); //set the left motor speed as 0 to 100%
motor(2, R); //set the right motor speed as 0 to 100%
delay(T);
}
Motor(50,50,100); //move Forward with 50% power for 100ms
Motor(-50,-50,100); //move Backward with 50% power for 100ms
Motor(50,0,100); //Turn Left with 50% power for 100ms
Motor(0,50,100); //Turn Right with 50% power for 100ms
Motor(50,-50,100); //Spin Left with 50% power for 100ms
Motor(-50,50,100); //Spin Right with 50% power for 100ms
Motor(0,0,1); //Stop moving
Basic coding for Line Following Mission - 2
Scenario 1 :
Both sensors detect the white surface. The robot moves forward.
Scenario 2 :
Left sensors detect a black line. Right sensor detects the white surface. The robot turns left.
Movement Decisions
Scenario 3 :
Right sensors detect a black line. Left sensor detects the white surface. The robot turns right.
#include <POP32.h>
int ref = 1500;
void setup() {
waitSW_OK();
}
void loop() {
if (analog(0) > ref && analog(1) > ref) {
Motor(40, 40, 1);
} else if (analog(0) < ref && analog(1) > ref) {
Motor(40, 0, 1);
} else if (analog(0) > ref && analog(1) < ref) {
Motor(0, 40, 1);
}
}
void Motor(int L, int R, int T) {
motor(1, L);
motor(2, R);
delay(T);
}
Description : Place the robot so that the sensor is between the black lines then press SW_OK. The robot will move following the black line.
Code 2-1 : Basic Line following Robot
void track() {
while (1) {
if (analog(0) > ref && analog(1) > ref) {
Motor(40, 40, 1);
} else if (analog(0) < ref && analog(1) > ref) {
Motor(40, 0, 1);
} else if (analog(0) > ref && analog(1) < ref) {
Motor(0, 40, 1);
} else if (analog(0) < ref && analog(1) < ref) {
Motor(20, 20, 350);
Motor(0, 0, 10);
break;
}
}
}
Listing 2-4 : Line following with intersection function
The robot moves along the black line. When the sensors detect an intersection of the black lines, the robot moves slightly past the intersection and stops moving.
#include <POP32.h>
int ref = 1500;
void setup() {
}
void loop() {
waitSW_OK();
fd(40);
delay(250);
track();
}
void track() {
while (1) {
if (analog(0) > ref && analog(1) > ref) {
Motor(40, 40, 1);
} else if (analog(0) < ref && analog(1) > ref) {
Motor(40, 0, 1);
} else if (analog(0) > ref && analog(1) < ref) {
Motor(0, 40, 1);
} else if (analog(0) < ref && analog(1) < ref) {
Motor(20, 20, 350);
Motor(0, 0, 10);
break;
}
}
}
void Motor(int L, int R, int T) {
motor(1, L);
motor(2, R);
delay(T);
}
Description : Place the robot so that the sensor is between the black lines. Press the SW_OK. The robot will move forward pass the intersection and along the line. When the sensors detect an intersection of the black lines. The robot moves pass the red intersection and stops.
stop
start
Code 2-2 : Line following robot with intersection
Line following robot with intersection demonstration
stop
start
Example code : IRQ-2-2-Line-following.ino�Download here : https://github.com/inexglobal/Intelligent-Rescue-Robot
void left() {
Motor(-50, 50, 200);
while (analog(0) > ref) {
Motor(-50, 50, 1);
}
while (analog(0) < ref) {
Motor(-50, 50, 1);
}
Motor(0, 0, 1);
}
Listing 2-5 : intersection-turn left function
The purpose of this function is to determine �the robot's movement along the line until the sensors detect an intersection. The robot then moves slightly past the intersection and turns left.
void right() {
Motor(50, -50, 200);
while (analog(1) > ref) {
Motor(50, -50, 1);
}
while (analog(1) < ref) {
Motor(50, -50, 1);
}
Motor(0, 0, 1);
}
Listing 2-6 : intersection-turn right function
The purpose of this function is to determine �the robot's movement along the line until the sensors detect an intersection. The robot then moves slightly past the intersection and turns right.
#include <POP32.h>
int ref = 1500;
void setup() {
// put your setup code here, to run once:
}
void loop() {
waitSW_OK();
track();
left();
track();
right();
track();
}
void track() {
while (1) {
if (analog(0) > ref && analog(1) > ref) {
Motor(40, 40, 1);
} else if (analog(0) < ref && analog(1) > ref) {
Motor(40, 0, 1);
} else if (analog(0) > ref && analog(1) < ref) {
Motor(0, 40, 1);
} else if (analog(0) < ref && analog(1) < ref) {
Motor(20, 20, 350);
Motor(0, 0, 10);
break;
}
}
}
void left() {
Motor(-50, 50, 200);
while (analog(0) > ref) {
Motor(-50, 50, 1);
}
while (analog(0) < ref) {
Motor(-50, 50, 1);
}
Motor(0, 0, 1);
}
void right() {
Motor(50, -50, 200);
while (analog(1) > ref) {
Motor(50, -50, 1);
}
while (analog(1) < ref) {
Motor(50, -50, 1);
}
Motor(0, 0, 1);
}
void Motor(int L, int R, int T) {
motor(1, L);
motor(2, R);
delay(T);
}
Code 2-3 : Line following robot with intersection (advance)
Result of Line following robot with intersection (advance)
stop
start
Ball Collection
Stage 3
Stage 3 : The field is flat with the white or light-colored background and the black grid line. The robot will start the mission by entering the red line. The robot must moves to collect and place all balls to the exiting green line.
Ball Collecting Mission - 1
The field has 4 of 4cm. balls with 4 colors (Red, Yellow, Blue and Green). The location of the balls are randomly to place at any point in the field.
PICK UP POSITIONS
The ball is placed on a platform about 7mm. height with 4 platforms each one of one color.
DROP OFF POSITIONS
The balls must be placed at the drop points with �four different heights : 20mm, 30mm, 40mm, and 50 mm.
Ball Collecting Mission - 2
The Gripper-X mechanism uses two servo motors to operate. One servo motor is mounted to the mechanism that grips the object and the other servo motor is mounted to the mechanism that lifts the object.
Controlling the Servo motor
The servo motor for gripping mechanism
The servo motor for lifting mechanism
Listing 3-1 : Gripping servo motor position readings
#include <POP32.h>
int x;
void setup() {
}
void loop() {
x = knob(180);
servo(1, x);
oled.text(1, 3, "%d ", x);
oled.show();
}
Description : To know the value of degrees that will cause the servo motor to open and grip mechanism. It uses the adjustable resistor; KNOB to adjust the value and then display the value on the OLED along with sending values to control the servo motor as well.
Connect the gripping servo motor to SERVO1�port.
Test the clamping servo motor of the Gripper-X
10
125
When the Listing 3-1 code is loaded, the OLED of POP-32i board will display a value which sent to control the servo motor position. Try adjusting the knob until the ends of the gripper touch each other. Observe the value on the OLED. Then adjust the knob again to spread the gripper out. Observe the value on the OLED again. Record both values.
Adjust the knob to open the gripper and record the value
Adjust the knob to make the gripper grips and record the position values.
Example of Servo1 value | |
Operation | Value |
Gripper is opened | 10 |
Gripper is gripped | 125 |
Record servo motor position values
Listing 3-2 : Lifting servo motor position reading
#include <POP32.h>
int x;
void setup() {
}
void loop() {
X = knob(180);
servo(2, x);
oled.text(1, 3, "%d ", x);
oled.show();
}
Connect the lifting servo motor to SERVO2 port.
Description : Changes the port to reads the lifting servo motor position values for lifting and save the results on the OLED screen.
Modify the code by changing to servo 2.
Test the lifting servo motor
Example of Servo2 value | |
Operation | Value |
Gripper is lowered | 130 |
Gripper is lifted | 10 |
After the Listing 3-2 is loaded, the OLED screen will display the value which sent to control the lifting servo motor position of the Gripper-X.
Adjust the knob so that the gripper is lowered to pick up �the ball at the pick-up point.
Adjust the knob so that �the gripper lifts the ball.
Drop off the ball testing
Example of Servo2 value | |
Height | Value |
20 mm. | 120 |
30 mm. | 110 |
40 mm. | 100 |
50 mm. | 90 |
The ball drop-off stands are different heights so the lifting servo motor’s (SERVO2) position value must be set for each height.
Adjust the knob so that the clamping servo motor is setting for correct position according to the height of the ball drop-off stand. It is not too high or too low and to prevent the ball bouncing out.
50 mm
40 mm
30 mm
20 mm
Adjust the knobs to find the most suitable value for each height of the ball drop-off stand
Code 3-1 : Code header determination �for setting the servo motors of Gripper-X
#include <POP32.h>
#define sv2_down 130 //Determine position of servo2 to dribble the ball.
#define sv2_up 10 //Determine position of servo2 to pick the ball.
#define sv2_down20 120 //Determine position of servo2 to drop the ball on the 20mm stand.
#define sv2_down30 110 //Determine position of servo2 to drop the ball on the 30mm stand.
#define sv2_down40 100 //Determine position of servo2 to drop the ball on the 40mm stand.
#define sv2_down50 90 //Determine position of servo2 to drop the ball on the 50mm stand.
#define sv1_collide 125 //Determine position of servo1 to clamp the ball.
#define sv1_spread 10 //Determine position of servo1 to drop the ball.
Description : We have defined some values as macros value.
Code 3-2 : servo_set function
void servo_set() {
//Use Servo function
servo(2, sv2_up);
delay(300);
servo(1, sv1_down);
delay(300);
}
Description : Enable servo2 port to lift the gripper mechanism with 0.3 seconds delay time and servo1 port to open the gripper with 0.3 seconds too.
This function is designed to set the starting position of the servo motor to facilitate the next function call and also helps to reduce the occurrence of vibration or other problems from the servo motor during the mission.
After the code is uploaded, the robot will raise gripper as shown in the figure.
Code 3-3 : pickup function
Control the servo motor to lift and place the ball
void pickup() {
// Use the for command to slow down the servo.
for (int i = sv2_up; i < sv2_down; i++) {
servo(2, i);
delay(10);
}
for (int i = sv1_down; i < sv1_collide ; i++) {
servo(1, i);
delay(10);
}
for (int i = sv2_down; i > sv2_up; i--) {
servo(2, i);
delay(10);
}
}
Description : Define a variable that will be used to increase or decrease position of the lifting servo motor by specifying the current position and the desired position. Then, call the servo motor using the values from the variable. In this example, the gripper will come down to grab the ball and lift the gripper mechanism up to continue moving.
Function for picking up the ball from the pick-up point
Code 3-4 : drop50 function
void drop50() {
for (int i = sv2_up; i < sv2_down50; i++) {
servo(2, i);
delay(10);
}
for (int i = sv1_collide; i > sv1_spread ; i--) {
servo(1, i);
delay(10);
}
for (int i = sv2_down50; i > sv2_up; i--) {
servo(2, i);
delay(10);
}
}
Description : Use the for statement and set the variables according to the function in the code 3-3 but change the servo motor position value to place the ball. In this example, the gripper will down to the appropriate position for placing the ball at 50mm, height then release the gripper. Next, lift the gripper up to continue the movement.
Function for placing the ball on the 50mm. Drop-off stand
Code 3-5 : drop40 function
void drop40() {
for (int i = sv2_up; i < sv2_down40; i++) {
servo(2, i);
delay(10);
}
for (int i = sv1_collide; i > sv1_spread ; i--) {
servo(1, i);
delay(10);
}
for (int i = sv2_down40; i > sv2_up; i--) {
servo(2, i);
delay(10);
}
}
Description: The principle and operation are the same as in Code 3-4, except that the servo motor’s shaft position at the placement point has been changed.
Function for placing the ball on the 40mm. Drop-off stand
Code 3-6 : drop30 function
void drop30() {
for (int i = sv2_up; i < sv2_down30; i++) {
servo(2, i);
delay(10);
}
for (int i = sv1_collide; i > sv1_spread ; i--) {
servo(1, i);
delay(10);
}
for (int i = sv2_down30; i > sv2_up; i--) {
servo(2, i);
delay(10);
}
}
Description: The principle and operation are the same as in Code 3-5, except that the servo motor’s shaft position at the placement point has been changed.
Function for placing the ball on the 30mm. Drop-off stand
Code 3-7 : drop20 function
void drop20() {
for (int i = sv2_up; i < sv2_down20; i++) {
servo(2, i);
delay(10);
}
for (int i = sv1_collide; i > sv1_spread ; i--) {
servo(1, i);
delay(10);
}
for (int i = sv2_down20; i > sv2_up; i--) {
servo(2, i);
delay(10);
}
}
Description: The principle and operation are the same as in Code 3-6, except that the servo motor’s shaft position at the placement point has been changed.
Function for placing the ball on the 20mm. Drop-off stand
Use ultrasonic sensor to detect the desired distance for the robot to stop.
PICK-UP POSITION
Code 3-7 : track_sonar function
Move along the line and check the distance
void track_sonar(int speed) {
while (SONAR_READ_CM > kpsonar) {
if (analog(0) > kpifr && analog(1) > kpifr) {
fd(speed);
} else if (analog(0) < kpifr && analog(1) > kpifr) {
sl(speed);
} else if (analog(0) > kpifr && analog(1) < kpifr) {
sr(speed);
} else if (analog(0) < kpifr && analog(1) < kpifr) {
ao();
break;
}
}
ao();
}
Description : Use an ultrasonic sensor to check �the distance between the robot and the ball pick-up station or the ball drop-off stand.
If the specified distance is reached, the robot will stop
Code 3-8 : yellowball function
Pick up and drop off the yellow ball ”
void yellowball() {
track(40);
right();
track_sonar(30);
fd(20);
delay(80);
ao();
pickup();
bk(30);
delay(250);
ao();
left();
track(40);
track(40);
track(40);
left();
track_sonar(30);
fd(20);
delay(180);
ao();
drop20();
bk(30);
delay(300);
ao();
right();
}
Description : Use various functions to collect and place balls.
Move forward or backward to optimize ball pickup and drop off.
Code 3-9 : blueball function
Pick up and drop off the blue ball
void blueball() {
track(30);
left();
track_sonar(30);
fd(20);
delay(100);
ao();
pickup();
bk(30);
delay(300);
ao();
left();
track(40);
track(40);
track(40);
left();
track_sonar(30);
fd(20);
delay(200);
ao();
drop40();
bk(30);
delay(300);
ao();
right();
}
Description : Use various functions to collect and place balls.
Code 3-10 : greenball function
Pick up and drop off the green ball
void greenball() {
track(30);
right();
track(40);
right();
track(40);
left();
track_sonar(30);
fd(20);
delay(100);
ao();
pickup();
bk(30);
delay(300);
ao();
left();
track(40);
left();
track(40);
left();
track(40);
track_sonar(30);
fd(20);
delay(200);
ao();
drop30();
bk(30);
delay(300);
ao();
right();
}
Description: Use various functions to collect and place balls.
Code 3-11 : redball function
Pick up and drop off the red ball
void redball() {
track(30);
left();
track_sonar(30);
fd(20);
delay(100);
ao();
pickup();
bk(30);
delay(300);
ao();
left();
track(30);
left();
track(40);
track(40);
track_sonar(30);
fd(20);
delay(200);
ao();
drop50();
bk(30);
delay(300);
ao();
right();
track(30);
right();
track(30);
}
Description: Use various functions to collect and place balls.
Code 3-12 : Main program
#include <POP32.h>
void setup() {
Setservo();
sw_ok_press();
yellowball();
blueball();
greenball();
redball();
}
void loop() {
}
Description: Use this function to collect and place all balls.
Download code here : https://github.com/inexglobal/Intelligent-Rescue-Robot
Stage selection
Stage 3
Ball Collecting
Stage 1
Maze Solving
Stage 2
Line Following
All three stages are in the same area and are continuous.
Stage 3
Ball Collecting
Stage 1
Maze Solving
Stage 2
Line Following
The steps are as follows
The steps are : Press the B button on the POP-32i board to start stage 1.
After working finished, it will automatically begin stage 2 until all stages are completed.
The A button is for starting stage 2
The OK button is for starting stage 3.
int Stage = 1;
void setup() {
while(1){
if(SW_B()){ // Press SW_B start stage 1
Stage=1;beep();break;
}
if(SW_A()){ // Press SW_A start stage 2
Stage=2;beep();break;
}
if(SW_OK()){ // Press SW_OK start stage 3
Stage=3;beep();break;
}
}
}
Stage selection code
Description :
void setup, it will wait for the button pressing to select which stage to start first. When any button is pressed, it will go to the next process in the void loop function immediately.
void loop() {
if(Stage==1){
/* -----Sequence Command--- */
Stage=2; // Set the next stage
}else if (Stage==2){
/* ----- Sequence Command--- */
Stage=3;// Set the next stage
}else if (Stage==3){
/* ----Sequence Command---- */
Stage=4;// Set the next stage
}else{
while(1); // END Stage
}
}
void loop, the variable "Stage" is used to determine the stage to start working.
Stage selection code
Code : SelectionStage.ino
Download code here : https://github.com/inexglobal/Intelligent-Rescue-Robot
SW_B go to stage 1
SW_A go to stage 2
SW_OK go to stage 3
Stage selection code