CrowBot firmware modifications for voice and tilt control

How the control works

The device is based on a DevBoard on which the developer's software is installed. The board is connected to the controlled device via Bluetooth and transmits recognised voice commands and its tilt angle. The user holds this "remote control" in their hand and uses tilt and voice commands to control the robot or other devices that he adapts to work with the board, which I hope this article will help.

There are two microcontrollers on the board, ESP32-S3 and ESP32-C3.

S3 is already flashed, and the robot control program starts by default when the device is turned on. There is no access to the source code and no way to cross-flash S3.

Interaction and the ability to change anything on the controller itself are only available on the C3 part. The firmware source code for it is open source and can be found here (for those who want to understand more deeply or modify the interaction mechanism).

The first method: the board works in voice command recognition mode for selecting fixed commands.

• ROBOT WAKE UP
• ROBOT SLEEP
• MANUAL CONTROL
• VOICE CONTROL
• GO RIGHT
• GO LEFT
• GO FORWARD
• GO BACK
• GO HOME
• SLOWER SPEED
• FASTER SPEED
• LIGHTS ON
• LIGHTS OFF
• PLAY MUSIC

The second control method is to control the tilt change of the board in the plane. In this case, real-time values of the tilt angles in the X and Y axes are sent to the controlled device, which can be converted into motion control commands.

To switch to the manual control mode, say the MANUAL CONTROL command and the board screen will change.

While voice commands are also still recognized and sent, you can continue to process them on the controlled device.

Ready implementation for CrowBOT BOLT

The easiest way to get started is to buy the control module and Elecrow's CrowBOT. The DevBoard developer has added factory firmware for it, including blocks to interact with the development board.

As I wrote above, the developer has already realized the possibility of interaction with the board “out-of-the-box", but for a specific device - CrowBOT Bolt from Elecrow. This is a training robot that can be programmed in Arduino IDE, Letscode and MicroPython.

The factory firmware for the robot from the manufacturer is available on the Elecrow wiki, and it already implements the IR remote control that comes with the robot.

We are interested in comparing the robot's original firmware from the manufacturer with the new firmware from the controller board developer to understand the code and use it as a basis for developing a program for any other device in our projects.

The new firmware can be found here: https://github.com/Grovety/CrowBot_GRC_program

Main firmware changes

• Connecting to the DevBoard via Bluetooth

In the original firmware of the robot there is a possibility to work with Bluetooth, because the robot can be equipped with a Bluetooth joystick for control. But we are interested in how the connection to our board is implemented.

Response to voice commands

Handling of events coming from the board and related to voice control

• DevBoard tilt control of the robotThe same applies to tilt control of the board.
• Audible signal when driving backwards

The developer added useful little things to make the firmware more interesting. For example, the robot starts to emit a warning sound at every reverse movement like many cars today

• Backlighting at turns (turn signals)

Also added is backlighting at turns, which makes using CrowBOT more colorful and exciting.

Let's take a closer look at the changes

At the very beginning of the program header files of libraries that allow to organize multitasking are linked:

13 │ #include <freertos/FreeRTOS.h>
14 │ #include <freertos/message_buffer.h>
15 │ #include <freertos/projdefs.h>

These libraries provide for creating and managing tasks, as well as messaging between them. In the future, this will help in playing audio and processing commands from the DevBoard.

Next, the Bluetooth UUID of the DevBoard is entered:

29 │ static BLEUUID serviceUUID("FFE0"); //Host service UUID
30 │ static BLEUUIDcharUUID("FFE1");

This specifies the device to which our robot will connect.

83 │ SemaphoreHandle_t xPlayMusicSemaphore;
84 │ SemaphoreHandle_t xDisconnectedSemaphore;
85 │ MessageBufferHandle_t xGrcCmdBuffer;

Declaring semaphores and a message buffer to control the execution of tasks and synchronization between them.

What is a semaphore?

It is easier to think of a semaphore as a plate with one candy. Tasks can check for the candy, try to take it, and return it after a while. It turns out to be just a bool, but it works a bit more complicated due to multithreading and related issues. It's also worth clarifying that besides binary semaphores (when there is one candy), there are non-binary semaphores (multiple candies).

In our case, we will use semaphores to communicate between tasks:

125 │     if (xSemaphoreTake(xPlayMusicSemaphore, portMAX_DELAY) == pdTRUE)

