File content as of revision 0:785972ab5892:

#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;;
    }