Diff: RMD.cpp

Revision:

10:5221a88e2240

Parent:

9:defd39d3fdf1

Child:

11:9ba7b854016f

--- a/RMD.cpp	Wed Nov 04 10:12:28 2020 +0000
+++ b/RMD.cpp	Wed Nov 04 10:30:13 2020 +0000
@@ -59,21 +59,15 @@
     CANdata[3] = num & 0xFF;
 }
 
-void RMD::read_pid(){
-    CANsent[0] = 0x30;
-    CANsent[1] = 0x00;
-    CANsent[2] = 0x00;
-    CANsent[3] = 0x00;
-    CANsent[4] = 0x00;
-    CANsent[5] = 0x00;
-    CANsent[6] = 0x00;
-    CANsent[7] = 0x00;
-    //CANsent = {0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
-    write(CANMessage(id, CANsent));
-    //_data_back = _can.read(msg);
-    //return _data_back;
+void RMD::int2byte2(int pos, int speed){
+
+    CANdata[0] = (pos >> 8) & 0xFF;
+    CANdata[1] = pos & 0xFF;
+    CANdata[2] = (speed >> 8) & 0xFF;
+    CANdata[3] = speed & 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)] 
@@ -102,9 +96,30 @@
     else {
         return false;
         }
-    
 }
 
+bool RMD::send_position(int id, int position, int speed){
+
+    int2byte2(position, 6*speed);
+    
+    CANsent[0] = 0xA6;
+    CANsent[1] = 0x00;
+    CANsent[2] = CANdata[3];
+    CANsent[3] = CANdata[2];
+    CANsent[4] = CANdata[1];
+    CANsent[5] = CANdata[0];
+    CANsent[6] = 0x00;
+    CANsent[7] = 0x00;
+    //CANsent = {0xA2, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], CANdata[2], CANdata[3]};
+    
+    if (write(CANMessage(id, CANsent)) == true) {
+        
+        return true;
+        }
+    else {
+        return false;
+        }
+}
 /*
 
 // Torque Unit is Nm