For example, this line means "Wait as long as possible for a candy to appear on the plate, and as soon as it appears, take it immediately".

258 │         xSemaphoreGive(xPlayMusicSemaphore);

And that line means: “Put the candy back where it belongs”.

87 │ #define MAX_GRC_MSG_LEN 20

Define the maximum length of a message from DevBoard.

89 │ #define MOTOR_MIN_VALUE 30
90 │ #define MOTOR_MAX_VALUE 255

The interval of absolute values supplied to the motors. Without this limit, the robot's motors just hum but don't work. Adding a minimum speed helps to eliminate noise at low power.

92 │ template<classT>
93 │ constexpr const T& clamp(const T&v, const T&lo, const T&hi)
94 │ {
95 │     returnstd::less<T>{}(v, lo)? lo : std::less<T>{}(hi,v) ? hi : v;
96 │ }

The clamp function allows you to limit the range of change of some value by setting its lower and upper limits.

98 │ void poly_transform(const float params[12], float x, float y, float *_x, float *_y) {
99 │   *_x = params[0] + params[1] * x +params[2] *y +
100 │               params[3] * powf(x, 2) +params[4] *x * y +
101 │               params[5] * powf(y, 2);
102 │   *_y =params[6 + 0] +params[6 + 1] *x + params[6 + 2] *y +
103 │               params[6 + 3] * powf(x, 2) +params[6 + 4] *x * y +
104 │               params[6 + 5] * powf(y, 2);
105 │ }

This function performs a polynomial coordinate transformation. This is used to convert the tilt angles received from the DevBoard into the powers supplied to the motors. Note that this transformation is non-linear. As mentioned before, the working power interval for the motors is [-255... 255], and the received angle is in the interval [-45... 45].

If we take 3 three-dimensional points for each motor (roll, pitch and expected power value) and interpolate, we can get a formula for this transformation. For example, for the left motor we take the points:

(45, 45, 256) - DevBoard is turned forward and to the right, so we should turn the left motor at maximum power to turn to the right.

(-22.5, 45, 128) - DevBoard is facing forward and slightly left, meaning we should move forward but also turn left, so expect half motor power.

(0, 0, 0, 0) - no tilt - no movement.

If we perform a bilinear interpolation, we obtain the formula for the left motor: l = 1.3274074074074107x+3.792592592592592593y+0.012641975308641975xy

The values are similar for the right motor.

107 │ void led_callback() //Callback function
   108 │ {
   109 │   FastLED.show();
   110 │ }
112 │ static bool moving_backwards = false;
113 │ voidbuzzer_callback() //Callback function
114 │ {
115 │   if (moving_backwards) {
116 │     ledcWriteTone(2,G4); //Buzzer
117 │     delay(150);
118 │     ledcWrite(2, 0);
119 │   }
120 │ }

The led_callback and buzzer_callback procedures are used to update the state of the LEDs and control the buzzer respectively. They will be called at certain intervals.

122 │ void play_music_task(void*arg)
   123 │ {
   124 │   for (;;) {
   125 │     if (xSemaphoreTake(xPlayMusicSemaphore, portMAX_DELAY) ==pdTRUE)
   126 │     {
   127 │       if (!moving_backwards) {
   128 │         for (int i = 0; i < Tone_length;i++) { //Buzzer
   129 │           ledcWriteTone(2, Tone[i]); //Buzzer
   130 │           delay(Time[i] * 1000);
   131 │         }
   132 │         ledcWrite(2, 0); //trun off Buzzer
   133 │       }
   134 │     }
   135 │   }
   136 │ }

A task that constantly tries to capture a semaphore and, if it succeeds, plays music. The Tone array contains the frequencies to be played, and the Time array contains the time at which a particular note is played. The semaphore is released in the main task, allowing the PLAY MUSIC voice command to be executed asynchronously.

And here is the main task itself:

