sornpraram xu / RMD

RMD-X Motor Library - Last Update V0.5 - On going...... Current available Function - read velocity - read all data - send velocity - send position

Diff: RMD.cpp

Revision:
5:07e41b6c6692
Parent:
4:083894adb121
Child:
7:2f2c3f244208
--- a/RMD.cpp	Wed Nov 04 09:56:37 2020 +0000
+++ b/RMD.cpp	Wed Nov 04 10:01:58 2020 +0000
@@ -23,6 +23,19 @@
 
 }
 
+void RMD::status_position(){
+    if (read(msg)){
+
+        int fv = (((uint16_t)msg.data[7] << 8) + ((uint16_t)msg.data[6])) / ((uint16_t)6);
+        
+        printf(" Position : %d\n", fv);
+        printf(" Position high byte: %x\n", msg.data[7]); 
+        printf(" Position low byte: %x\n", msg.data[6]);      
+        printf(" -------------------------- \n");  
+        }
+
+}
+
 void RMD::status(){
     if (read(msg)){
         printf("Message received: %d\n", msg.id);