Skip to content

Commit b70b540

Browse files
gbr1gbr1
authored
Merge pull request #16 from arduino-libraries/thread_and_lift
add: semaphore handling on writing, get_lifted()
2 parents ede6004 + 7ad3ca2 commit b70b540

File tree

5 files changed

+123
-20
lines changed

5 files changed

+123
-20
lines changed

‎examples/hot_wheels/hot_wheels.ino‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
/*
2+
This file is part of the Arduino_Alvik library.
3+
4+
Copyright (c) 2024 Arduino SA
5+
6+
This Source Code Form is subject to the terms of the Mozilla Public
7+
License, v. 2.0. If a copy of the MPL was not distributed with this
8+
file, You can obtain one at http://mozilla.org/MPL/2.0/.
9+
10+
*/
11+
12+
// This examples shows how to create a thread. When Alvik is lifted, it stops.
13+
14+
#include "Arduino_Alvik.h"
15+
16+
Arduino_Alvik alvik;
17+
18+
TaskHandle_t lift_task = NULL;
19+
20+
21+
uint8_t color_val=0;
22+
23+
void setup() {
24+
alvik.begin();
25+
xTaskCreate(&check_lift, "lift_task", 10000, NULL, 0, &lift_task);
26+
}
27+
28+
void loop() {
29+
blinking_leds(color_val);
30+
color_val = (color_val + 1) % 7;
31+
delay(500);
32+
}
33+
34+
void blinking_leds(uint8_t val){
35+
alvik.left_led.set_color(val & 0x01, val & 0x02, val & 0x04);
36+
alvik.right_led.set_color(val & 0x02, val & 0x04, val & 0x01);
37+
}
38+
39+
void check_lift(void * pvParameters){
40+
while(1){
41+
if (alvik.get_lifted()){
42+
alvik.brake();
43+
}
44+
else{
45+
alvik.drive(5,0);
46+
}
47+
}
48+
}

‎library.properties‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
name=Arduino_Alvik
2-
version=1.1.0
2+
version=1.1.1
33
author=Arduino, Giovanni di Dio Bruno, Lucio Rossi
44
maintainer=Arduino <[email protected]>
55
sentence=Library to code Arduino Alvik robot

‎src/Arduino_Alvik.cpp‎

Lines changed: 57 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#include "default_colors.h"
1616

