Merge pull request #7 from arduino-libraries/devel

0.2.0 - ready
arduino-libraries · Feb 2, 2024 · 86dcea6 · 86dcea6
2 parents 65a837e + d06a2ad
commit 86dcea6
Show file tree

Hide file tree

Showing 6 changed files with 50 additions and 22 deletions.
diff --git a/examples/firmware_01/firmware_01.ino b/examples/firmware_01/firmware_01.ino
@@ -28,12 +28,15 @@ uint8_t code;
 uint8_t label;
 uint8_t control_type;
 uint8_t msg_size;
-
+uint8_t ack_required=0;
+bool ack_check=false;
+uint8_t ack_code=0;
 
 unsigned long tmotor=0;
 unsigned long tsend=0;
 unsigned long tsensor=0;
 unsigned long timu=0;
+unsigned long tack=0;
 
 
 float left, right, value;
@@ -73,6 +76,7 @@ void setup(){
   tsend=millis();
   tsensor=millis();
   timu=millis();
+  tack=millis();
 }
 
 void loop(){
@@ -154,21 +158,33 @@ void loop(){
         packeter.unpacketC1F(code, value);
         alvik.disablePositionControl();
         alvik.rotate(value);
+        ack_required=MOVEMENT_ROTATE;
+        ack_check=true;
         break;
 
       case 'G':
         packeter.unpacketC1F(code, value);
         alvik.disablePositionControl();
         alvik.move(value);
+        ack_required=MOVEMENT_MOVE;
+        ack_check=true;
         break;
 
       case 'Z':
         packeter.unpacketC3F(code, x, y, theta);
         alvik.resetPose(x, y, theta);
         break;
+
+      case 'X':
+        packeter.unpacketC1B(code, ack_code);
+        if (ack_code == 'K') {
+          ack_check = false;
+        }
+        break;
     }
   }
 
