Revision 20:0756a01f7415, committed 2020-12-19

Comitter:

sornpraram

Date:

Sat Dec 19 08:43:14 2020 +0000

Parent:

19:51558e307b28

Commit message:

Final V0.5

Changed in this revision

--- a/RMD.cpp	Wed Nov 04 14:34:47 2020 +0000
+++ b/RMD.cpp	Sat Dec 19 08:43:14 2020 +0000
@@ -5,11 +5,13 @@
 
 #define DEVICE_CAN_ID_1 0x141
 
+// CAN BUS connection
 RMD::RMD(PinName rx, PinName tx) : CAN(rx,tx,1000000) {
     frequency(1000000);
 
 }
 
+// Reading current velocity output (degree per sec)
 int RMD::status_velocity(){
     if (read(msg)){
 
@@ -18,29 +20,19 @@
         }
     else (){
         return 0;
-        //printf(" Speed : %d\n", fv);
-//        printf(" Speed high byte: %x\n", msg.data[5]); 
-//        printf(" Speed low byte: %x\n", msg.data[4]);      
-//        printf(" ---------------------------- \n");  
-
         }
-    
-
 }
 
+// Reading current position output (degree)
 void RMD::status_position(){
     if (read(msg)){
 
         int fv = (((uint16_t)msg.data[7] << 8) + ((uint16_t)msg.data[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");  
+        return fv;
         }
-
 }
 
+// Reading everything
 void RMD::status(){
     if (read(msg)){
         printf("Message received: %d\n", msg.id);
@@ -56,16 +48,16 @@
         }
 }
 
+// Helper function for convert int to byte (used inside library only)
 void RMD::int2byte(int num){
-
     CANdata[0] = (num >> 24) & 0xFF;
     CANdata[1] = (num >> 16) & 0xFF;
     CANdata[2] = (num >> 8) & 0xFF;
     CANdata[3] = num & 0xFF;
 }
 
+// Helper function for convert int to byte (used inside library only)
 void RMD::int2byte2(int pos, int speed){
-
     CANdata[0] = speed & 0xFF;
     CANdata[1] = (speed >> 8) & 0xFF;
     CANdata[2] = pos & 0xFF;
@@ -74,18 +66,10 @@
     CANdata[5] = (pos >> 24) & 0xFF;
 }
 
-
-// Speed Unit is Degree per seconds
-// Speed Sensitivity is 0.01 dps per LSB (Least Significant Bit)
-// Speed Input is dps vary from 32bits[-888,000 (0.01)dps, +888,000 (0.01)dps] (0xFFF27340 , 0x000D8CC0) [32bits is (−2,147,483,648, +2,147,483,647)] 
-// Speed Output is dps vary from (-8880 dps, +8880 dps) (-1480 rpm , +1480 rpm)
+// Drive speed. Output is dps vary from (-8880 dps, +8880 dps) (-1480 rpm , +1480 rpm) (0.01 dps sensitivity)
 // Ex: (int 321, int 8880) >>> Motor1, Speed:1480 rpm (MAX)
 bool RMD::send_speed(int id, int speed){
-    //printf(" ID: %d\n", id);
-    //printf(" Speed: %d\n", speed);
-    //printf(" ----------------------- \n");
-    int2byte(6*speed);
-    
+    int2byte(6*speed); 
     CANsent[0] = 0xA2;
     CANsent[1] = 0x00;
     CANsent[2] = 0x00;
@@ -94,10 +78,7 @@
     CANsent[5] = CANdata[2];
     CANsent[6] = CANdata[1];
     CANsent[7] = CANdata[0];
-    //CANsent = {0xA2, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], CANdata[2], CANdata[3]};
-    
     if (write(CANMessage(id, CANsent)) == true) {
-        
         return true;
         }
     else {
@@ -105,10 +86,10 @@
         }
 }
 
+// Drive position. Output is degree vary from (-8880 dps, +8880 dps) (-1480 rpm , +1480 rpm) (0.01 dps sensitivity)
+// Ex: (int 321, int 8880) >>> Motor1, Speed:1480 rpm (MAX)
 bool RMD::send_position(int id, int position, int speed){
-
     int2byte2(position, 6*speed);
-    
     CANsent[0] = 0xA4;
     CANsent[1] = 0x00;
     CANsent[2] = CANdata[0];