DP
Dependencies: FastAnalogIn mbed-rtos mbed
Fork of dipl_prace_v10 by
can.cpp
- Committer:
- romankrej
- Date:
- 2015-04-26
- Revision:
- 0:f3b355df6f26
File content as of revision 0:f3b355df6f26:
#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; }