Skip to content

Commit 433bd7f

Browse files
authored
Merge pull request #2 from arduino-libraries/ack-mgmt
new ack management
2 parents 9793eee + cb6406f commit 433bd7f

File tree

3 files changed

+39
-9
lines changed

3 files changed

+39
-9
lines changed

src/Arduino_Alvik.cpp

Lines changed: 33 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,7 @@ void Arduino_Alvik::reset_hw(){
4545
}
4646

4747
void Arduino_Alvik::wait_for_ack(){
48+
waiting_ack = 0x00;
4849
while(last_ack != 0x00){
4950
delay(20);
5051
}
@@ -56,7 +57,8 @@ int Arduino_Alvik::begin(const bool verbose, const uint8_t core){
5657

5758
verbose_output = verbose;
5859

59-
last_ack = 255;
60+
last_ack = 0;
61+
waiting_ack = NO_ACK;
6062

6163
version[0] = 0;
6264
version[1] = 0;
@@ -242,7 +244,12 @@ int Arduino_Alvik::parse_message(){
242244
switch(code){
243245
// get ack code
244246
case 'x':
245-
packeter->unpacketC1B(code, last_ack);
247+
if (waiting_ack == NO_ACK){
248+
packeter->unpacketC1B(code, last_ack);
249+
last_ack = 0x00;
250+
} else {
251+
packeter->unpacketC1B(code, last_ack);
252+
}
246253
break;
247254

248255

@@ -410,35 +417,54 @@ void Arduino_Alvik::reset_pose(const float x, const float y, const float theta,
410417
}
411418

412419
bool Arduino_Alvik::is_target_reached(){
413-
if ((last_ack != 'M') && (last_ack != 'R')){
420+
421+
if (waiting_ack == NO_ACK){
422+
return true;
423+
}
424+
425+
if (last_ack != waiting_ack){
414426
delay(50);
415427
return false;
416428
}
417429
msg_size = packeter->packetC1B('X', 'K');
418430
uart->write(packeter->msg, msg_size);
431+
waiting_ack = NO_ACK;
432+
last_ack = 0x00;
419433
delay(200);
420434
return true;
421435
}
422436

423-
void Arduino_Alvik::wait_for_target(){ //it is private
424-
while (!is_target_reached()){}
437+
void Arduino_Alvik::wait_for_target(const int idle_time){ //it is private
438+
unsigned long start_t = millis();
439+
440+
while (true){
441+
if (((millis() - start_t) >= idle_time*1000) && is_target_reached()) {
442+
break;
443+
} else
444+
{
445+
delay(100);
446+
}
447+
448+
}
425449
}
426450

427451
void Arduino_Alvik::rotate(const float angle, const uint8_t unit, const bool blocking){
428452
delay(200);
429453
msg_size = packeter->packetC1F('R', convert_angle(angle, unit, DEG));
430454
uart->write(packeter->msg, msg_size);
455+
waiting_ack = 'R';
431456
if (blocking){
432-
wait_for_target();
457+
wait_for_target(round(angle/MOTOR_CONTROL_DEG_S));
433458
}
434459
}
435460

436461
void Arduino_Alvik::move(const float distance, const uint8_t unit, const bool blocking){
437462
delay(200);
438463
msg_size = packeter->packetC1F('G', convert_distance(distance, unit, MM));
439464
uart->write(packeter->msg, msg_size);
465+
waiting_ack = 'M';
440466
if (blocking){
441-
wait_for_target();
467+
wait_for_target(round(distance/MOTOR_CONTROL_MM_S));
442468
}
443469
}
444470

src/Arduino_Alvik.h

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@ class Arduino_Alvik{
3636
uint8_t msg_size;
3737

3838
uint8_t last_ack;
39+
uint8_t waiting_ack;
3940

4041
float converted_angular;
4142

@@ -96,7 +97,7 @@ class Arduino_Alvik{
9697

9798
void get_touch(); // service function to parse touch
9899
void set_leds(); // service function to set leds by a byte
99-
void wait_for_target(); // service function that wait for ack
100+
void wait_for_target(const int idle_time); // service function that wait for ack
100101

101102
float limit(float value, const float min, const float max); // limit a value
102103
float normalize(float value, const float min, const float max); // normalize a value

src/definitions.h

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -32,11 +32,14 @@
3232

3333
#define CHARGE_THRESHOLD 97
3434

35+
#define NO_ACK 0xFF
36+
3537
const float WHEEL_DIAMETER_MM = 34.0;
3638
const float WHEEL_TRACK_MM = 89.0;
3739
const float MOTOR_MAX_RPM = 70.0;
3840
const float ROBOT_MAX_DEG_S = 6*(2*MOTOR_MAX_RPM*WHEEL_DIAMETER_MM)/WHEEL_TRACK_MM;
39-
41+
const float MOTOR_CONTROL_MM_S = 100.0;
42+
const float MOTOR_CONTROL_DEG_S = 100.0;
4043

4144
// unit conversion constants
4245

0 commit comments

Comments
 (0)