138 │ void grc_cmd_task(void*arg)
   139 │ {
   140 │   const floatd_speed = 0.5;

Changing the velocity factor (for FASTER SPEED and SLOWER SPEED voice commands)

141 │   float speed = 1;

Velocity factor itself (for GO FORWARD and GO BACK commands)

142 │   bool imu_control_mode =false;

Whether we are in tilt control mode:

143 │   auto init_imu_control_state = [&imu_control_mode]() {
   144 │     Serial.printf("Enable imu control mode\n");
   145 │     imu_control_mode = true;
   146 │     ticker.attach_ms(300, led_callback); //lighting task
   147 │     ticker1.attach_ms(900, buzzer_callback); //buzzer task
   148 │   };

When we switch to tilt control mode, we start periodically calling led_callback and buzzer_callback.

149 │   auto release_imu_control_state = [&imu_control_mode, &moving_backwards]() {
   150 │     Serial.printf("Disable imu control mode\n");
   151 │     imu_control_mode = false;
   152 │     moving_backwards = false;
   153 │     Motor(0, 0, 0, 0);
   154 │     ledcWrite(2, 0);
   155 │     fill_solid(leds, 4, CRGB::Black);
   156 │     FastLED.show();
   157 │     ticker.detach();
   158 │     ticker1.detach();
   159 │   };

When switch to voice control mode, we turn off motors and LED and stop periodic call of led_callback and buzzer_callback:

160 │   auto front_lights_cmd = [](bool front_lights_en) {
   161 │     uint8_t val = 0;
   162 │     if (front_lights_en) {
   163 │       FastLED.setBrightness(255); //RGB lamp brightness range: 0-255
   164 │       val = 255;
   165 │     }
   166 │     myRGBcolor6.r = val;
   167 │     myRGBcolor6.g = val;
   168 │     myRGBcolor6.b = val;
   169 │     fill_solid(RGBleds, 6, myRGBcolor6);
   170 │     FastLED.show();
   171 │   };

Turn on (front_lights_en == true) or turn off (front_lights_en == false) the “headlights”

173 │   char msg[MAX_GRC_MSG_LEN];

Message buffer

174 │   for (;;) {
   175 │     size_t length = xMessageBufferReceive(xGrcCmdBuffer, &msg,MAX_GRC_MSG_LEN,portMAX_DELAY);
176 │     std::string value(msg, length);

Expect a message from the buffer. Once received, translate it into a string.

177 │     conststd::string::size_type coords_offset = value.find("XY");
   178 │     const bool recv_imu_coords = coords_offset != std::string::npos;
   179 │     if (recv_imu_coords && !imu_control_mode) {
   180 │       init_imu_control_state();
   181 │     }

The presence of XY in the message signals the receipt of data from the tilt sensor. If we are not in tilt control mode and we have received XY, switch to it.

183 │     //********************************GRC voice command******************************************
   184 │     if (!imu_control_mode) {
   185 │       Serial.printf("Its characteristic value is: %s\n", value.c_str());
   186 │       if (value == "GO FORWARD") //forward
   187 │       {
   188 │         leds[2] = CRGB::Green;
   189 │         leds[3] = CRGB::Green;
   190 │         fill_solid(RGBleds, 6, myRGBcolor6);
   191 │         FastLED.show();
   192 │         Motor(float(160) * speed, 0, float(160) *speed, 0);
   193 │         delay(600);
   194 │         Motor(0, 0, 0, 0);
   195 │         fill_solid(leds, 4, CRGB::Black);
   196 │         FastLED.show();
   197 │       }
   198 │ 
  199 │       if (value == "GO BACK") //backward
   200 │       {
   201 │         leds[0] = CRGB::Red;
   202 │         leds[1] = CRGB::Red;
   203 │         fill_solid(RGBleds, 6, myRGBcolor6);
   204 │         FastLED.show();
   205 │         Motor(0, float(160) * speed, 0, float(160) *speed);
   206 │         delay(600);
   207 │         Motor(0, 0, 0, 0);
   208 │         fill_solid(leds, 4, CRGB::Black);
   209 │         FastLED.show();
   210 │       }
   211 │ 
  212 │       if (value == "GO RIGHT") //towards the right
   213 │       {
   214 │         leds[2] = CRGB::Green;
   215 │         fill_solid(RGBleds, 6, myRGBcolor6);
   216 │         FastLED.show();
   217 │         Motor(80, 0, 0, 80);
   218 │         delay(350);
   219 │         Motor(0, 0, 0, 0);
   220 │         fill_solid(leds, 4, CRGB::Black);
   221 │         FastLED.show();
   222 │       }
   223 │ 
  224 │       if (value == "GO LEFT") //towards the left
   225 │       {
   226 │         leds[3] = CRGB::Green;
   227 │         fill_solid(RGBleds, 6, myRGBcolor6);
   228 │         FastLED.show();
   229 │         Motor(0, 80, 80, 0);
   230 │         delay(350);
   231 │         Motor(0, 0, 0, 0);
   232 │         fill_solid(leds, 4, CRGB::Black);
   233 │         FastLED.show();
   234 │       }
   235 │ 
  236 │       if (value == "FASTER SPEED") // change speed
   237 │       {
   238 │         speed = std::min(1.5f, speed +d_speed);
   239 │       }
   240 │ 
  241 │       if (value == "SLOWER SPEED") // change speed
   242 │       {
   243 │         speed = std::max(0.5f, speed -d_speed);
   244 │       }
   245 │ 
  246 │       if (value == "LIGHTS ON") // enable lights
   247 │       {
   248 │         front_lights_cmd(true);
   249 │       }
   250 │ 
  251 │       if (value == "LIGHTS OFF") // disable lights
   252 │       {
   253 │         front_lights_cmd(false);
   254 │       }
   255 │ 
  256 │       if (value == "PLAY MUSIC") // toggle music
   257 │       {
   258 │         xSemaphoreGive(xPlayMusicSemaphore);
   259 │       }
   260 │ 
  261 │       if (value == "MANUAL CONTROL") // enable imu_control_mode
   262 │       {
   263 │         init_imu_control_state();
   264 │       }

We execute commands in voice mode.

265 │     } else {
   266 │       if (value == "VOICE CONTROL") // disable imu_control_mode
   267 │       {
   268 │         release_imu_control_state();
   269 │       }
   270 │       if (value == "LIGHTS ON") // enable lights
   271 │       {
   272 │         front_lights_cmd(true);
   273 │       }
   274 │       if (value == "LIGHTS OFF") // disable lights
   275 │       {
   276 │         front_lights_cmd(false);
   277 │       }
   278 │       if (value == "PLAY MUSIC") // toggle music
   279 │       {
   280 │         xSemaphoreGive(xPlayMusicSemaphore);
   281 │       }

And now the processing of voice commands in tilt control mode.

Let's move on to the tilt angle processing:

282 │       //********************************GRC imucommand******************************************
   283 │       if (recv_imu_coords) {
   284 │         int8_t ix, iy;
   285 │         memcpy(&ix, &value.c_str()[coords_offset + 2], sizeof(int8_t));
   286 │         memcpy(&iy, &value.c_str()[coords_offset + 3], sizeof(int8_t));

We get 2 bytes from the message: X and Y angle

287 │ 
  288 │         float x,y;
   289 │         x = clamp(float(ix), -45.f, 45.f);
   290 │         y = clamp(float(iy), -45.f, 45.f);

We limit the values in the interval [-45... 45]

291 │ 
  292 │         static const float params[] = {
  293 │           1.689410597460024e-14, 1.3274074074074107, 3.792592592592593,
  294 │           -1.0409086843526395e-17, 0.012641975308641975, -2.995326321410946e-18,
  295 │           3.215022330213294e-08, -1.3274074134624825, 3.7925925925925945,
  296 │           8.344758175431135e-18, -0.012641975308641976, -2.3814980193096928e-11};
  297 │         float l,r;
  298 │         poly_transform(params,x, y, &l, &r);

Converting tilt angles to motor values.

300 │         if (l <= MOTOR_MIN_VALUE && l >= -MOTOR_MIN_VALUE) {
   301 │           l = 0;
   302 │         }
   303 │         if (r <= MOTOR_MIN_VALUE && r >= -MOTOR_MIN_VALUE) {
   304 │           r = 0;
   305 │         }

If the values supplied to the motors are small enough, we simply set them to zero. This is essentially just the “noise canceller” for the motors that was described above.

This may need a little bit of explanation about the robot's “headlights”:

In addition to the RGB leds (high beam), there is also an array of 4 headlights:

leds[0] - rear right

leds[1] - rear left

leds[2] - front right

leds[3] - front left

307 │         fill_solid(leds, 4, CRGB::Black);

By default, the headlights are off.

308 │         int lf,lb, rf, rb;
   309 │         if (r < 0.f) {
   310 │           rf = 0;
   311 │           rb = std::min(int(-r), MOTOR_MAX_VALUE);
   312 │         } else {
   313 │           rf = std::min(int(r), MOTOR_MAX_VALUE);
   314 │           rb = 0;
   315 │           myRGBcolor.r = 0;
   316 │           myRGBcolor.g = rf;
   317 │           myRGBcolor.b = 0;
   318 │           leds[3] = myRGBcolor;
   319 │         }

We limit the power of the motors, if moving forward/left, we turn on the front turn signal.

320 │         if (l < 0.f) {
   321 │           lf = 0;
   322 │           lb = std::min(int(-l), MOTOR_MAX_VALUE);
   323 │         } else {
   324 │           lf = std::min(int(l), MOTOR_MAX_VALUE);
   325 │           lb = 0;
   326 │           myRGBcolor.r = 0;
   327 │           myRGBcolor.g = lf;
   328 │           myRGBcolor.b = 0;
   329 │           leds[2] = myRGBcolor;
   330 │         }

Similarly for the left motor

331 │         if (r < 0.f && l < 0.f) {
   332 │           myRGBcolor.r = rb;
   333 │           myRGBcolor.g = 0;
   334 │           myRGBcolor.b = 0;
   335 │           leds[0] = myRGBcolor;
   336 │           myRGBcolor.r = lb;
   337 │           myRGBcolor.g = 0;
   338 │           myRGBcolor.b = 0;
   339 │           leds[1] = myRGBcolor;
   340 │         }

When reversing, we turn on the reverse signal

341 │         Motor(lf, lb,rf, rb);
   342 │         if (l < -MOTOR_MIN_VALUE && r < -MOTOR_MIN_VALUE) {
   343 │           moving_backwards =true;
   344 │         } else {
   345 │           moving_backwards =false;
   346 │         }
   347 │       }
   348 │     }
   349 │   }
   350 │ }

