File content as of revision 13:09e4446b41cc:

#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);

}

int 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");  
        }
    return fv

}

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");  
        }

}

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::int2byte2(int pos, int speed){

    CANdata[0] = speed & 0xFF;
    CANdata[1] = (speed >> 8) & 0xFF;
    CANdata[2] = pos & 0xFF;
    CANdata[3] = (pos >> 8) & 0xFF;
    CANdata[4] = (pos >> 16) & 0xFF;
    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)
// 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;
        }
}

bool RMD::send_position(int id, int position, int speed){

    int2byte2(position, 6*speed);
    
    CANsent[0] = 0xA4;
    CANsent[1] = 0x00;
    CANsent[2] = CANdata[0];
    CANsent[3] = CANdata[1];
    CANsent[4] = CANdata[2];
    CANsent[5] = CANdata[3];
    CANsent[6] = CANdata[4];
    CANsent[7] = CANdata[5];
    //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;
        }
}

*/