can.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "can.h"
00004 
00005 float current;
00006 int vel;
00007 int mode;
00008 float rSpeed;
00009 float mSpeed;
00010 char data[4],address[2],data_size,cmd;
00011 
00012 void neco(void) {
00013 
00014 }
00015 CANMessage canmsg;
00016 
00017 CANbus::CANbus(PinName CANpin1, PinName CANpin2) : Can(CANpin1,CANpin2) {
00018     Can.frequency(500000);
00019     Can.attach(this,&CANbus::CAN_frame_received);
00020 }
00021 
00022 //vysilani synchronizace
00023 void CANbus::servo_sync(void) {
00024     Can.write(CANMessage(0x80));
00025 }
00026 
00027 int CANbus::get_data(char data[], char size) {
00028     int ret_data = 0;
00029     for(int i = 0; i < 2*size; i++) {
00030         ret_data |= data[i] << 8*i; 
00031     }
00032     return ret_data;
00033 }
00034 
00035 //posila SDO zadost
00036 int CANbus::send_SDO_request(char cmd, int adr, char size,int data) {
00037     char data_send[8];
00038     data_send[0] = cmd;
00039     data_send[1] = 0xFF & adr;;
00040     data_send[2] = (0xFF00 & adr) >> 8;
00041     data_send[3] = size;
00042     data_send[4] = 0xFF & data;
00043     data_send[5] = (0xFF00 & data) >> 8;
00044     data_send[6] = (0xFF0000 & data)>>16;
00045     data_send[7] = (0xFF000000 & data)>>24;
00046     if(Can.write(CANMessage(SDO_REQUEST_ID,data_send,8)))
00047         return 1;
00048     else
00049         return 0;
00050 }
00051 
00052 //posila PDO s zadanou hodnotou rychlosti
00053 int CANbus::send_PDO_speed(int velocity) {
00054     char data_send[6];
00055     int vel = (int)(velocity * 286331.153);
00056     data_send[0] = 0xFF & vel;
00057     data_send[1] = (0xFF00 & vel) >> 8; 
00058     data_send[2] = (0xFF0000 & vel) >> 16;
00059     data_send[3] = (0xFF000000 & vel) >> 24;
00060     data_send[4] = 0x00;
00061     data_send[5] = 0x00;
00062     if(Can.write(CANMessage(PDO_RECEIVE_ID,data_send,6)))
00063         return 1;
00064     else
00065         return 0;
00066 }
00067 
00068 //posila PDO s zadanou hodnotou proudu
00069 int CANbus::send_PDO_current(float curr) {
00070     char data_send[6];
00071     int cur = (int)(curr*1434.816);
00072     data_send[0] = 0xFF & cur;
00073     data_send[1] = (0xFF00 & cur) >> 8; 
00074     data_send[2] = 0x00;
00075     data_send[3] = 0x00;
00076     data_send[4] = 0x00;
00077     data_send[5] = 0x00;
00078     if(Can.write(CANMessage(PDO_RECEIVE_ID,data_send,6)))
00079         return 1;
00080     else
00081         return 0;
00082 }
00083 
00084 //posila SDO zadost na vycteni rychlosti
00085 void CANbus::actual_speed() {
00086     char data_send[8];
00087     data_send[0] = CMD_SDO_READ_REQ;
00088     data_send[1] = 0x9A;
00089     data_send[2] = 0x01;
00090     data_send[3] = 0x2;
00091     data_send[4] = 0x0;
00092     data_send[5] = 0x0;
00093     data_send[6] = 0x0;
00094     data_send[7] = 0x0;
00095     
00096     if(Can.write(CANMessage(SDO_REQUEST_ID ,data_send,8)));
00097 }
00098 
00099 //vysila SDO s zadosti na vycteni modu
00100 void CANbus::get_mode() {
00101     char data_send[8];
00102     data_send[0] = CMD_SDO_READ_REQ;
00103     data_send[1] = 0xEF;
00104     data_send[2] = 0x01;
00105     data_send[3] = 0x2;
00106     data_send[4] = 0x0;
00107     data_send[5] = 0x0;
00108     data_send[6] = 0x0;
00109     data_send[7] = 0x0;
00110     
00111     if(Can.write(CANMessage(SDO_REQUEST_ID ,data_send,8)));
00112 }
00113 
00114 //vysila SDO s zadosti o vycteni proudu do motoru
00115 void CANbus::actual_current() {
00116     char data_send[8];
00117     data_send[0] = CMD_SDO_READ_REQ;
00118     data_send[1] = 0xA8;
00119     data_send[2] = 0x01;
00120     data_send[3] = 0x1;
00121     data_send[4] = 0x0;
00122     data_send[5] = 0x0;
00123     data_send[6] = 0x0;
00124     data_send[7] = 0x0;
00125     
00126     if(Can.write(CANMessage(SDO_REQUEST_ID ,data_send,8)));
00127 
00128 }
00129 
00130 //obsluha preruseni CANu
00131 void CANbus::CAN_frame_received(void) {
00132     char curr[2];
00133     Can.read(canmsg);
00134     switch(canmsg.id) {
00135         case SDO_RESPONSE_ID: 
00136             cmd = canmsg.data[0];
00137             address[0] = canmsg.data[1];
00138             address[1] = canmsg.data[2];
00139             data_size = canmsg.data[3];
00140             data[0] = canmsg.data[4];
00141             data[1] = canmsg.data[5];
00142             data[2] = canmsg.data[6];
00143             data[3] = canmsg.data[7];
00144             if((address[0] == 0x9A) && (address[1] == 0x1)){
00145                 rSpeed = (((float)get_data(data, 0x02)/286331.153)*0.10472)/7.7;
00146                 //mSpeed = ((float)get_data(data, 0x02)/286331.153);
00147                 }
00148             if((address[0] == 0xEF) && (address[1] == 0x1))
00149                 mode = get_data(data, 0x02);
00150             break;
00151             
00152         case PDO_TRANSMIT_ID: /*plati pro rychlostni a momentove rizeni*/
00153             curr[0] = canmsg.data[5];
00154             curr[1] = canmsg.data[6];
00155             current = (float)((short int)get_data(curr,1)/1434.816);
00156             break;
00157         default: 
00158             break;
00159     }
00160 }
00161 
00162 void CANbus::enable_servo() {
00163     send_SDO_request(CMD_SDO_WRITE_REQ_2,MASTER_CMD,1,REQ_ENABLE_SERVO);
00164 }
00165 
00166 void CANbus::disable_servo() {
00167     send_SDO_request(CMD_SDO_WRITE_REQ_2,MASTER_CMD,1,REQ_DISABLE_SERVO);
00168 }
00169 
00170 void CANbus::set_mode() {
00171     send_SDO_request(CMD_SDO_WRITE_REQ_4,SERVO_MODE,2,MOMENT_MODE);
00172 }
00173 
00174 void CANbus::set_current(float cur) {
00175     send_PDO_current(cur);    
00176 }