File content as of revision 0:79ed3a6c9ccc:

#include "mbed.h"
#include "rtos.h"
#include "can.h"

float current;
int vel;
int mode;
float rSpeed;
float mSpeed;
char data[4],address[2],data_size,cmd;

void neco(void) {

}
CANMessage canmsg;

CANbus::CANbus(PinName CANpin1, PinName CANpin2) : Can(CANpin1,CANpin2) {
    Can.frequency(500000);
    Can.attach(this,&CANbus::CAN_frame_received);
}

//vysilani synchronizace
void CANbus::servo_sync(void) {
    Can.write(CANMessage(0x80));
}

int CANbus::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;
}

//posila SDO zadost
int CANbus::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;
}

//posila PDO s zadanou hodnotou rychlosti
int CANbus::send_PDO_speed(int velocity) {
    char data_send[6];
    int vel = (int)(velocity * 286331.153);
    data_send[0] = 0xFF & vel;
    data_send[1] = (0xFF00 & vel) >> 8; 
    data_send[2] = (0xFF0000 & vel) >> 16;
    data_send[3] = (0xFF000000 & vel) >> 24;
    data_send[4] = 0x00;
    data_send[5] = 0x00;
    if(Can.write(CANMessage(PDO_RECEIVE_ID,data_send,6)))
        return 1;
    else
        return 0;
}

//posila PDO s zadanou hodnotou proudu
int CANbus::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;
}

//posila SDO zadost na vycteni rychlosti
void CANbus::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)));
}

//vysila SDO s zadosti na vycteni modu
void CANbus::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)));
}

//vysila SDO s zadosti o vycteni proudu do motoru
void CANbus::actual_current() {
    char data_send[8];
    data_send[0] = CMD_SDO_READ_REQ;
    data_send[1] = 0xA8;
    data_send[2] = 0x01;
    data_send[3] = 0x1;
    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)));

}

//obsluha preruseni CANu
void CANbus::CAN_frame_received(void) {
    char curr[2];
    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)){
                rSpeed = (((float)get_data(data, 0x02)/286331.153)*0.10472)/7.7;
                //mSpeed = ((float)get_data(data, 0x02)/286331.153);
                }
            if((address[0] == 0xEF) && (address[1] == 0x1))
                mode = get_data(data, 0x02);
            break;
            
        case PDO_TRANSMIT_ID: /*plati pro rychlostni a momentove rizeni*/
            curr[0] = canmsg.data[5];
            curr[1] = canmsg.data[6];
            current = (float)((short int)get_data(curr,1)/1434.816);
            break;
        default: 
            break;
    }
}

void CANbus::enable_servo() {
    send_SDO_request(CMD_SDO_WRITE_REQ_2,MASTER_CMD,1,REQ_ENABLE_SERVO);
}

void CANbus::disable_servo() {
    send_SDO_request(CMD_SDO_WRITE_REQ_2,MASTER_CMD,1,REQ_DISABLE_SERVO);
}

void CANbus::set_mode() {
    send_SDO_request(CMD_SDO_WRITE_REQ_4,SERVO_MODE,2,MOMENT_MODE);
}

void CANbus::set_current(float cur) {
    send_PDO_current(cur);    
}