Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
RMD.cpp
00001 #include "RMD.h" 00002 #include "mbed.h" 00003 #include <iostream> 00004 #include <bitset> 00005 00006 #define DEVICE_CAN_ID_1 0x141 00007 00008 // CAN BUS connection 00009 RMD::RMD(PinName rx, PinName tx) : CAN(rx,tx,1000000) { 00010 frequency(1000000); 00011 00012 } 00013 00014 // Reading current velocity output (degree per sec) 00015 int RMD::status_velocity(){ 00016 if (read(msg)){ 00017 00018 int fv = (((uint16_t)msg.data[5] << 8) + ((uint16_t)msg.data[4])) / ((uint16_t)6); 00019 return fv; 00020 } 00021 else (){ 00022 return 0; 00023 } 00024 } 00025 00026 // Reading current position output (degree) 00027 void RMD::status_position(){ 00028 if (read(msg)){ 00029 00030 int fv = (((uint16_t)msg.data[7] << 8) + ((uint16_t)msg.data[6])); 00031 return fv; 00032 } 00033 } 00034 00035 // Reading everything 00036 void RMD::status(){ 00037 if (read(msg)){ 00038 printf("Message received: %d\n", msg.id); 00039 printf(" Length: %d\n", msg.len); 00040 printf(" Command: %2x\n", msg.data[0]); 00041 printf(" Motor temp: %2x\n", msg.data[1]); // Null 00042 printf(" Torque current low byte: %2x\n", msg.data[2]); 00043 printf(" Torque current high byte: %2x\n", msg.data[3]); 00044 printf(" Speed low byte: %2x\n", msg.data[4]); 00045 printf(" Speed high byte: %2x\n", msg.data[5]); 00046 printf(" Position low byte: %2x\n", msg.data[6]); 00047 printf(" Position high byte: %2x\n", msg.data[7]); 00048 } 00049 } 00050 00051 // Helper function for convert int to byte (used inside library only) 00052 void RMD::int2byte(int num){ 00053 CANdata[0] = (num >> 24) & 0xFF; 00054 CANdata[1] = (num >> 16) & 0xFF; 00055 CANdata[2] = (num >> 8) & 0xFF; 00056 CANdata[3] = num & 0xFF; 00057 } 00058 00059 // Helper function for convert int to byte (used inside library only) 00060 void RMD::int2byte2(int pos, int speed){ 00061 CANdata[0] = speed & 0xFF; 00062 CANdata[1] = (speed >> 8) & 0xFF; 00063 CANdata[2] = pos & 0xFF; 00064 CANdata[3] = (pos >> 8) & 0xFF; 00065 CANdata[4] = (pos >> 16) & 0xFF; 00066 CANdata[5] = (pos >> 24) & 0xFF; 00067 } 00068 00069 // Drive speed. Output is dps vary from (-8880 dps, +8880 dps) (-1480 rpm , +1480 rpm) (0.01 dps sensitivity) 00070 // Ex: (int 321, int 8880) >>> Motor1, Speed:1480 rpm (MAX) 00071 bool RMD::send_speed(int id, int speed){ 00072 int2byte(6*speed); 00073 CANsent[0] = 0xA2; 00074 CANsent[1] = 0x00; 00075 CANsent[2] = 0x00; 00076 CANsent[3] = 0x00; 00077 CANsent[4] = CANdata[3]; 00078 CANsent[5] = CANdata[2]; 00079 CANsent[6] = CANdata[1]; 00080 CANsent[7] = CANdata[0]; 00081 if (write(CANMessage(id, CANsent)) == true) { 00082 return true; 00083 } 00084 else { 00085 return false; 00086 } 00087 } 00088 00089 // Drive position. Output is degree vary from (-8880 dps, +8880 dps) (-1480 rpm , +1480 rpm) (0.01 dps sensitivity) 00090 // Ex: (int 321, int 8880) >>> Motor1, Speed:1480 rpm (MAX) 00091 bool RMD::send_position(int id, int position, int speed){ 00092 int2byte2(position, 6*speed); 00093 CANsent[0] = 0xA4; 00094 CANsent[1] = 0x00; 00095 CANsent[2] = CANdata[0]; 00096 CANsent[3] = CANdata[1]; 00097 CANsent[4] = CANdata[2]; 00098 CANsent[5] = CANdata[3]; 00099 CANsent[6] = CANdata[4]; 00100 CANsent[7] = CANdata[5]; 00101 //CANsent = {0xA2, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], CANdata[2], CANdata[3]}; 00102 00103 if (write(CANMessage(id, CANsent)) == true) { 00104 00105 return true; 00106 } 00107 else { 00108 return false; 00109 } 00110 } 00111 /* 00112 00113 // Torque Unit is Nm 00114 // Torque Input is current vary from 16bit(−2000, +2000) (0xF830 , 0x07D0) [16bits is (−32768, +32767)] 00115 // Input current mapping to (-32A, 32A) 00116 // Torque Output is Nm vary from () 00117 bool RMD::send_torque(int id, int torque){ 00118 printf(" ID: %d\n", id); 00119 printf(" Torque: %d\n", speed); 00120 printf(" ----------------------- \n"); 00121 int2byte(speed); 00122 00123 CANsent[0] = 0xA1; 00124 CANsent[1] = 0x00; 00125 CANsent[2] = 0x00; 00126 CANsent[3] = 0x00; 00127 CANsent[4] = CANdata[3]; 00128 CANsent[5] = CANdata[2]; 00129 CANsent[6] = 0x00; 00130 CANsent[7] = 0x00; 00131 //CANsent = {0xA1, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], 0x00, 0x00}; 00132 00133 if (write(CANMessage(id, CANsent)) == true) { 00134 return true; 00135 } 00136 else { 00137 return false; 00138 } 00139 } 00140 00141 bool RMD::send_position(int id, int position){ 00142 printf(" ID: %d\n", id); 00143 printf(" Speed: %d\n", speed); 00144 printf(" ----------------------- \n"); 00145 int2byte(speed*6); 00146 00147 CANsent[0] = 0xA3; 00148 CANsent[1] = 0x00; 00149 CANsent[2] = 0x00; 00150 CANsent[3] = 0x00; 00151 CANsent[4] = CANdata[3]; 00152 CANsent[5] = CANdata[2]; 00153 CANsent[6] = CANdata[1]; 00154 CANsent[7] = CANdata[0]; 00155 //CANsent = {0xA3, 0x00, 0x00, 0x00, CANdata[0], CANdata[1], CANdata[2], CANdata[3]}; 00156 00157 if (write(CANMessage(id, CANsent)) == true) { 00158 return true; 00159 } 00160 else { 00161 return false; 00162 } 00163 } 00164 00165 */
Generated on Thu Jul 28 2022 01:37:11 by
