RMD-X Motor Library - Last Update V0.5 - On going...... Current available Function - read velocity - read all data - send velocity - send position
RMD.cpp
- Committer:
- sornpraram
- Date:
- 2020-11-04
- Revision:
- 7:2f2c3f244208
- Parent:
- 5:07e41b6c6692
- Child:
- 8:82b39d6566c2
File content as of revision 7:2f2c3f244208:
#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 fv = (((uint16_t)msg.data[5] << 8) + ((uint16_t)msg.data[4])) / ((uint16_t)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_position(){ if (read(msg)){ int fv = (((uint16_t)msg.data[7] << 8) + ((uint16_t)msg.data[6])) / ((uint16_t)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"); } } 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; } } */