### 6.2.2 Onboard RGB LED
The three colors of the onboard RGB LED—red, green, and blue—can be individually controlled, allowing the LED to produce a variety of colorful display effects.
### 6.2.3 Program Download
[rgb_test](../_static/source_code/rgb_test.zip)
:::{Note}
- Please remove the Bluetooth module before downloading the program. If not removed, a serial interface conflict may occur, causing the download to fail.
- When connecting the Type-B download cable, make sure the battery box switch is set to **"OFF."** This helps prevent accidental contact between the download cable and the expansion board's power pin, which could result in a short circuit.
:::
(1) Locate and open "**rgb_test.ino**" program file in the same dictionary as this section.
(2) Connect the Arduino to the computer with the Type-B cable.
(3) Click the **"Select Board"**, and the software will automatically detect current Arduino serial interface. Then click to connect.
(4) Click 
to download the program to the Arduino, and then wait for the download to complete.
### 6.2.4 Program Outcome
After powering on the robot, the RGB LED on the Arduino expansion board will light up red. When the KEY1 button is pressed, the LED remains white; when the button is released, it returns to red.
### 6.2.5 Program Analysis
[rgb_test](../_static/source_code/rgb_test.zip)
* **Import Library File**
{lineno-start=11}
```
#include "FastLED.h"
```
Import the required RGB control library for the game.
* **Define Pin and Create Objects**
(1) Define the pins for the RGB LED and the button.
{lineno-start=13}
```
const static uint8_t ledPin = 2;
const static uint8_t keyPin = 3;
static CRGB rgbs[1];
```
(2) Create an RGB LED object and a variable to store the button status.
{lineno-start=16}
```
static CRGB rgbs[1];
bool keyState; ///< 按键状态检测(Detect button status)
```
* **Initial Settings**
(1) In the `setup()` function, initialize the necessary hardware components. Start by setting up the serial port with a baud rate of 9600 for communication.
{lineno-start=22}
```
void setup() {
Serial.begin(9600); // 初始化串口通信(Initialize serial communication)
```
(2) Configure the button pin as an input. By default, its voltage level will be low when the system is powered on. Then, initialize the RGB LED on the expansion board using the **FastLED** library. Connect the LED to the designated ledPin, and use the Rgb_Show(255, 255, 255) function to set its initial color to white.
{lineno-start=25}
```
pinMode(keyPin, INPUT);
FastLED.addLeds
### 6.3.2 Onboard Buzzer Overview
The onboard buzzer is a 5V passive buzzer, capable of producing different tones by varying the frequency of the output PWM signal. By programming specific frequencies and durations, we can make the buzzer play simple melodies.
### 6.3.3 Program Download
[buzzer_test](../_static/source_code/buzzer_test.zip)
:::{Notes}
- Be sure to **remove the Bluetooth module** before uploading the program. If it's connected, the upload will fail due to a conflict on the serial interface.
- **Turn the battery box switch to "OFF"** before connecting the Type-B USB cable. This helps prevent accidental short circuits if the cable touches the power pins on the expansion board.
:::
(1) Locate and open the buzzer_test program file (found in the same directory as this lesson).
(2) Connect your Arduino board to the computer using a Type-B USB cable.
(3) In the Arduino IDE, click "**Select Board**". The software will automatically detect the connected Arduino board and its serial port. Click to confirm the connection.
(4) Click 
to compile and upload the program to the board. Wait for the upload to complete successfully.
### 6.3.4 Program Outcome
Once the robot is powered on, **pressing the KEY1 button** on the expansion board will trigger the buzzer to play a predefined melody.
### 6.3.5 Code Analysis
[buzzer_test](../_static/source_code/buzzer_test.zip)
* **Import Required Libraries**
{lineno-start=11}
```
#include "tone.h"
```
Begin by importing the tone library used for controlling the buzzer.
* **Define Pins and Variables**
(1) Define the buzzer pin and button pin.
{lineno-start=14}
```
static int song[98] = {
NOTE_E4, NOTE_E4, NOTE_E4, NOTE_C4, NOTE_E4, NOTE_G4, NOTE_G3,
NOTE_C4, NOTE_G3, NOTE_E3, NOTE_A3, NOTE_B3, NOTE_AS3, NOTE_A3, NOTE_G3, NOTE_E4, NOTE_G4, NOTE_A4, NOTE_F4, NOTE_G4, NOTE_E4, NOTE_C4, NOTE_D4, NOTE_B3,
NOTE_C4, NOTE_G3, NOTE_E3, NOTE_A3, NOTE_B3, NOTE_AS3, NOTE_A3, NOTE_G3, NOTE_E4, NOTE_G4, NOTE_A4, NOTE_F4, NOTE_G4, NOTE_E4, NOTE_C4, NOTE_D4, NOTE_B3,
NOTE_G4, NOTE_FS4, NOTE_E4, NOTE_DS4, NOTE_E4, NOTE_GS3, NOTE_A3, NOTE_C4, NOTE_A3, NOTE_C4, NOTE_D4, NOTE_G4, NOTE_FS4, NOTE_E4, NOTE_DS4, NOTE_E4, NOTE_C5, NOTE_C5, NOTE_C5,
NOTE_G4, NOTE_FS4, NOTE_E4, NOTE_DS4, NOTE_E4, NOTE_GS3, NOTE_A3, NOTE_C4, NOTE_A3, NOTE_C4, NOTE_D4, NOTE_DS4, NOTE_D4, NOTE_C4,
NOTE_C4, NOTE_C4, NOTE_C4, NOTE_C4, NOTE_D4, NOTE_E4, NOTE_C4, NOTE_A3, NOTE_G3, NOTE_C4, NOTE_C4, NOTE_C4, NOTE_C4, NOTE_D4, NOTE_E4,
NOTE_C4, NOTE_C4, NOTE_C4, NOTE_C4, NOTE_D4, NOTE_E4, NOTE_C4, NOTE_A3, NOTE_G3
};
```
(2) Create variables:
keyState – stores the button's current state.
playMusic – controls whether the music should play.
{lineno-start=25}
```
static int noteDurations[98] = {
8,4,4,8,4,2,2,
3,3,3,4,4,8,4,8,8,8,4,8,4,3,8,8,3,
3,3,3,4,4,8,4,8,8,8,4,8,4,3,8,8,2,
8,8,8,4,4,8,8,4,8,8,3,8,8,8,4,4,4,8,2,
8,8,8,4,4,8,8,4,8,8,3,3,3,1,
8,4,4,8,4,8,4,8,2,8,4,4,8,4,1,
8,4,4,8,4,8,4,8,2
};
```
(3) Define the melody and rhythm:
The rhythm determines the duration of each note. In this case, a tempo of one beat per second is used.
{lineno-start=36}
```
const static uint8_t buzzerPin = 3; ///< 按键状态检测(detect button status)
const static uint8_t keyPin = A3;
bool keyState; ///< 按键状态检测(detect button status)
bool taskStart = 0;
```
* **Initial Settings**
Within the `setup()` function:
**(1) Initialize hardware:** Set up the serial port with a baud rate of 9600 and a timeout of 500ms for reading data.
{lineno-start=40}
```
void setup() {
pinMode(keyPin, INPUT);
Serial.begin(9600);
// 设置串行端口读取数据的超时时间(Set the timeout for serial port data reading)
Serial.setTimeout(500);
}
```
**(2) Configure pins:** Set the button pin as an input. By default, its value is low when powered on.
{lineno-start=41}
```
pinMode(keyPin, INPUT);
```
**(3) Initialize buzzer:** Call the `play_tune()` function to set up the tone and timing for the melody.
* **Main Loop**
In the `loop()` function, use the `analogRead()` function to read the value of the button pin. Based on the button's state, control the color of the RGB LED. If the value read is 0 (indicating a low voltage level, meaning the button is pressed), call the `tune_task()` function to trigger the buzzer to play music.
{lineno-start=47}
```
void loop() {
keyState = analogRead(keyPin); //检测按键状态(detect button status)
if (!keyState) taskStart = 1;
if (taskStart)
{
tune_task(); // 播放音乐(Play music)
taskStart = 0;
}
}
```
* **Music Playback Logic**
{lineno-start=58}
```
void tune_task(void) {
for (int thisNote = 0; thisNote <98; thisNote++)
{
int noteDuration = 1000/noteDurations[thisNote];// 计算每个节拍的时间，以一个节拍一秒为例，四分之一拍就是1000/4毫秒，八分之一拍就是1000/8毫秒(Calculate the time for each beat, assuming one beat per second, a quarter note is 1000/4 milliseconds, and an eighth note is 1000/8 milliseconds)
tone(buzzerPin, song[thisNote],noteDuration);
int pauseBetweenNotes = noteDuration * 1.10; //每个音符间的停顿间隔，以该音符的130%为佳(The pause interval between each note, 130% of the duration of this note is recommended)
delay(pauseBetweenNotes);
noTone(buzzerPin);
}
}
```
The program loops through each note and its corresponding rhythm, converting the rhythm into the appropriate play duration. This information is then sent to the buzzer to produce the sound.
To create a more realistic musical effect, a short pause is added after each note. By default, this pause is set to **1.3 times** the note's play duration.
### 6.3.6 FAQ
**Q: The program fails to upload. What should I do?**
**A:** Please check whether the Bluetooth module is still connected to the robot. If it is, **remove the Bluetooth module** before attempting to upload the program again.
## 6.4 Ultrasonic Ranging
In this lesson, you'll learn how to use a glowing ultrasonic sensor to detect the distance of nearby obstacles. You'll also control the color of the sensor's built-in RGB LED based on the measured distance.
### 6.4.1 Program Flowchart
### 6.4.2 Ultrasonic Sensor Overview
This lesson uses a glowing ultrasonic ranging module that communicates via the I2C interface. It reads distance values using ultrasonic pulses and displays visual feedback through an RGB LED.
* **Working Principle**
The ultrasonic sensor will automatically transmit 8 square waves at 40khz during ranging, and then detect whether there is a signal return. If there is a signal return, it outputs a high voltage level, whose continuing time is the time of the ultrasonic from transmitting to returning.
**Formula:**
Measurement distance = (High-level time × Sound speed (340 m/s)) / 2
* **Schematic Description**
The sensor is powered by a CS100 chip.
Pins TP and TN emit 8 square waves at 40 kHz.
Pins RP and RN receive the echo.
The chip calculates distance using the formula above.
**Specifications:**
**Supply Voltage:** 5V
**Operating Current:** 2mA
**Effective Ranging Distance:** 2 cm – 400 cm
### 6.4.3 Uploading the Program
:::{Note}
* Remove the Bluetooth module before uploading the program. Otherwise, the upload may fail due to serial port conflicts.
- **Turn off the battery box** before connecting the Type-B USB cable. This prevents accidental contact with the power pins, which could cause a short circuit.
:::
(1) Locate and open the ultrasonic_test program file (found in the same directory as this tutorial).
(2) Connect the Arduino board to your computer using a Type-B USB cable.
(3) Click **"Select Board"** in the IDE. The software will automatically detect the connected Arduino and its port. Click to confirm.
(3) Click 
to compile and upload the program. Wait for the upload to complete.
### 6.4.4 Program Outcome
When powered on, the ultrasonic module will measure the distance to an obstacle and change the RGB LED color accordingly.
Place an object directly in front of the ultrasonic sensor and move it slowly toward the sensor. The RGB LED behavior changes as follows:
**Distance \< 80 mm** → Red breathing light
**80 mm – 180 mm** → Red gradient (brighter as distance increases)
**180 mm – 320 mm** → Blue gradient (brighter as distance increases)
**320 mm – 500 mm** → Green gradient (brighter as distance increases)
**\> 500 mm** → Solid green light
### 6.4.5 Program Analysis
The source code is located in the ultrasonic_test.ino file. Refer to the flowchart above for an overview of the program logic.
* **Importing the Required Library**
{lineno-start=12}
```
#include "Ultrasound.h"
```
Start by importing the necessary library for controlling the glowing ultrasonic sensor.
* **Pin Definition and Object Creation**
Define the size of the filter array and a variable to store the filtered distance.
Create an object of the ultrasonic sensor class to fetch distance readings.
{lineno-start=14}
```
#define FILTER_N 3 ///< 滤波法数组容量(Filter array capacity)
int Filter_Value;
int filter_buf[FILTER_N + 1];
Ultrasound ultrasound; ///< 实例化超声波类(Instantiate the ultrasound class)
```
* **Initial Setup**
In the `setup()` function:
Initialize the serial port at a baud rate of 9600.
Set up necessary hardware configurations.
{lineno-start=24}
```
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
}
```
* **Main Function**
In the `loop()` function:
Continuously call the ultrasonic task function to perform real-time distance detection and update LED colors accordingly.
```
void loop() {
// put your main code here, to run repeatedly:
ultrasonic_distance();
}
```
* **Ultrasonic Detection Logic**
(1) The `ultrasonic_distance()` function measures the distance and changes the LED color.
{lineno-start=46}
```
int ultrasonic_distance(){
uint8_t s;
uint16_t distance = Filter();///< 获得滤波器输出值(Get the output value of the filter)
Serial.print("Distance: ");///< 获取并且串口打印距离，单位mm(Get and print the distance via serial port, unit: mm)
Serial.print(distance);
Serial.println(" mm");
```
(2) \< 80 mm → Red breathing mode, updates every 0.1 seconds.
{lineno-start=53}
```
if (distance > 0 && distance <= 80){
ultrasound.Breathing(1, 0, 0, 1, 0, 0);///< 呼吸灯模式，周期0.1s，颜色红色(Red breathing light mode, in 0.1s)
}
```
(3) 80–180 mm → Red gradient, color becomes lighter as distance increases.
{lineno-start=57}
```
else if (distance > 80 && distance <= 180){
s = map(distance,80,180,0,255);
ultrasound.Color((255-s), 0, 0, (255-s), 0, 0);///< 红色渐变(Red gradient)
}
```
(4) 180–320 mm → Blue gradient, color becomes bluer as distance increases.
{lineno-start=62}
```
else if (distance > 180 && distance <= 320){
s = map(distance,180,320,0,255);
ultrasound.Color(0, 0, s, 0, 0, s);///< 蓝色渐变(Blue gradient)
}
```
(5) 320–500 mm → Green gradient, color becomes greener as distance increases.
{lineno-start=67}
```
else if (distance > 320 && distance <= 500){
s = map(distance,320,500,0,255);
ultrasound.Color(0, s, 255-s, 0, s, 255-s);///< 绿色渐变(Green gradient)
}
```
(6) \> 500 mm → Solid green light.
{lineno-start=71}
```
else if (distance > 500){
ultrasound.Color(0, 255, 0, 0, 255, 0);///< 绿色(Green)
}
```
### 6.4.6 Function Extension
How to modify the color gradient of an RGB LED in the distance between 80 and 180mm from red to yellow?
Please refer to the following steps:
(1) Find mapping instructions in the program for controlling the RGB lights to change with the change of the distance. Set R in RGB elements to change as the change of "**s**" by "**map**" function. `map (distance, 80, 180, 0, 255)` maps the obstacle distance to the R element.
{lineno-start=59}
```
ultrasound.Color((255-s), 0, 0, (255-s), 0, 0);///< 红色渐变(Red gradient)
```
(2) Change the value of the G element to be the same as the value of the A element. Note that the range of color elements is 0 to 255. Next, download the program again. The closer the obstacle is, the more the proportion of red and the less proportion of green. The RGB light displays yellow.
{lineno-start=59}
```
ultrasound.Color((255-s), (255-s), 0, (255-s), (255-s), 0);
```
For more detailed information about the RGB color table, please access: 
### 6.5.2 Ultrasonic Sensor Overview
This project uses a glowing ultrasonic ranging module with an I2C interface. It reads distance measurements and provides visual feedback through its RGB LED.
* **Working Principle**
The ultrasonic sensor automatically emits 8 square waves at 40 kHz during each measurement cycle. It then detects whether a signal returns. If a return signal is detected, the sensor outputs a high voltage signal. The duration of this high signal represents the round-trip time of the ultrasonic wave.
**Formula:**
Measurement Distance = (High-Level Time × Speed of Sound (340 m/s)) ÷ 2
* **Schematic Diagram**
The sensor is driven by the CS100 chip. The TP and TN pins transmit 8 square waves at 40 kHz, while RP and RN receive the echo signal. The distance is then calculated using the formula above.
* **Specifications:**
Supply Voltage: 5V
Operating Current: 2mA
Effective Range: 20 mm – 4000 mm
### 6.5.3 Program Download
:::{Note}
- Remove the Bluetooth module before uploading the program to avoid serial port conflicts.
- Turn off the battery box before connecting the Type-B USB cable to avoid accidental short circuits.
:::
(1) Locate and open the ultrasonic_following program file (found in the same folder as this tutorial).
(2) Connect your Arduino board to your computer using a Type-B USB cable.
(3) In the Arduino IDE, click **"Select Board."** The IDE will automatically detect the connected Arduino and port.
(4) Click 
to compile and upload the code. Wait for the process to complete.
### 6.5.4 Program Outcome
Once powered on, the **miniAuto** robot will:
① Display different RGB light colors on the ultrasonic sensor based on the measured distance.
② Move forward, backward, or stop, depending on how far the object is from the sensor.
**Try it out:**
Place an object in front of the sensor and slowly move it closer. You'll see the following behaviors:
(1) RGB LED Behavior
Distance \< 80 mm → Red breathing light
80–180 mm → Red gradient (increasing brightness)
180–320 mm → Blue gradient (increasing brightness)
320–500 mm → Green gradient (increasing brightness)
500 mm → Solid green light
(2) The car control effect:
Distance \< 200 mm → Car moves backward
200–300 mm → Car stops
300–700 mm → Car moves forward
700 mm → Car stops again
### 6.5.5 Program Analysis
The code is located in ultrasonic_following.ino. Use the flowchart above to follow the program logic.
* **Import Library File**
{lineno-start=12}
```
#include "Ultrasound.h"
```
First, import the necessary library to control the glowing ultrasonic sensor.
* **Define Pin and Create Objects**
Define a variable pwm_min for minimum PWM mapping.
Create an ultrasonic sensor object to retrieve distance data.
Set up a digital motor pin array and a dis variable to store measured distance.
{lineno-start=19}
```
Ultrasound ultrasound; //实例化超声波类(Instantiate the ultrasound class)
const static uint8_t pwm_min = 50;
const static uint8_t motorpwmPin[4] = { 10, 9, 6, 11} ;
const static uint8_t motordirectionPin[4] = { 12, 8, 7, 13};
uint16_t dis;
```
* **Setup Function**
In `setup()`:
Initialize the serial port (baud rate: 9600).
Call `Motor_Init()` to initialize the motors.
{lineno-start=33}
```
void setup() {
Serial.begin(9600);
Motor_Init();
}
```
* **Main Function**
In the `loop()` function:
Continuously call the `ultrasonic_distance()` function to read the sensor value and update the RGB LED color.
Use the measured distance to control robot movement in real time.
{lineno-start=38}
```
void loop() {
ultrasonic_distance();
dis = ultrasonic_distance();
if(dis >=700) Velocity_Controller( 0, 0, 0, 0);
if(dis >= 300 && dis < 700) Velocity_Controller( 0,50, 0, 0);
if(dis >= 200 && dis < 300) Velocity_Controller( 0, 0, 0, 0);
if(dis < 200) Velocity_Controller( 180, 50, 0, 0);
}
```
* **Ultrasonic Detection**
Call the `ultrasonic_distance()` function within the main `loop()` function. This function continuously measures the distance and updates the RGB LED color on the ultrasonic sensor accordingly.
{lineno-start=65}
```
/* 超声波距离数据获取(Obtain ultrasonic distance data) */
uint16_t ultrasonic_distance(){
uint8_t s;
uint16_t distance = Filter(); // 获得滤波器输出值(Get the output value of the filter)
Serial.print("Distance: ");Serial.print(distance);Serial.println(" mm"); //获取并且串口打印距离，单位mm(Get and print the distance via serial port, unit: mm)
```
(1) Distance: 0–80 mm
The RGB LED enters **red breathing mode**, pulsing red light at a 0.1-second interval.
{lineno-start=71}
```
if (distance > 0 && distance <= 80){
ultrasound.Breathing(1, 0, 0, 1, 0, 0); //呼吸灯模式，周期0.1s，颜色红色(Red breathing light mode, in 0.1s)
}
```
(2) Distance: 80–180 mm
The LED displays a **red gradient**. The red color becomes progressively lighter as the distance increases.
{lineno-start=75}
```
else if (distance > 80 && distance <= 180){
s = map(distance,80,180,0,255);
ultrasound.Color((255-s), 0, 0, (255-s), 0, 0); //红色渐变(Red gradient)
}
```
(3) Distance: 180–320 mm
The LED displays a **blue gradient**, with the blue intensity increasing as the object moves farther.
{lineno-start=80}
```
else if (distance > 180 && distance <= 320){
s = map(distance,180,320,0,255);
ultrasound.Color(0, 0, s, 0, 0, s); //蓝色渐变(Blue gradient)
}
```
(4) Distance: 320–500 mm
The LED displays a **green gradient**, becoming brighter with increasing distance.
{lineno-start=85}
```
else if (distance > 320 && distance <= 500){
s = map(distance,320,500,0,255);
ultrasound.Color(0, s, 255-s, 0, s, 255-s); //绿色渐变(Green gradient)
}
```
(5) Distance: Greater than 500 mm
The LED remains in a **steady green mode**, indicating the object is far away.
{lineno-start=89}
```
else if (distance > 500){
ultrasound.Color(0, 255, 0, 0, 255, 0); //绿色(Green)
}
```
* **Velocity Control Function**
In the **velocity control function**, the speed and direction of each motor are calculated based on the **kinematics of the mecanum wheel system**.
(1) The angle parameter determines the car's movement direction. An angle of **0°** corresponds to the front of the car, and angles increase **counterclockwise**.
(2) The velocity parameter controls how fast the car moves.
(3) The rot parameter sets the car's rotational movement: **positive values** cause **counterclockwise rotation**, and **negative values** cause **clockwise rotation**.
(4) The drift parameter enables or disables the car's drifting mode.
{lineno-start=103}
```
/**
* @brief 速度控制函数(Speed control function)
* @param angle 用于控制小车的运动方向，小车以车头为0度方向，逆时针为正方向。("angle" controls the robot's motion direction, with the front of the robot as 0 degrees and counterclockwise as the positive direction)
* 取值为0~359(range from 0 to 359)
* @param velocity 用于控制小车速度，取值为0~100。("velocity" controls the robot's speed, with a value range of 0 to 100)
* @param rot 用于控制小车的自转速度，取值为-100~100，若大于0小车有一个逆("rot" controls the robot's self-rotation speed, with a value range of -100 to 100)
* 时针的自转速度，若小于0则有一个顺时针的自转速度。(If it is greater than 0, the robot has a counterclockwise self-rotation speed. If it is less than 0, the robot has a clockwise self-rotation speed)
* @param drift 用于决定小车是否开启漂移功能，取值为0或1，若为0则开启，反之关闭。("drift" determines whether the robot enables drift. Value range: 0 or 1. If it is 0, drift is enabled; otherwise, it is disabled)
* @retval None
*/
void Velocity_Controller(uint16_t angle, uint8_t velocity,int8_t rot,bool drift) {
int8_t velocity_0, velocity_1, velocity_2, velocity_3;
float speed = 1;
angle += 90;
float rad = angle * PI / 180;
if (rot == 0) speed = 1;///< 速度因子(Speed factor)
else speed = 0.5;
velocity /= sqrt(2);
if (drift) {
velocity_0 = (velocity * sin(rad) - velocity * cos(rad)) * speed;
velocity_1 = (velocity * sin(rad) + velocity * cos(rad)) * speed;
velocity_2 = (velocity * sin(rad) - velocity * cos(rad)) * speed - rot * speed * 2;
velocity_3 = (velocity * sin(rad) + velocity * cos(rad)) * speed + rot * speed * 2;
} else {
velocity_0 = (velocity * sin(rad) - velocity * cos(rad)) * speed + rot * speed;
velocity_1 = (velocity * sin(rad) + velocity * cos(rad)) * speed - rot * speed;
velocity_2 = (velocity * sin(rad) - velocity * cos(rad)) * speed - rot * speed;
velocity_3 = (velocity * sin(rad) + velocity * cos(rad)) * speed + rot * speed;
}
Motors_Set(velocity_0, velocity_1, velocity_2, velocity_3);
}
```
* **Motor Control Function**
The motor control function uses the speed values calculated in the velocity control function to drive each motor using **PWM (Pulse Width Modulation)**.
The motors array stores the calculated speed values for each motor.
The pwm_set array converts these speed values into corresponding PWM signals that are then sent to each motor to control their output.
{lineno-start=135}
```
/**
* @brief PWM与轮子转向设置函数(PWM and wheel turning setting function)
* @param Motor_x 作为PWM与电机转向的控制数值。根据麦克纳姆轮的运动学分析求得。("Motor_x" is the control value for PWM and motor rotating. Calculated based on the kinematics analysis of mecanum wheels)
* @retval None
*/
void Motors_Set(int8_t Motor_0, int8_t Motor_1, int8_t Motor_2, int8_t Motor_3) {
int8_t pwm_set[4];
int8_t motors[4] = { Motor_0, Motor_1, Motor_2, Motor_3};
bool direction[4] = { 1, 0, 0, 1};///< 前进 左1 右0(Forward; left 1; right 0)
for(uint8_t i; i < 4; ++i) {
if(motors[i] < 0) direction[i] = !direction[i];
else direction[i] = direction[i];
if(motors[i] == 0) pwm_set[i] = 0;
else pwm_set[i] = map(abs(motors[i]), 0, 100, pwm_min, 255);
digitalWrite(motordirectionPin[i], direction[i]);
analogWrite(motorpwmPin[i], pwm_set[i]);
}
}
```
### 6.5.6 Function Extension
To modify the color gradient of the RGB LED so that it transitions from red to yellow when the detected distance is between 80 mm and 180 mm, you'll need to adjust the RGB mapping logic in your program.
Please refer to the following steps:
(1) First, locate the section of code responsible for mapping the RGB values according to the distance. Use the map() function to modify the red (R) component based on the obstacle distance. For example, map(distance, 80, 180, 0, 255) will scale the red value proportionally within the specified range.
{lineno-start=77}
```
ultrasound.Color((255-s), 0, 0, (255-s), 0, 0); //红色渐变(Red gradient)
```
(2) Next, to achieve a yellow gradient, set the green (G) component equal to the red (R) value, while keeping the blue (B) component at zero. Since yellow is a mix of red and green, this will create a smooth transition from red to yellow as the green component increases with distance. Remember that RGB color values must remain within the range of 0 to 255. Once you've made these changes, upload the program again. As the object moves away from the sensor within this range, the light will gradually shift from solid red to bright yellow.
{lineno-start=77}
```
ultrasound.Color((255-s), (255-s), 0, (255-s), (255-s), 0); //红色渐变(Red gradient)
```
For reference on RGB values and color combinations, you can visit the RGB color table at: https://www.bchrt.com/tools/rgbcolor/.
### 6.5.7 FAQ
**Q1: After uploading the code, the ultrasonic sensor always reads a distance of 0. What should I do?**
**A:** Please ensure that the 4-pin cable is properly connected to the I2C interface. An incorrect or loose connection can cause the sensor to fail to return valid data.
**Q2: The distance readings from the ultrasonic sensor are sometimes accurate, but sometimes inconsistent. Why is that?**
**A:** For more reliable results, use a smooth and flat object as the target for distance measurement. Additionally, avoid keeping obstacles too close to the sensor for extended periods, as this may affect accuracy.
## 6.6 Obstacle Avoidance
In this section, you can learn to detect the distance of obstacle through glowing ultrasonic module. The robot car can be simultaneously controlled to turn right to avoid obstacle. In this section, you'll use a glowing ultrasonic sensor to detect the distance of an obstacle and control the car to move forward or backward accordingly.
### 6.6.1 Program Flowchart
### 6.6.2 Ultrasonic Sensor
In this lesson, we use a glowing ultrasonic distance sensor module that communicates via the I2C interface. The module retrieves distance measurements from the ultrasonic sensor through I2C communication.
During operation, the module automatically emits eight 40kHz square wave pulses and then listens for an echo. If an echo is detected, it outputs a high signal. The duration of this high-level signal corresponds to the time it takes for the ultrasonic wave to travel to the obstacle and return.
The distance is calculated using the following formula:
**Distance = (High-level duration × Speed of sound (340 m/s)) ÷ 2**
### 6.6.3 Program Download
[ultrasonic_avoid](../_static/source_code/ultrasonic_avoid.zip)
:::{Note}
- **Remove the Bluetooth module** before uploading the program. Otherwise, the upload may fail due to serial port conflicts.
- **Turn off the battery box** before connecting the Type-B USB cable. This prevents accidental contact with the power pins, which could cause a short circuit.
:::
(1) Locate and open the ultrasonic_avoid program file (found in the same directory as this tutorial).
(2) Connect the Arduino board to your computer using a Type-B USB cable.
(3) Click **"Select Board"** in the IDE. The software will automatically detect the connected Arduino and its port. Click to confirm.
(4) Click 
to compile and upload the program. Wait for the upload to complete.
### 6.6.4 Program Outcome
**When the robot car is powered on, it will adjust the color of the glowing ultrasonic module based on the distance to nearby obstacles and will control the car's movement accordingly.**
Place an obstacle directly in front of the ultrasonic sensor and slowly move it closer.
(1) Ultrasonic Sensor Color Effects:
Distance \< 80mm: The RGB light on the ultrasonic module enters red breathing mode.
80mm ≤ Distance \< 180mm: The RGB light on the ultrasonic module transitions through a red gradient, becoming lighter as the distance increases.
180mm ≤ Distance \< 320mm: The RGB light changes to a blue gradient, intensifying as the distance increases.
320mm ≤ Distance \< 500mm: The RGB light shifts to a green gradient, getting brighter as the distance increases.
Distance \> 500mm: The RGB light becomes a solid green.
(2) Car Movement Behavior:
Distance >=300mm: The car moves forward.
Distance \< 300mm: The car turns.
### 6.6.5 Program Analysis
[ultrasonic_avoid](../_static/source_code/ultrasonic_avoid.zip)
The program used for this project is called **"ultrasonic_avoid.ino"** Refer to the following flowchart to understand the implementation logic of the program.
* **Import Library File**
{lineno-start=12}
```
#include "Ultrasound.h"
```
Import the necessary library to control the glowing ultrasonic sensor used in the project.
* **Define Pin and Create Objects**
Define an ultrasonic class to retrieve distance data. Create a variable called **"pwm_min"** to store the minimum PWM mapping value. Additionally, define an array to specify the digital motor pins. A variable "**dis**" is also created to store the distance value obtained from the ultrasonic sensor.
{lineno-start=19}
```
Ultrasound ultrasound; ///< 实例化超声波类(Instantiate the ultrasound class)
const static uint8_t keyPin = 3;
const static uint8_t pwm_min = 50;
const static uint8_t motorpwmPin[4] = { 10, 9, 6, 11} ;
const static uint8_t motordirectionPin[4] = { 12, 8, 7, 13};
bool keyState; ///< 按键状态检测(Detect button status)
bool taskStart = 0;
uint16_t dis;
```
* **Initial Setup**
In the `setup()` function, initialize the hardware components. First, set up the serial port with a baud rate of 9600 for communication. Next, call the `Motor_Init()` function to initialize and bind the motors.
{lineno-start=37}
```
void setup() {
Serial.begin(9600);
Serial.setTimeout(500);
pinMode(keyPin, INPUT);
Motor_Init();
printf("2");
}
```
* **Main Function**
In the `loop()` function, when the button is pressed, the ultrasonic obstacle avoidance game begins. The ultrasonic task function is continuously called to retrieve distance data and adjust the color of the glowing ultrasonic sensor. The robot's behavior is then controlled based on the detected data.
{lineno-start=45}
```
void loop() {
keyState = analogRead(keyPin);
printf("111");
if(!keyState) taskStart = 1;
if(taskStart) {
ultrasonic_distance();
dis = ultrasonic_distance();
Velocity_Controller( 0, 100, 0, 0);
while(dis < 300) {
dis = ultrasonic_distance();
Velocity_Controller( 0, 0, -100, 0);
}
}
}
```
* **Ultrasonic Detection**
Call the `ultrasonic_distance()` function in the `loop()` function to measure the distance and change the RGB light color accordingly.
{lineno-start=85}
```
uint16_t ultrasonic_distance(){
uint8_t s;
uint16_t distance = Filter(); ///< 获得滤波器输出值(Get the output value of the filter)
Serial.print("Distance: ");Serial.print(distance);Serial.println(" mm"); ///< 获取并且串口打印距离，单位mm(Get and print the distance via serial port, unit: mm)
```
**(1) Distance: 0 to 80mm** – The ultrasonic sensor displays a red breathing light with a 0.1s transition speed.
{lineno-start=90}
```
if (distance > 0 && distance <= 80){
ultrasound.Breathing(1, 0, 0, 1, 0, 0); ///< 呼吸灯模式，周期0.1s，颜色红色(Red breathing light mode, in 0.1s)
}
```
**(2) Distance: 80 to 180mm** – The sensor light gradually transitions from red, getting lighter as the distance increases.
{lineno-start=94}
```
else if (distance > 80 && distance <= 180){
s = map(distance,80,180,0,255);
ultrasound.Color((255-s), 0, 0, (255-s), 0, 0); ///< 红色渐变(Red gradient)
}
```
**(3) Distance: 180 to 320mm** – The sensor light changes to a blue gradient, becoming bluer as the distance increases.
{lineno-start=99}
```
else if (distance > 180 && distance <= 320){
s = map(distance,180,320,0,255);
ultrasound.Color(0, 0, s, 0, 0, s); ///< 蓝色渐变(Blue gradient)
}
```
**(4) Distance: 320 to 500mm** – The sensor light transitions to a green gradient, intensifying as the distance increases.
{lineno-start=104}
```
else if (distance > 320 && distance <= 500){
s = map(distance,320,500,0,255);
ultrasound.Color(0, s, 255-s, 0, s, 255-s); ///< 绿色渐变(Green gradient)
}
```
**(5) Distance: \>500mm –** The sensor light turns steady green.
{lineno-start=108}
```
else if (distance > 500){
ultrasound.Color(0, 255, 0, 0, 255, 0); ///< 绿色(Green)
}
```
* **Velocity Control Function**
The velocity control function calculates the control values for each motor based on the kinematic analysis of the mecanum wheel. The "**angle**" parameter defines the direction of motion for the car (0 degrees points to the front, with counterclockwise as the positive direction). The "**velocity**" parameter controls the speed of the car. The "**rot**" parameter defines the rotation direction of the car: counterclockwise for positive values and clockwise for negative values. The "**drift**" parameter enables or disables the car's drift function.
{lineno-start=122}
```
/**
* @brief 速度控制函数(Speed control function)
* @param angle 用于控制小车的运动方向，小车以车头为0度方向，逆时针为正方向。("angle" controls the robot's motion direction, with the front of the robot as 0 degrees and counterclockwise as the positive direction)
* 取值为0~359(range from 0 to 359)
* @param velocity 用于控制小车速度，取值为0~100。("velocity" controls the robot's speed, with a value range of 0 to 100)
* @param rot 用于控制小车的自转速度，取值为-100~100，若大于0小车有一个逆("rot" controls the robot's self-rotation speed, with a value range of -100 to 100)
* 时针的自转速度，若小于0则有一个顺时针的自转速度。(If it is greater than 0, the robot has a counterclockwise self-rotation speed. If it is less than 0, the robot has a clockwise self-rotation speed)
* @param drift 用于决定小车是否开启漂移功能，取值为0或1，若为0则开启，反之关闭。("drift" determines whether the robot enables drift. Value range: 0 or 1. If it is 0, drift is enabled; otherwise, it is disabled)
* @retval None
*/
void Velocity_Controller(uint16_t angle, uint8_t velocity,int8_t rot,bool drift) {
int8_t velocity_0, velocity_1, velocity_2, velocity_3;
float speed = 1;
angle += 90;
float rad = angle * PI / 180;
if (rot == 0) speed = 1;///< 速度因子(Speed factor)
else speed = 0.5;
velocity /= sqrt(2);
if (drift) {
velocity_0 = (velocity * sin(rad) - velocity * cos(rad)) * speed;
velocity_1 = (velocity * sin(rad) + velocity * cos(rad)) * speed;
velocity_2 = (velocity * sin(rad) - velocity * cos(rad)) * speed - rot * speed * 2;
velocity_3 = (velocity * sin(rad) + velocity * cos(rad)) * speed + rot * speed * 2;
} else {
velocity_0 = (velocity * sin(rad) - velocity * cos(rad)) * speed + rot * speed;
velocity_1 = (velocity * sin(rad) + velocity * cos(rad)) * speed - rot * speed;
velocity_2 = (velocity * sin(rad) - velocity * cos(rad)) * speed - rot * speed;
velocity_3 = (velocity * sin(rad) + velocity * cos(rad)) * speed + rot * speed;
}
Motors_Set(velocity_0, velocity_1, velocity_2, velocity_3);
}
```
* **Motor Control Function**
The motor control function uses the values calculated in the velocity control function to control each motor using PWM. The **"motors"** array maps the speed values for each motor to PWM values, while the **"pwm_set"** array controls the PWM signals sent to each motor.
{lineno-start=154}
```
/**
* @brief PWM与轮子转向设置函数(PWM and wheel turning setting function)
* @param Motor_x 作为PWM与电机转向的控制数值。根据麦克纳姆轮的运动学分析求得。("Motor_x" is the control value for PWM and motor rotating. Calculated based on the kinematics analysis of mecanum wheels)
* @retval None
*/
void Motors_Set(int8_t Motor_0, int8_t Motor_1, int8_t Motor_2, int8_t Motor_3) {
int8_t pwm_set[4];
int8_t motors[4] = { Motor_0, Motor_1, Motor_2, Motor_3};
bool direction[4] = { 1, 0, 0, 1};///< 前进 左1 右0(Forward; left 1; right 0)
for(uint8_t i; i < 4; ++i) {
if(motors[i] < 0) direction[i] = !direction[i];
else direction[i] = direction[i];
if(motors[i] == 0) pwm_set[i] = 0;
else pwm_set[i] = map(abs(motors[i]), 0, 100, pwm_min, 255);
digitalWrite(motordirectionPin[i], direction[i]);
analogWrite(motorpwmPin[i], pwm_set[i]);
}
}
```
### 6.6.6 Function Extension
**How to modify the robot's turning direction from right to left when encountering an obstacle?**
Follow these steps to modify the robot's turning direction:
(1) Locate the section of the code that controls the robot's turning behavior. In the `Velocity_Controller()` function, the robot turns to the right (clockwise) at a speed of 100. This is controlled by the value of the turning parameter: when the value is less than 0, the robot turns clockwise; when the value is greater than 0, it turns counterclockwise.
{lineno-start=55}
```
Velocity_Controller( 0, 0, -100, 0);
```
(2) To change the turning direction to left (counterclockwise), modify the turning parameter. Replace the value -100 with 100. This will cause the robot to turn left at a speed of 100.
{lineno-start=45}
```
void loop() {
keyState = analogRead(keyPin);
printf("111");
if(!keyState) taskStart = 1;
if(taskStart) {
ultrasonic_distance();
dis = ultrasonic_distance();
Velocity_Controller( 0, 100, 0, 0);
while(dis < 300) {
dis = ultrasonic_distance();
Velocity_Controller( 0, 0, -100, 0);
}
}
}
```
### 6.6.7 FAQ
**Q1: Since the code was uploaded, the distance measured by the ultrasound is always 0.**
**A:** Please check if the 4-pin cable is properly connected to the I2C interface. A loose or incorrect connection could result in no data being received, leading to a reading of 0.
**Q2: The distance measured by the ultrasonic sensor is sometimes accurate, and sometimes inaccurate.**
**A:** Ensure that you are using a smooth, flat object for distance measurement. Avoid placing the sensor too close to the obstacle for prolonged periods, as this could affect the accuracy. Also, ensure the sensor is not obstructed and is aligned properly for consistent readings.
## 6.7 Line Following
In this section, we will use the 4-channel line follower sensor to detect a black line and control the miniAuto to follow the line.
### 6.7.1 Program Flowchart
### 6.7.2 4-ch Line Follower
In this lesson, we will use a 4-channel line follower that utilizes an I2C communication interface to read data from the sensor probes.
The sensor module consists of four probes, each equipped with an infrared emitter and an infrared receiver. The infrared light is strongly reflected by white surfaces and weakly reflected by black surfaces. This variation in reflection allows the sensor to detect whether the line is present.
### 6.7.3 Program Download
[tracking_test](../_static/source_code/tracking_test.zip)
:::{Note}
- **Remove the Bluetooth module** before uploading the program. Otherwise, the upload may fail due to serial port conflicts.
- **Turn off the battery box** before connecting the Type-B USB cable. This prevents accidental contact with the power pins, which could cause a short circuit.
:::
(1) Locate and open the **tracking_test** program file.
(2) Connect Arduino to the computer with the Type-B cable.
(3) Click **"Select Board"** in the IDE. The software will automatically detect the connected Arduino and its port. Click to confirm.
(4) Click 
to compile and upload the program. Wait for the upload to complete.
### 6.7.4 Program Outcome
Place the car on the black line and turn on the power. Once the onboard button is pressed, the car will enter line-following mode. It will follow the black line when detected, and move backward if the line is lost—continuing in reverse until the line is detected again.
The onboard button
:::{Note} Be sure to position the car on the black line before starting the program; otherwise, it will keep moving backward in search of the line. :::
### 6.7.5 Program Analysis
[tracking_test](../_static/source_code/tracking_test.zip)
This section provides an overview of how the line-following program is structured and functions.
* **Import Library File**
{lineno-start=12}
```
#include 
### 6.7.7 FAQ
Q1: After uploading the code, the car keeps moving backward.
A: Please ensure the car is placed on the black line before starting the program. Otherwise, it will not detect the line and will continue reversing.
Q2: The 4-channel line follower's line detection is inaccurate.
A: Try adjusting the potentiometer on the 4-channel line follower to fine-tune the detection sensitivity. Rotate the knob until the sensor accurately detects the black line.
## 6.8 Pedestrian Detection
In this section, the 4-channel line follower is used to detect black lines, enabling the robot car to follow the path. If the ultrasonic sensor detects a pedestrian on the line during operation, the car will automatically take evasive action to avoid the obstacle.
### 6.8.1 Program Flowchart
### 6.8.2 Module Introduction
* **4-Channel Line Follower**
The 4-channel line follower uses an I2C communication interface, allowing the module to read data from its probes via I2C.
It features four probes, each consisting of an infrared emitter and an infrared receiver. White surfaces reflect infrared light strongly, while black surfaces reflect it weakly. This difference enables the sensor to determine whether a black line has been detected.
* **Ultrasonic Sensor**
This module is a glowing ultrasonic ranging sensor that also uses an I2C communication interface to transmit the measured distance data. It integrates two RGB LEDs positioned near the ultrasonic probes. These LEDs can adjust brightness and display various colors by blending red (R), green (G), and blue (B) light channels.
During distance measurement, the sensor automatically emits eight 40kHz square wave pulses and then checks for any returning signals. If a signal is received, it outputs a high voltage level. The duration of this high level corresponds to the time it takes for the ultrasonic wave to travel to the object and back.
### 6.8.3 Program Download
[tracking_avoid](../_static/source_code/tracking_avoid.zip)
:::{Note}
- **Remove the Bluetooth module** before uploading the program. Otherwise, the upload may fail due to serial port conflicts.
- **Turn off the battery box** before connecting the Type-B USB cable. This prevents accidental contact with the power pins, which could cause a short circuit.
:::
(1) Locate and open the tracking_avoid program file (found in the same directory as this tutorial).
(2) Connect Arduino to the computer with the Type-B cable.
(3) Click **"Select Board"** in the IDE. The software will automatically detect the connected Arduino and its port. Click to confirm.
(4) Click 
to compile and upload the program. Wait for the upload to complete.
### 6.8.4 Program Outcome
Place the car on the black line first. Once the button is pressed, the car will activate the line-following program. When the car detects the black line, it will follow it. If the black line is lost, the car will move backward until it detects the line again. During the line-following process, if the ultrasonic sensor detects a person in front, the car will stop.
:::{Note}
Ensure that the car is placed on the black line before starting the program, as the car will otherwise continue moving backward.
:::
### 6.8.5 Program Analysis
[tracking_avoid](../_static/source_code/tracking_avoid.zip)
* **Import Library File**
{lineno-start=12}
```
#include 
### 6.9.2 Module Introduction
In this section, we will use the 4-channel line follower, which features an I2C communication interface for reading data from the sensor probes.
The module includes four probes, each consisting of an infrared emitter and an infrared receiver. White surfaces strongly reflect infrared light, while black surfaces reflect it weakly. This difference in reflection allows the sensor to determine whether a line is detected.
### 6.9.3 Program Download
[tracking_crossroads](../_static/source_code/tracking_crossroads.zip)
:::{Note}
- **Remove the Bluetooth module** before uploading the program. Otherwise, the upload may fail due to serial port conflicts.
- **Turn off the battery box** before connecting the Type-B USB cable. This prevents accidental contact with the power pins, which could cause a short circuit.
:::
(1) Locate and open the tracking_crossroads program file (found in the same directory as this tutorial).
(2) Connect the Arduino board to your computer using a Type-B USB cable.
(3) Click **"Select Board"** in the IDE. The software will automatically detect the connected Arduino and its port. Click to confirm.
(4) Click 
to compile and upload the program. Wait for the upload to complete.
### 6.9.4 Program Outcome
Place the car on the black line and power it on. Once the onboard button is pressed, the car will enter the line-following mode. It will follow the black line when detected. If the line is lost, the car will move backward until it detects the black line again.
The onboard button
:::{Note} Ensure the car is placed on the black line before starting the line-following program; otherwise, the car will keep moving backward. :::
### 6.9.5 Program Analysis
[tracking_crossroads](../_static/source_code/tracking_crossroads.zip)
* **Import Library File**
{lineno-start=12}
```
#include 
### 6.10.2 Program Download
[app_control](../_static/source_code/10_app_control.zip)
:::{Note}
- **Remove the Bluetooth module** before uploading the program. Otherwise, the upload may fail due to serial port conflicts.
- **Turn off the battery box** before connecting the Type-B USB cable. This prevents accidental contact with the power pins, which could cause a short circuit.
:::
(1) Locate and open the [app_control/app_control.ino](../_static/source_code/10_app_control.zip) program file (found in the same directory as this tutorial).
(2) Connect the Arduino board to your computer using a Type-B USB cable.
(3) Click **"Select Board"** in the IDE. The software will automatically detect the connected Arduino and its port. Click to confirm.
(4) Click 
to compile and upload the program. Wait for the upload to complete.
### 6.10.3 Program Outcome
Please access "[**4. App Contro**l](https://docs.hiwonder.com/projects/miniAuto/en/latest/docs/4.App_control.html)" to view the program outcome.
### 6.10.4 Brief Program Analysis
[app_control](../_static/source_code/10_app_control.zip)
* **Import Library File**
Import the required library files of RGB LED, servo control and ultrasonic sensor for the game.
{lineno-start=13}
```
#include