1717
Arduino_Alvik::Arduino_Alvik():i2c(Wire){
18+
buffer_semaphore = xSemaphoreCreateRecursiveMutex();
1819
update_semaphore = xSemaphoreCreateMutex();
1920
uart = new HardwareSerial(UART); //&Serial0
2021
packeter = new ucPack(200);
@@ -30,14 +31,14 @@ Arduino_Alvik::Arduino_Alvik():i2c(Wire){
3031
robot_vel_semaphore = xSemaphoreCreateMutex();
3132
robot_pos_semaphore = xSemaphoreCreateMutex();
3233

33-
left_led = ArduinoAlvikRgbLed(uart, packeter, "left_led", &led_state, 2);
34-
right_led = ArduinoAlvikRgbLed(uart, packeter,"right_led", &led_state, 5);
34+
left_led = ArduinoAlvikRgbLed(uart, packeter, &buffer_semaphore, "left_led", &led_state, 2);
35+
right_led = ArduinoAlvikRgbLed(uart, packeter, &buffer_semaphore, "right_led", &led_state, 5);
3536

36-
left_wheel = ArduinoAlvikWheel(uart, packeter, 'L', &joints_velocity[0], &joints_position[0], WHEEL_DIAMETER_MM, *this);
37-
right_wheel = ArduinoAlvikWheel(uart, packeter, 'R', &joints_velocity[1], &joints_position[1], WHEEL_DIAMETER_MM, *this);
37+
left_wheel = ArduinoAlvikWheel(uart, packeter, &buffer_semaphore, 'L', &joints_velocity[0], &joints_position[0], WHEEL_DIAMETER_MM, *this);
38+
right_wheel = ArduinoAlvikWheel(uart, packeter, &buffer_semaphore, 'R', &joints_velocity[1], &joints_position[1], WHEEL_DIAMETER_MM, *this);
3839

39-
servo_A = ArduinoAlvikServo(uart, packeter, 'A', 0, servo_positions);
40-
servo_B = ArduinoAlvikServo(uart, packeter, 'B', 1, servo_positions);
40+
servo_A = ArduinoAlvikServo(uart, packeter, &buffer_semaphore, 'A', 0, servo_positions);
41+
servo_B = ArduinoAlvikServo(uart, packeter, &buffer_semaphore, 'B', 1, servo_positions);
4142
}
4243

4344
void Arduino_Alvik::reset_hw(){ //it is private
@@ -404,8 +405,10 @@ void Arduino_Alvik::get_wheels_speed(float & left, float & right, const uint8_t
404405
}
405406

406407
void Arduino_Alvik::set_wheels_speed(const float left, const float right, const uint8_t unit){
408+
while (!xSemaphoreTakeRecursive(buffer_semaphore, TICK_TIME_SEMAPHORE));
407409
msg_size = packeter->packetC2F('J', convert_rotational_speed(left, unit, RPM), convert_rotational_speed(right, unit, RPM));
408410
uart->write(packeter->msg, msg_size);
411+
xSemaphoreGiveRecursive(buffer_semaphore);
409412
}
410413

411414
void Arduino_Alvik::get_wheels_position(float & left, float & right, const uint8_t unit){
@@ -416,12 +419,14 @@ void Arduino_Alvik::get_wheels_position(float & left, float & right, const uint8
416419
}
417420

418421
void Arduino_Alvik::set_wheels_position(const float left, const float right, const uint8_t unit, const bool blocking){
422+
while (!xSemaphoreTakeRecursive(buffer_semaphore, TICK_TIME_SEMAPHORE));
419423
msg_size = packeter->packetC2F('A', convert_angle(left, unit, DEG), convert_angle(right, unit, DEG));
420424
uart->write(packeter->msg, msg_size);
421425
waiting_ack = 'P';
422426
if (blocking){
423427
wait_for_target(max(left, right) / MOTOR_CONTROL_DEG_S);
424428
}
429+
xSemaphoreGiveRecursive(buffer_semaphore);
425430
}
426431

427432
void Arduino_Alvik::get_drive_speed(float & linear, float & angular, const uint8_t linear_unit, const uint8_t angular_unit){
@@ -437,6 +442,7 @@ void Arduino_Alvik::get_drive_speed(float & linear, float & angular, const uint8
437442
}
438443

439444
void Arduino_Alvik::drive(const float linear, const float angular, const uint8_t linear_unit, const uint8_t angular_unit){
445+
while (!xSemaphoreTakeRecursive(buffer_semaphore, TICK_TIME_SEMAPHORE));
440446
if (angular_unit == PERCENTAGE){
441447
converted_angular = (angular/ROBOT_MAX_DEG_S)*100.0;
442448
}
@@ -445,6 +451,7 @@ void Arduino_Alvik::drive(const float linear, const float angular, const uint8_t
445451
}
446452
msg_size = packeter->packetC2F('V', convert_speed(linear, linear_unit, MM_S), converted_angular);
447453
uart->write(packeter->msg, msg_size);
454+
xSemaphoreGiveRecursive(buffer_semaphore);
448455
}
449456

450457
void Arduino_Alvik::get_pose(float & x, float & y, float & theta, const uint8_t distance_unit, const uint8_t angle_unit){
@@ -456,11 +463,14 @@ void Arduino_Alvik::get_pose(float & x, float & y, float & theta, const uint8_t
456463
}
457464

458465
void Arduino_Alvik::reset_pose(const float x, const float y, const float theta, const uint8_t distance_unit, const uint8_t angle_unit){
466+
while (!xSemaphoreTakeRecursive(buffer_semaphore, TICK_TIME_SEMAPHORE));
459467
msg_size = packeter->packetC3F('Z', convert_distance(x, distance_unit, MM), convert_distance(y, distance_unit, MM), convert_distance(theta, angle_unit, DEG));
460468
uart->write(packeter->msg, msg_size);
469+
xSemaphoreGiveRecursive(buffer_semaphore);
461470
}
462471

463472
bool Arduino_Alvik::is_target_reached(){
473+
while (!xSemaphoreTakeRecursive(buffer_semaphore, TICK_TIME_SEMAPHORE));
464474
if (waiting_ack == NO_ACK){
465475
return true;
466476
}
@@ -470,43 +480,48 @@ bool Arduino_Alvik::is_target_reached(){
470480
waiting_ack = NO_ACK;
471481
last_ack = 0x00;
472482
delay(100);
483+
xSemaphoreGiveRecursive(buffer_semaphore);
473484
return true;
474485
}
486+
xSemaphoreGiveRecursive(buffer_semaphore);
475487
return false;
476488
}
477489

478-
void Arduino_Alvik::wait_for_target(const int idle_time){ //it is private
490+
void Arduino_Alvik::wait_for_target(const int idle_time){ //it is private
479491
unsigned long start_t = millis();
480492

481493
while (true){
482494
if (((millis() - start_t) >= idle_time*1000) && is_target_reached()) {
483495
break;
484-
} else
485-
{
496+
}
497+
else{
486498
delay(100);
487499
}
488-
489500
}
490501
}
491502

492503
void Arduino_Alvik::rotate(const float angle, const uint8_t unit, const bool blocking){
504+
while (!xSemaphoreTakeRecursive(buffer_semaphore, TICK_TIME_SEMAPHORE));
493505
delay(200);
494506
msg_size = packeter->packetC1F('R', convert_angle(angle, unit, DEG));
495507
uart->write(packeter->msg, msg_size);
496508
waiting_ack = 'R';
497509
if (blocking){
498510
wait_for_target(round(angle/MOTOR_CONTROL_DEG_S));
499511
}
512+
xSemaphoreGiveRecursive(buffer_semaphore);
500513
}
501514

502515
void Arduino_Alvik::move(const float distance, const uint8_t unit, const bool blocking){
516+
while (!xSemaphoreTakeRecursive(buffer_semaphore, TICK_TIME_SEMAPHORE));
503517
delay(200);
504518
msg_size = packeter->packetC1F('G', convert_distance(distance, unit, MM));
505519
uart->write(packeter->msg, msg_size);
506520
waiting_ack = 'M';
507521
if (blocking){
508522
wait_for_target(round(distance/MOTOR_CONTROL_MM_S));
509523
}
524+
xSemaphoreGiveRecursive(buffer_semaphore);
510525
}
511526

512527
void Arduino_Alvik::brake(){
@@ -841,6 +856,10 @@ bool Arduino_Alvik::get_shake(){
841856
return move_bits & 0b00000001;
842857
}
843858

859+
bool Arduino_Alvik::get_lifted(){
860+
return move_bits & 0b00000010;
861+
}
862+
844863
String Arduino_Alvik::get_tilt(){
845864
if (move_bits & 0b00000100){
846865
return "X";
@@ -943,8 +962,10 @@ bool Arduino_Alvik::is_battery_charging(){
943962
//-----------------------------------------------------------------------------------------------//
944963

945964
void Arduino_Alvik::set_leds(){ //it is private
965+
while (!xSemaphoreTakeRecursive(buffer_semaphore, TICK_TIME_SEMAPHORE));
946966
msg_size = packeter->packetC1B('L', led_state);
947967
uart->write(packeter->msg, msg_size);
968+
xSemaphoreGiveRecursive(buffer_semaphore);
948969
}
949970

950971
void Arduino_Alvik::set_builtin_led(const bool value){
@@ -968,10 +989,12 @@ void Arduino_Alvik::set_illuminator(const bool value){
968989
}
969990

970991
void Arduino_Alvik::set_servo_positions(const uint8_t a_position, const uint8_t b_position){
992+
while (!xSemaphoreTakeRecursive(buffer_semaphore, TICK_TIME_SEMAPHORE));
971993
servo_positions[0] = a_position;
972994
servo_positions[1] = b_position;
973995
msg_size = packeter->packetC2B('S', a_position, b_position);
974996
uart->write(packeter->msg, msg_size);
997+
xSemaphoreGiveRecursive(buffer_semaphore);
975998
}
976999

9771000
void Arduino_Alvik::get_servo_positions(int & a_position, int & b_position){
@@ -980,8 +1003,10 @@ void Arduino_Alvik::get_servo_positions(int & a_position, int & b_position){
9801003
}
9811004

9821005
void Arduino_Alvik::set_behaviour(const uint8_t behaviour){
1006+
while (!xSemaphoreTakeRecursive(buffer_semaphore, TICK_TIME_SEMAPHORE));
9831007
msg_size = packeter->packetC1B('B', behaviour);
9841008
uart->write(packeter->msg, msg_size);
1009+
xSemaphoreGiveRecursive(buffer_semaphore);
9851010
}
9861011

9871012
void Arduino_Alvik::get_version(uint8_t & upper, uint8_t & middle, uint8_t & lower, String version){
@@ -1032,10 +1057,12 @@ bool Arduino_Alvik::check_firmware_compatibility(){
10321057
// RGB led class //
10331058
//-----------------------------------------------------------------------------------------------//
10341059

1035-
Arduino_Alvik::ArduinoAlvikRgbLed::ArduinoAlvikRgbLed(HardwareSerial * serial, ucPack * packeter, String label,
1060+
Arduino_Alvik::ArduinoAlvikRgbLed::ArduinoAlvikRgbLed(HardwareSerial * serial, ucPack * packeter, SemaphoreHandle_t * buffer_semaphore,
1061+
String label,
10361062
uint8_t * led_state, uint8_t offset){
10371063
_serial = serial;
10381064
_packeter = packeter;
1065+
_buffer_semaphore = buffer_semaphore;
10391066
this->label = label;
10401067
_led_state = led_state;
10411068
_offset = offset;
@@ -1044,6 +1071,7 @@ Arduino_Alvik::ArduinoAlvikRgbLed::ArduinoAlvikRgbLed(HardwareSerial * serial, u
10441071
void Arduino_Alvik::ArduinoAlvikRgbLed::operator=(const ArduinoAlvikRgbLed& other){
10451072
_serial = other._serial;
10461073
_packeter = other._packeter;
1074+
_buffer_semaphore = other._buffer_semaphore;
10471075
label = other.label;
10481076
_led_state = other._led_state;
10491077
_offset = other._offset;
@@ -1072,41 +1100,49 @@ void Arduino_Alvik::ArduinoAlvikRgbLed::set_color(const bool red, const bool gre
10721100
else{
10731101
(*_led_state) = (*_led_state) & ~(1<<(_offset+2));
10741102
}
1075-
1103+
while (!xSemaphoreTakeRecursive(*_buffer_semaphore, TICK_TIME_SEMAPHORE));
10761104
_msg_size = _packeter->packetC1B('L', *_led_state);
10771105
_serial->write(_packeter->msg, _msg_size);
1106+
xSemaphoreGiveRecursive(*_buffer_semaphore);
10781107
}
10791108

10801109

10811110
//-----------------------------------------------------------------------------------------------//
10821111
// wheel class //
10831112
//-----------------------------------------------------------------------------------------------//
10841113

1085-
Arduino_Alvik::ArduinoAlvikWheel::ArduinoAlvikWheel(HardwareSerial * serial, ucPack * packeter, uint8_t label,
1114+
Arduino_Alvik::ArduinoAlvikWheel::ArduinoAlvikWheel(HardwareSerial * serial, ucPack * packeter, SemaphoreHandle_t * buffer_semaphore,
1115+
uint8_t label,
10861116
float * joint_velocity, float * joint_position, float wheel_diameter, Arduino_Alvik & alvik):_alvik(&alvik){
10871117
_serial = serial;
10881118
_packeter = packeter;
1119+
_buffer_semaphore = buffer_semaphore;
10891120
_label = label;
10901121
_wheel_diameter = wheel_diameter;
10911122
_joint_velocity = joint_velocity;
10921123
_joint_position = joint_position;
10931124
}
10941125

10951126
void Arduino_Alvik::ArduinoAlvikWheel::reset(const float initial_position, const uint8_t unit){
1127+
while (!xSemaphoreTakeRecursive(*_buffer_semaphore, TICK_TIME_SEMAPHORE));
10961128
_msg_size = _packeter->packetC2B1F('W', _label, 'Z', convert_angle(initial_position, unit, DEG));
10971129
_serial->write(_packeter->msg, _msg_size);
1130+
xSemaphoreGiveRecursive(*_buffer_semaphore);
10981131
}
10991132

11001133
void Arduino_Alvik::ArduinoAlvikWheel::set_pid_gains(const float kp, const float ki, const float kd){
1134+
while (!xSemaphoreTakeRecursive(*_buffer_semaphore, TICK_TIME_SEMAPHORE));
11011135
_msg_size = _packeter->packetC1B3F('P', _label, kp, ki, kd);
11021136
_serial->write(_packeter->msg, _msg_size);
1137+
xSemaphoreGiveRecursive(*_buffer_semaphore);
11031138
}
11041139

11051140
void Arduino_Alvik::ArduinoAlvikWheel::stop(){
11061141
set_speed(0);
11071142
}
11081143

11091144
void Arduino_Alvik::ArduinoAlvikWheel::set_speed(const float velocity, const uint8_t unit){
1145+
while (!xSemaphoreTakeRecursive(*_buffer_semaphore, TICK_TIME_SEMAPHORE));
11101146
if (unit==PERCENTAGE){
11111147
converted_vel = (velocity/100.0)*MOTOR_MAX_RPM;
11121148
}
@@ -1115,6 +1151,7 @@ void Arduino_Alvik::ArduinoAlvikWheel::set_speed(const float velocity, const uin
11151151
}
11161152
_msg_size = _packeter->packetC2B1F('W', _label, 'V', converted_vel);
11171153
_serial->write(_packeter->msg, _msg_size);
1154+
xSemaphoreGiveRecursive(*_buffer_semaphore);
11181155
}
11191156

11201157
float Arduino_Alvik::ArduinoAlvikWheel::get_speed(const uint8_t unit){
@@ -1125,12 +1162,14 @@ float Arduino_Alvik::ArduinoAlvikWheel::get_speed(const uint8_t unit){
11251162
}
11261163

11271164
void Arduino_Alvik::ArduinoAlvikWheel::set_position(const float position, const uint8_t unit, const bool blocking){
1165+
while (!xSemaphoreTakeRecursive(*_buffer_semaphore, TICK_TIME_SEMAPHORE));
11281166
_msg_size = _packeter->packetC2B1F('W', _label, 'P', convert_angle(position, unit, DEG));
11291167
_serial->write(_packeter->msg, _msg_size);
11301168
_alvik->waiting_ack = 'P';
11311169
if (blocking){
11321170
_alvik->wait_for_target(position / MOTOR_CONTROL_DEG_S);
11331171
}
1172+
xSemaphoreGiveRecursive(*_buffer_semaphore);
11341173
}
11351174

11361175
float Arduino_Alvik::ArduinoAlvikWheel::get_position(const uint8_t unit){
@@ -1141,19 +1180,23 @@ float Arduino_Alvik::ArduinoAlvikWheel::get_position(const uint8_t unit){
11411180
// servo class //
11421181
//-----------------------------------------------------------------------------------------------//
11431182

1144-
Arduino_Alvik::ArduinoAlvikServo::ArduinoAlvikServo(HardwareSerial * serial, ucPack * packeter, char label,
1183+
Arduino_Alvik::ArduinoAlvikServo::ArduinoAlvikServo(HardwareSerial * serial, ucPack * packeter, SemaphoreHandle_t * buffer_semaphore,
1184+
char label,
11451185
uint8_t servo_id, uint8_t * positions){
11461186
_serial = serial;
11471187
_packeter = packeter;
1188+
_buffer_semaphore = buffer_semaphore;
11481189
_label = label;
11491190
_servo_id = servo_id;
11501191
_positions = positions;
11511192
}
11521193

11531194
void Arduino_Alvik::ArduinoAlvikServo::set_position(const uint8_t position){
1195+
while (!xSemaphoreTakeRecursive(*_buffer_semaphore, TICK_TIME_SEMAPHORE));
11541196
_positions[_servo_id] = position;
11551197
_msg_size = _packeter->packetC2B('S', _positions[0], _positions[1]);
11561198
_serial->write(_packeter->msg, _msg_size);
1199+
xSemaphoreGiveRecursive(*_buffer_semaphore);
11571200
}
11581201

11591202
int Arduino_Alvik::ArduinoAlvikServo::get_position(){

0 commit comments

Comments
 (0)