Finally, we tell the motors their power.

This procedure is responsible for receiving messages from the control device.

352 │ static void NotifyCallback(BLERemoteCharacteristic* pBLERemoteCharacteristic, uint8_t*pData, size_t length, bool isNotify) {
   353 │   xMessageBufferSend(xGrcCmdBuffer,pData, length, 0);
   354 │ }
 Adds a message to the buffer that is processed by the main task.

On loss of Bluetooth communication

366 │       // TODO: stop all activity on disconnect
   367 │       Motor(0, 0, 0, 0);
   368 │       xSemaphoreGive(xDisconnectedSemaphore);

we turn off the motors and release the semaphore.

715 │   xPlayMusicSemaphore = xSemaphoreCreateBinary();
   716 │   xDisconnectedSemaphore = xSemaphoreCreateBinary();
   717 │   xGrcCmdBuffer = xMessageBufferCreate(MAX_GRC_MSG_LEN * 5);
   718 │   xTaskCreate(play_music_task, "play_music_task", 1024, NULL, 1, NULL);
   719 │   xTaskCreate(grc_cmd_task, "grc_cmd_task", 4 * 1024, NULL, 1, NULL);

Creating semaphores, buffers and tasks

At the connection to the device, we initialize the lights and motors with default values:

764 │         FastLED.setBrightness(255); //RGB lamp brightness range: 0-255
   765 │         fill_solid(leds, 4, CRGB::Black);
   766 │         fill_solid(RGBleds, 6, CRGB::Black);
   767 │         FastLED.show();
   768 │         Motor(0, 0, 0, 0);
   769 │         ledcWrite(2, 0);

When connecting, we try to capture the device disconnect semaphore.

780 │       xSemaphoreTake(xDisconnectedSemaphore, portMAX_DELAY);

Basically, we expect a disconnect signal. And when we get it, we continue looking for devices.

Conclusion

Let's summarize the discussed changes in the firmware:

1. Addition of multithreading for parallel processing of commands regardless of their reception, as well as asynchronous playback of sounds

2. Two control modes with different capabilities

3. Polynomial conversion of DevBoard rotation angle to motor power values

As you can see from the article, this did not come without some changes to proprietary sources. The added functionality is very different from the standard functionality, so there are quite a few changes. And if someone finds something interesting here, they can use it in their projects.

In particular, if you need voice control, all you need to do is receive messages from Bluetooth. Then, just process the voice commands by simple string comparison; the rest is already implemented in the code above. Now, for tilt angle processing, all you need to do is take 2 and 3 bytes of the source message and convert them to a number with a sign. These will be the X and Y rotation angles.

I hope this analysis helps those who want to use a control board for their equipment.

Also, it's worth mentioning that you can build other projects with the same dev board.

For example, a voice-activated PIN lock:

https://www.hackster.io/Grovety/autonomous-voice-activated-electromagnetic-lock-96ed9d