+  // sensors publish
   if (millis()-tsensor>10){
     tsensor=millis();
     switch(sensor_id){
@@ -204,6 +220,7 @@ void loop(){
     }
   } 
 
+  // motors update & publish
   if (millis()-tmotor>20){
     tmotor=millis();
     alvik.updateMotors();
@@ -218,24 +235,28 @@ void loop(){
     msg_size = packeter.packetC2F('v', alvik.getLinearVelocity(), alvik.getAngularVelocity());
     alvik.serial->write(packeter.msg, msg_size);
     // pose
-    msg_size = packeter.packetC3F('s', alvik.getX(), alvik.getY(), alvik.getTheta());
+    msg_size = packeter.packetC3F('z', alvik.getX(), alvik.getY(), alvik.getTheta());
     alvik.serial->write(packeter.msg, msg_size);
+  }
 
-    if (alvik.getKinematicsMovement()!=MOVEMENT_DISABLED){
-      if (alvik.isTargetReached()){
-        if (alvik.getKinematicsMovement()==MOVEMENT_ROTATE){
-          msg_size = packeter.packetC1B('x', 'R');
-        }
-        if (alvik.getKinematicsMovement()==MOVEMENT_MOVE){
-          msg_size = packeter.packetC1B('x', 'M');
-        }
-        alvik.serial->write(packeter.msg, msg_size);
-        //alvik.disableKinematicsMovement();
+  // acknowledge
+  if (millis()-tack > 100){
+    tack = millis();
+    if (ack_check && alvik.isTargetReached()){
+      if (ack_required == MOVEMENT_ROTATE){
+        msg_size = packeter.packetC1B('x', 'R');
+      }
+      if (ack_required == MOVEMENT_MOVE){
+        msg_size = packeter.packetC1B('x', 'M');
       }
-
     }
+    else{
+      msg_size = packeter.packetC1B('x', 0);
+    }
+    alvik.serial->write(packeter.msg, msg_size);
   }
 
+  // imu update
   if (millis()-timu>10){
     timu=millis();
     alvik.updateImu();

diff --git a/library.properties b/library.properties
@@ -1,5 +1,5 @@
 name=Arduino Alvik Carrier
-version=0.1.2
+version=0.2.0
 author=Arduino, Giovanni di Dio Bruno, Lucio Rossi
 maintainer=Arduino <[email protected]>
 sentence=Library and firmware for Arduino Alvik Carrier board

diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp
@@ -87,6 +87,7 @@ int Arduino_AlvikCarrier::begin(){
     beginLeds();
 
     serial->begin(UART_BAUD);
+    serial->flush();
 
     // setup alternate functions
     AF_Tim2_pwm();
@@ -678,13 +679,17 @@ void Arduino_AlvikCarrier::updateKinematics(){
             drive(0, round(rotate_pid->getControlOutput()/10.0)*10);
             if (abs(rotate_pid->getError())<ROTATE_THREASHOLD){
                 kinematics_achieved=true;
+                disableKinematicsMovement();
+                drive(0,0);
             }
         }
         if (kinematics_movement==MOVEMENT_MOVE){
             move_pid->update((kinematics->getTravel()-previous_travel)*move_direction);
             drive(round(move_pid->getControlOutput()/10.0)*10, 0);
             if (abs(move_pid->getError())<MOVE_THREADSHOLD){
                 kinematics_achieved=true;
+                disableKinematicsMovement();
+                drive(0,0);
             }
 
         }
@@ -746,10 +751,11 @@ void Arduino_AlvikCarrier::lockingRotate(const float angle){
 }
 
 void Arduino_AlvikCarrier::rotate(const float angle){
+    disableKinematicsMovement();
+    kinematics_achieved=false;
     rotate_pid->reset();
     rotate_pid->setReference(kinematics->getTheta()+angle);
     kinematics_movement=MOVEMENT_ROTATE;
-    kinematics_achieved=false;
 }
 
 void Arduino_AlvikCarrier::lockingMove(const float distance){
@@ -779,6 +785,9 @@ void Arduino_AlvikCarrier::lockingMove(const float distance){
 }
 
 void Arduino_AlvikCarrier::move(const float distance){
+    disableKinematicsMovement();
+    kinematics_achieved=false;
+
     move_pid->reset();
     previous_travel=kinematics->getTravel();
     if (distance<0){
@@ -789,7 +798,6 @@ void Arduino_AlvikCarrier::move(const float distance){
     }
     move_pid->setReference(distance);
     kinematics_movement=MOVEMENT_MOVE;
-    kinematics_achieved=false;
 }
 
 void Arduino_AlvikCarrier::disableKinematicsMovement(){

diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h
@@ -234,8 +234,8 @@ class Arduino_AlvikCarrier{
         void lockingRotate(const float angle);                                          // rotate of angle degrees     
         void lockingMove(const float distance);                                         // move of distance millimeters
 
-        void disableKinematicsMovement();
-        bool isTargetReached();
+        void disableKinematicsMovement();                                               // disable movements that requires kinematics
+        bool isTargetReached();                                                         // get true if a movement is accomplished
         uint8_t getKinematicsMovement();                                                // get which kind of motion is running in kinematic control
 
 

diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h
@@ -46,7 +46,7 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO;
 #define ROTATE_KD_DEFAULT 0.001
 #define ROTATE_CONTROL_PERIOD 0.02
 #define ROTATE_MAX_SPEED 45.0
-#define ROTATE_THREASHOLD 3
+#define ROTATE_THREASHOLD 1
 
 #define MOVE_KP_DEFAULT 5.0
 #define MOVE_KI_DEFAULT 0.0
@@ -79,8 +79,8 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ);
 
 // Library version
 #define VERSION_BYTE_HIGH 0
-#define VERSION_BYTE_MID 1
-#define VERSION_BYTE_LOW 2
+#define VERSION_BYTE_MID 2
+#define VERSION_BYTE_LOW 0
 
 
 

diff --git a/src/robotics/kinematics.h b/src/robotics/kinematics.h
@@ -174,7 +174,6 @@ class Kinematics{
             delta_left=0.0;
             delta_right=0.0;
             delta_travel=0.0;
-
         }
 
         void resetTravel(){