In the first article of this series, I have provided a general introduction to the functions, structures, and other aspects of robots as a whole. Starting from this article, I will provide a detailed introduction to each part of the robot, including my design ideas, ideas, and the final implementation process.
The files and codes involved in this article can be found in my code repository: https://github.com/softdream/robot\_projects/tree/master/bottom\_control\_board
In the development sequence from bottom to top, I will also explain in this order. So the first thing to talk about is the underlying control board. This is one of the core components of the entire robot. As the external devices of the robot are diverse, they need to be uniformly driven and managed. Therefore, I designed a circuit board specifically for this purpose. The circuit board adopts an ESP32 microcontroller as the controller, which realizes the control function of the motor, communication function with the upper controller, and data acquisition function of various sensors. This control board can package the collected sensor data and send it to the upper controller through the serial port for processing. At the same time, the upper controller will send control instructions to ESP32 through the serial port. ESP32 needs to parse the control instructions and control the motor speed to control the robot’s movement.
I will upload the schematic diagram, PCB file, and BOM table of the underlying control board to the GitHub repository. Interested users can directly retrieve and use them.
The program design of the underlying control board will be the core content of this article, and the following will provide a detailed introduction with several key hardware devices as examples.
1. DC motor control and encoder data acquisition
This robot uses two brushed DC deceleration motors, with a maximum speed of approximately 300RPM (revolutions per minute) under 12V power supply. At the same time, the tail of each DC motor is integrated with an AB phase encoder. The encoder outputs 234 pulse signals for each revolution of the motor. Friends who have played with smart cars know that the drive of a brushed DC motor is relatively simple. The motor drive chip I used on the bottom driver board is TB6612FN, which can use PWM signals to control the motor speed and GPIO level signals to control the direction of motor rotation. So, first define the functions of the pins as follows:
```
#define IN1 26
#define IN2 27
#define PWMA 14
#define IN3 25
#define IN4 4
#define PWMB 0
#define LA 13
#define LB 12
#define RA 2
#define RB 15
```
The ESP32 provides an interface for outputting PWM signals through pins, which can be used directly. There are mainly three interfaces:
```
ledcSetup(pwm_Channel_A, FREQ, resolution);
ledcAttachPin(PWMA, pwm_Channel_A);
ledcWrite(pwm_Channel_A, pwm_value);
```
You can control the speed of the motor using the three interfaces above, which is very convenient. However, the data reception of the encoder is slightly more complex. In order to be able to distinguish the direction of motor rotation while measuring the motor speed, there are generally two outputs on the encoder, one A-phase output and one B-phase output. There is a phase difference between the A-phase output signal and the B-phase output signal, which can be used to determine the direction of motor rotation. To enable the external interrupt function of the pin on ESP32, when the AB phase pulse arrives (triggered by the edge), the forward and reverse directions are determined by the pin’s voltage level, and then a counter is used to record the number of pulses.
```
void leftEncoder()
{
if( digitalRead(LA) && !digitalRead(LB) ){
l_count ++;
}
else if( !digitalRead(LA) && digitalRead(LB) ){
l_count ++;
}
else {
l_count --;
}
}
```
At the same time as obtaining the number of pulses, we can use the large M method to measure the speed of the motor. The so-called large M method is to count the number of pulses within a fixed time period, and we know the total number of pulses that the motor has in one revolution. Based on the proportion, we can calculate the number of revolutions the motor has in a unit time period.
To achieve this function, we first need to enable the timer on ESP32. Fortunately, the ESP32 library provides a Ticker class that can directly call the timer:
```
Ticker control_timer;
```
Set the timer timing and timer interrupt function during initialization:
```
control_timer.attach_ms( interrupt_time_control, motorControl );
```
Using the large M method for speed measurement in the timer interrupt function:
```
void motorControl()
{
float l_rpm = ( 60 * l_count / ( pulse_number * 0.05 ) );
float r_rpm = -60 * r_count / ( pulse_number * 0.05 );
…
}
```
2. Chassis kinematic model
After measuring the rotational speeds of two wheels through an encoder, we can calculate the increment of rotation angle per unit time and the unique increment per unit time of robot operation based on the kinematic model of the chassis, laying a foundation for subsequent odometer estimation. Here we use a two wheel differential motion model:
Assuming that the left wheel displacement of the robot is ls and the right wheel displacement is rs (both values are measured by the encoder), and the distance between the two wheels of the robot is base_width, then:
The displacement increment is: (ls+rs)/2;
The angle increment is: (rs - ls)/base_width;
Assuming that the left wheel speed of the robot is l-v and the right wheel speed is r-v, then:
Linear speed: (l-v+r-v)/2;
Angular velocity: (r-v - l-v)/base_width;
```
float getDeltaS( long l_count_c, long r_count_c )
{
float l_s = ((float)l_count_c / pulse_number) * circumference;
float r_s = -((float)r_count_c / pulse_number) * circumference;
return (l_s + r_s) / 2;
}
```
```
float getDeltaAngle( long l_count_c, long r_count_c )
{
float l_s = ((float)l_count_c / pulse_number) * circumference;
float r_s = -((float)r_count_c / pulse_number) * circumference;
return (r_s - l_s) / base_width;
}
```
At the same time, we can analyze the DC command sent by the upper controller based on the kinematic model, and interpret it as the rotational speed that the left and right wheels should have. The control command is (linear velocity, angular velocity).
```
void cacuRPM( float v, float w )
{
//convert m/s to m/min
float v_mins = v * 60;
//convert rad/s to rad/min
float w_mins = w * 60;
//Vt = ω * radius
float w_vel = w_mins * base_width;
float x_rpm = v_mins / circumference;
float tan_rpm = w_vel / circumference;
required_rpm_left = x_rpm - tan_rpm * 0.5;
required_rpm_right = x_rpm + tan_rpm * 0.5;
}
```
3. PID algorithm
We can calculate the rotational speed that the left and right wheels of the robot should have under given linear and angular velocities using the cacuRPM (float v, float w) function. In order to maintain the required speed of the wheels, we also need to use PID control algorithm. The PID control algorithm should be said to be one of the simplest but also the most effective control algorithms.
In this project, the implementation of PID control algorithm is mainly in the pid.cpp file, and its core code is as follows:
```
const float PID::caculate( const float& target, const float& current )
{
// error
float error = target - current;
// Proportional
float p_out = kp_ * error;
// Integral
integral_ += error;
if( target == 0 && error == 0 ){
integral_ = 0;
}
float i_out = ki_ * integral_;
// Derivative
float derivative = ( error - pre_error_ );
float d_out = kd_ * derivative;
// total output
float out = p_out + i_out + d_out;
// limit
if( out > max_ ){
out = max_;
}
else if( out < min_ ){
out = min_;
}
// update the pre_error
pre_error_ = error;
return out;
}
```
In each interrupt function of the timer, we use the PID algorithm to calculate the control variables of the two wheels separately, and then use this control variable as the PWM value of the motor.
4. IMU control
The IMU model we use here is MPU6050, which can measure three-axis acceleration information and three-axis angular velocity information. The MPU6050 uses the IIC protocol to communicate with the ESP32 microcontroller. During program design, the MPU6050_light library is required, which provides calibration and value reading interfaces for IMUs and can be used directly. In this project, simple encapsulation is required.
Firstly, the initialization of IMU:
```
void imuInit()
{
Wire.begin(IMU_SDA, IMU_SCL);
byte status = mpu.begin();
while(status != 0){ }
uart1.print(“calibration”);
delay(100);
mpu.calcOffsets(true, true);
delay(2000);
uart1.print(“done”);
}
```
Next, encapsulate an interface for reading data:
```
typedef struct ImuData_
{
float ax;
float ay;
float gz;
}ImuData;
ImuData getImuData()
{
ImuData imu;
mpu.update();
imu.ax = mpu.getAccX();
imu.ay = mpu.getAccY();
imu.gz = mpu.getGyroZ();
return imu;
}
```
5. Serial port
In this project, two serial ports on ESP32 were used, with serial port 0 mainly used for program download and debugging. Serial port 1 is used for communication with the upper controller. The initialization of serial port 1 is:
```
HardwareSerial uart1(1);
void uartInit( int baudrate)
{
uart1.begin( baudrate, SERIAL_8N1, UART1_RX, UART1_TX );
}
```
We need to send the measured data to the upper controller through serial port 1 where needed. For example, we need to send this data during a timer interrupt (timestamp, robot line speed, robot position increment, robot angle increment, z-axis angular speed, left wheel speed, right wheel speed). Therefore, we only need to write it as follows:
```
uart1.printf( “meas %ld %.4f %.4f %.4f %.4f %.4f %.4f\n”, millis(), velocity, delta_s, delta_angle, imu.gz, l_rpm, r_rpm );
```
Just send the entire message to be sent as a string.
6. Ultrasonic sensor
Ultrasonic sensors play an auxiliary role in obstacle avoidance. The code for driving the sensor under ESP32 is also relatively simple, and the process is as follows:
```
float getDistance()
{
digitalWrite( Trig, HIGH );
delayMicroseconds(10);
digitalWrite( Trig, LOW );
float dist = pulseIn( Echo, HIGH ) * 0.034 / 2;
return dist;
}
```
7. Main function analysis
```
void setup()
{
Serial.begin(115200);
uartInit(230400);
motorGpioInit();
delay(100);
imuInit();
delay(100);
encoderInit();
delay(100);
motorInit();
delay(100);
xTaskCreate(taskSensors,
“sensors”,
8*1024,
NULL,
1,
NULL);
}
```
In setup(), we mainly did some initialization work and opened a new task. Note that in ESP32, the freeRTOS operating system can be run, allowing for concurrent execution of multiple tasks, and xTaskCreat() is the interface function for creating tasks.
Next, in the loop() function, our main task is to receive non blocking instruction information sent by the upper controller:
```
void loop()
{
int len = uart1.available();
if( len > 0 ){
memset(recv_buff, 0, sizeof(recv_buff));
uart1.readBytes( recv_buff, len );
if( len == 8 ){
Control u;
memcpy( &u, recv_buff, sizeof(u) );
cacuRPM( u.v, u.w );
}
}
delay(100);
}
```
In addition, we have created a new task called “sensors” above. In this task, we will poll and read the values of each sensor. This code can be modified according to the actual situation:
```
void taskSensors(void *parameter)
{
ultraSonicInit();
delay(100);
while(1){
int humidity = 0;
int temperature = 0;
getHumidityAndTemperature( humidity, temperature );
float distance = getDistance();
float voltage = getBatteryVoltage();
uart1.printf( “sens %d %d %.2f %.2f\n”, humidity, temperature, distance, voltage );
vTaskDelay(100);
}
vTaskDelete(NULL);
}
```
The above is the program analysis of the entire underlying control board. In the next article, I will introduce how the upper controller receives and processes this data.