@@ -45,6 +45,7 @@ void Arduino_Alvik::reset_hw(){
45
45
}
46
46
47
47
void Arduino_Alvik::wait_for_ack (){
48
+ waiting_ack = 0x00 ;
48
49
while (last_ack != 0x00 ){
49
50
delay (20 );
50
51
}
@@ -56,7 +57,8 @@ int Arduino_Alvik::begin(const bool verbose, const uint8_t core){
56
57
57
58
verbose_output = verbose;
58
59
59
- last_ack = 255 ;
60
+ last_ack = 0 ;
61
+ waiting_ack = NO_ACK;
60
62
61
63
version[0 ] = 0 ;
62
64
version[1 ] = 0 ;
@@ -242,7 +244,12 @@ int Arduino_Alvik::parse_message(){
242
244
switch (code){
243
245
// get ack code
244
246
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
+ }
246
253
break ;
247
254
248
255
@@ -410,35 +417,54 @@ void Arduino_Alvik::reset_pose(const float x, const float y, const float theta,
410
417
}
411
418
412
419
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){
414
426
delay (50 );
415
427
return false ;
416
428
}
417
429
msg_size = packeter->packetC1B (' X' , ' K' );
418
430
uart->write (packeter->msg , msg_size);
431
+ waiting_ack = NO_ACK;
432
+ last_ack = 0x00 ;
419
433
delay (200 );
420
434
return true ;
421
435
}
422
436
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
+ }
425
449
}
426
450
427
451
void Arduino_Alvik::rotate (const float angle, const uint8_t unit, const bool blocking){
428
452
delay (200 );
429
453
msg_size = packeter->packetC1F (' R' , convert_angle (angle, unit, DEG));
430
454
uart->write (packeter->msg , msg_size);
455
+ waiting_ack = ' R' ;
431
456
if (blocking){
432
- wait_for_target ();
457
+ wait_for_target (round (angle/MOTOR_CONTROL_DEG_S) );
433
458
}
434
459
}
435
460
436
461
void Arduino_Alvik::move (const float distance, const uint8_t unit, const bool blocking){
437
462
delay (200 );
438
463
msg_size = packeter->packetC1F (' G' , convert_distance (distance, unit, MM));
439
464
uart->write (packeter->msg , msg_size);
465
+ waiting_ack = ' M' ;
440
466
if (blocking){
441
- wait_for_target ();
467
+ wait_for_target (round (distance/MOTOR_CONTROL_MM_S) );
442
468
}
443
469
}
444
470
0 commit comments