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:
- 20:0756a01f7415
- Parent:
- 19:51558e307b28
--- 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];