sornpraram xu / RMD
Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers RMD.cpp Source File

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 */