Diff: can.cpp

Revision:

0:f3b355df6f26

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/can.cpp	Sun Apr 26 13:14:02 2015 +0000
@@ -0,0 +1,207 @@
+#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;
+}