Diff: Daan_Test1_maxon.cpp

Revision:

0:785972ab5892

Child:

1:822c609fb2ce

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Daan_Test1_maxon.cpp	Thu Apr 04 10:08:08 2019 +0000
@@ -0,0 +1,125 @@
+#include "mbed.h"
+#include "Daan_Test1_maxon.h"
+#include "rtos.h"
+#define EPOS_DEBUG
+
+extern RawSerial pc;
+extern CAN can;
+
+CANMessage create_CAN_msg(int COB_ID, int LENGTH, char * DATA){             //Je opent hier de functie create_CAN_msg
+        // create the message
+        CANMessage messageOut;         //MessageOut is gekoppeld aan CAN message en ga je hieronder vullen
+ 
+        messageOut.format = CANStandard;  // standard or extended ID (can be skipped for standard)
+        messageOut.id = COB_ID;
+        messageOut.len = LENGTH;
+    
+        for ( int i = 0 ; i<LENGTH ; i++) {
+            messageOut.data[i] = DATA[i]; // repeat for each byte.
+        }
+    
+        if(can.write(messageOut)) {
+            #ifdef EPOS_DEBUG
+            pc.printf("sending message : ");
+            for ( int ii = 0 ; ii<LENGTH ; ii++) {
+                pc.printf("%02x ",messageOut.data[ii]);
+            }
+            pc.printf("\r\n");
+            #endif
+            return 1;
+        } 
+        return messageOut;
+    }
+    
+    //Maken van alle CAN messages 
+    CANMessage EPOS::Shutdown(){
+    return create_CAN_msg(0x600+NODE_ID,8,(char*)Shutdown_Data);
+    }
+    
+    CANMessage EPOS::SwitchOnAndEnable(){
+    return create_CAN_msg(0x600+NODE_ID,8,(char*)Switch_On_And_Enable_Data);
+    }
+        
+    CANMessage EPOS::StatusWord(){
+    return create_CAN_msg(0x600 + NODE_ID,4,(char*)StatusWord_Data);
+    }
+    
+    CANMessage EPOS::Homing_Mode(){
+    return create_CAN_msg(0x600+NODE_ID,8,(char*)Homing_Mode_Data);
+    }
+    
+    CANMessage EPOS::Homing_Method_positive(){
+    return create_CAN_msg(0x600+NODE_ID,8,(char*)Homing_Method_Data_positive);
+    }
+    
+    CANMessage EPOS::Homing_Method_negative(){
+    return create_CAN_msg(0x600+NODE_ID,8,(char*)Homing_Method_Data_negative);
+    }
+    
+    CANMessage EPOS::StartHoming(){
+    return create_CAN_msg(0x600+NODE_ID,8,(char*)Start_Homing_Data);
+    }
+    
+    
+    void EPOS::Homing(){
+    
+        int foo=0;                        //foo is leeg en daar worden alle messages die hieronder vermeld staan aan toegevoegd
+    
+        foo += can.write(Homing_Mode());
+        Thread::wait(50);
+
+        foo += can.write(Homing_Method_positive());
+        Thread::wait(50);
+        
+        foo += can.write(Shutdown());       //op de can schrijven wat er in shutdown staat
+        Thread::wait(50);
+
+        foo += can.write(SwitchOnAndEnable());
+        Thread::wait(50);
+ 
+        foo += can.write(StartHoming());
+    
+        CANMessage msg;
+    
+        Thread::wait(150);
+    
+        int iii=0;
+        while(iii<100){
+        
+            #ifdef EPOS_DEBUG
+            pc.printf("\n Checking if homing NODE(%i) finished attempt :(%d) \r\n",NODE_ID,iii);
+            #endif
+        
+            can.write(StatusWord());
+        
+            Thread::wait(100);
+        
+           if (msg.data[5] == 0x95){ // case home found
+                #ifdef EPOS_DEBUG
+                pc.printf("Homeing NODE(%i) Succesfull!! \r\n",NODE_ID);
+                #endif
+                //EPOS_HOME[NODE_ID-1]=1;
+                return;
+            }
+            else if ( !(msg.data[4] == 0x37)){ // case homing failed
+                #ifdef EPOS_DEBUG
+                pc.printf("Homeing NODE(%i) FAILED!! \r\n",NODE_ID);
+                #endif
+                //EPOS_HOME[NODE_ID-1]=0;
+                return;
+            }
+        
+            iii++;
+            Thread::wait(100);
+        }
+    
+        #ifdef EPOS_DEBUG
+        printf("\n Homing NODE(%i) timed out \r\n Resetting Device\r\n",NODE_ID);
+        #endif
+    
+        //can.write(ReSet1());
+        //can.write(ReSet2());
+        //EPOS_HOME[NODE_ID-1]=0;;
+    } 
+    
+       
\ No newline at end of file