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:
- 10:5221a88e2240
- Parent:
- 9:defd39d3fdf1
- Child:
- 11:9ba7b854016f
diff -r defd39d3fdf1 -r 5221a88e2240 RMD.cpp --- 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