File content as of revision 1:28d74f044818:

#include "can.h"
#include "threads.h"

CANMessage canmsg;

/*
 * Constructor
*/
cCan::cCan(PinName CANpin1, PinName CANpin2) : Can(CANpin1,CANpin2) {
    
    Can.frequency(CAN_FREQ);
    Can.attach(this,&cCan::CAN_frame_received);
    cCan::setMode(MOMENT_MODE);
    cCan::enableServo();
    cCan::nullResolver();
}


/* 
 * This method sends synchronization frame
 */
void cCan::sync(void) {
    Can.write(CANMessage(0x80)); 
}


/*
* This method convert bytes to data
*/
int cCan::get_data(char data[], char size) {
    int ret_data = 0;
    for(int i = 0; i < 2*size; i++) {
        ret_data |= data[i] << 8*i; 
    }
    return ret_data;
}

/*
* This method seds SDO request to servoamplifier
*/
int cCan::send_SDO_request(char cmd, int adr, char size,int data) {
    char data_send[8];
    data_send[0] = cmd;
    data_send[1] = 0xFF & adr;;
    data_send[2] = (0xFF00 & adr) >> 8;
    data_send[3] = size;
    data_send[4] = 0xFF & data;
    data_send[5] = (0xFF00 & data) >> 8;
    data_send[6] = (0xFF0000 & data)>>16;
    data_send[7] = (0xFF000000 & data)>>24;
    if(Can.write(CANMessage(SDO_REQUEST_ID,data_send,8)))
        return 1;
    else
        return 0;
}


/*
* This method sends PDO with required current
*/
int cCan::send_PDO_current(float curr) {
    char data_send[6];
    int cur = (int)(-curr*1434.816);
    data_send[0] = 0xFF & cur;
    data_send[1] = (0xFF00 & cur) >> 8; 
    data_send[2] = 0x00;
    data_send[3] = 0x00;
    data_send[4] = 0x00;
    data_send[5] = 0x00;
    if(Can.write(CANMessage(PDO_RECEIVE_ID,data_send,6))) {
        return 1;
    }
    else {
        
        return 0;
    }
}

/*
* This method sends SDO request of actual speed
*/
void cCan::actual_speed() {
    char data_send[8];
    data_send[0] = CMD_SDO_READ_REQ;
    data_send[1] = 0x9A;
    data_send[2] = 0x01;
    data_send[3] = 0x2;
    data_send[4] = 0x0;
    data_send[5] = 0x0;
    data_send[6] = 0x0;
    data_send[7] = 0x0;
    
    if(Can.write(CANMessage(SDO_REQUEST_ID ,data_send,8)));
}

/*
* This method send SDO request of mode of servo
*/
void cCan::get_mode() {
    char data_send[8];
    data_send[0] = CMD_SDO_READ_REQ;
    data_send[1] = 0xEF;
    data_send[2] = 0x01;
    data_send[3] = 0x2;
    data_send[4] = 0x0;
    data_send[5] = 0x0;
    data_send[6] = 0x0;
    data_send[7] = 0x0;
    
    if(Can.write(CANMessage(SDO_REQUEST_ID ,data_send,8)));
}

/*
* This method is function when CAN frame received
*/
void cCan::CAN_frame_received(void) {
    char curr[2];
    char pos[4] = {0,0,0,0};
    Can.read(canmsg);
    switch(canmsg.id) {
        
        case SDO_RESPONSE_ID: 
            cmd = canmsg.data[0];
            address[0] = canmsg.data[1];
            address[1] = canmsg.data[2];
            data_size = canmsg.data[3];
            data[0] = canmsg.data[4];
            data[1] = canmsg.data[5];
            data[2] = canmsg.data[6];
            data[3] = canmsg.data[7];
            
            if((address[0] == 0x9A) && (address[1] == 0x1)){
                omega = (((float)get_data(data, 0x02)/286331.153)*-0.10472)/7.7;
                thread->signal_set(0x03);
            }
            
            if((address[0] == 0xEF) && (address[1] == 0x1))
                mode = get_data(data, 0x02);
            break;
            
        case PDO_TRANSMIT_ID: 

            pos[0] = canmsg.data[1];
            pos[1] = canmsg.data[2];
            pos[2] = canmsg.data[3];
            pos[3] = canmsg.data[4];
            curr[0] = canmsg.data[5];
            curr[1] = canmsg.data[6];
            
            current = (float)(short int)get_data(curr,1)*(-0.000697);
            phi = (float)(get_data(pos,2))*(-0.000012448);
            thread->signal_set(0x02);
            break;
        
        default: 
            break;
    }
}

/*
* For enable servo
*/
void cCan::enableServo() {
    send_SDO_request(CMD_SDO_WRITE_REQ_2,MASTER_CMD,1,REQ_ENABLE_SERVO);
}

/*
* For disable servo
*/
void cCan::disableServo() {
    send_SDO_request(CMD_SDO_WRITE_REQ_2,MASTER_CMD,1,REQ_DISABLE_SERVO);
}

/*
* For null resolver
*/
void cCan::nullResolver() {
    send_SDO_request(CMD_SDO_WRITE_REQ_2,MASTER_CMD,1,REQ_NULL_RES);
}

/*
* For set mode of servo
*/
void cCan::setMode(int req_mode) {
    send_SDO_request(CMD_SDO_WRITE_REQ_4,SERVO_MODE,2,req_mode);
}

/*
* For setting required current
*/
void cCan::setCurrent(float cur) {
    send_PDO_current(cur);
}

/*
*Get angular position of wheel
*/
float cCan::getPhi() {
    return phi;
}

/*
* Get angular velocity of wheel 
*/
float cCan::getOmega() {
    return omega;
}