Diff: RMD.cpp

Revision:

0:433e4ec5de88

Child:

1:5c31dcbaa375

diff -r 000000000000 -r 433e4ec5de88 RMD.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RMD.cpp	Wed Nov 04 09:20:53 2020 +0000
@@ -0,0 +1,151 @@
+#include "RMD.h"
+#include "mbed.h"
+#include <iostream>
+#include <bitset>
+
+#define DEVICE_CAN_ID_1 0x141
+
+RMD::RMD(PinName rx, PinName tx) : CAN(rx,tx,1000000) {
+    frequency(1000000);
+
+}
+
+void RMD::status_velocity(){
+    if (read(msg)){
+        int v_high = msg.data[5];
+        int v_low = msg.data[4];
+        v_high = v_high << 8 ;
+        int fv = (v_high + v_low)/6;
+        
+        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");  
+        }
+
+}
+
+void RMD::status(){
+    if (read(msg)){
+        printf("Message received: %d\n", msg.id);
+        printf(" Length: %d\n", msg.len);
+        printf(" Command: %2x\n", msg.data[0]);
+        printf(" Motor temp: %2x\n", msg.data[1]); // Null
+        printf(" Torque current low byte: %2x\n", msg.data[2]);
+        printf(" Torque current high byte: %2x\n", msg.data[3]);
+        printf(" Speed low byte: %2x\n", msg.data[4]);
+        printf(" Speed high byte: %2x\n", msg.data[5]);
+        printf(" Position low byte: %2x\n", msg.data[6]);
+        printf(" Position high byte: %2x\n", msg.data[7]);        
+        }
+}
+
+void RMD::int2byte(int num){
+
+    CANdata[0] = (num >> 24) & 0xFF;
+    CANdata[1] = (num >> 16) & 0xFF;
+    CANdata[2] = (num >> 8) & 0xFF;
+    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;
+}
+
+// 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)
+// 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);
+    
+    CANsent[0] = 0xA2;
+    CANsent[1] = 0x00;
+    CANsent[2] = 0x00;
+    CANsent[3] = 0x00;
+    CANsent[4] = CANdata[3];
+    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 {
+        return false;
+        }
+    
+}
+
+/*
+
+// Torque Unit is Nm
+// Torque Input is current vary from 16bit(−2000, +2000) (0xF830 , 0x07D0)  [16bits is (−32768, +32767)]
+// Input current mapping to (-32A, 32A)
+// Torque Output is Nm vary from ()
+bool RMD::send_torque(int id, int torque){
+    printf(" ID: %d\n", id);
+    printf(" Torque: %d\n", speed);
+    printf(" ----------------------- \n");
+    int2byte(speed);
+    
+    CANsent[0] = 0xA1;
+    CANsent[1] = 0x00;
+    CANsent[2] = 0x00;
+    CANsent[3] = 0x00;
+    CANsent[4] = CANdata[3];
+    CANsent[5] = CANdata[2];
+    CANsent[6] = 0x00;
+    CANsent[7] = 0x00;
+    //CANsent = {0xA1, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], 0x00, 0x00};
+    
+    if (write(CANMessage(id, CANsent)) == true) {
+        return true;
+        }
+    else {
+        return false;
+        }
+}
+
+bool RMD::send_position(int id, int position){
+    printf(" ID: %d\n", id);
+    printf(" Speed: %d\n", speed);
+    printf(" ----------------------- \n");
+    int2byte(speed*6);
+    
+    CANsent[0] = 0xA3;
+    CANsent[1] = 0x00;
+    CANsent[2] = 0x00;
+    CANsent[3] = 0x00;
+    CANsent[4] = CANdata[3];
+    CANsent[5] = CANdata[2];
+    CANsent[6] = CANdata[1];
+    CANsent[7] = CANdata[0];
+    //CANsent = {0xA3, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], CANdata[2], CANdata[3]};
+    
+    if (write(CANMessage(id, CANsent)) == true) {
+        return true;
+        }
+    else {
+        return false;
+        }
+}
+
+*/