File content as of revision 1:3d46a0660d05:

#include "mbed.h"
#include "Daan_Test1_maxon.h"
#include "rtos.h"
#define EPOS_DEBUG

extern RawSerial pc;
extern CAN can;



/* -----------------------------------------------------------------------------
You are going to open the function create_CAN_message. In this function, a 
message is made that consists of the right NODE_ID, Length and the data. 
This data comes from the data that is written in the .h file. 
MessageOut is a CAN message that is part of the funtion create_CAN_msg and 
will be filled with the right data. 
can.write is a function from mbed and gives a 0 or 1 as output:
    0 when nothing is written on the CAN line
    1 when a message is written on the CAN line
----------------------------------------------------------------------------- */
CANMessage create_CAN_msg(int COB_ID, int LENGTH, char * DATA){             
        CANMessage messageOut;                                                  
 
        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)) {                                             //can.write() gives a 1 or 0 as output
            #ifdef EPOS_DEBUG
            pc.printf("sending message : ");                                    //output of can.write is 1: print something
            for ( int ii = 0 ; ii<LENGTH ; ii++) {
                pc.printf("%02x ",messageOut.data[ii]);                         
            }
            pc.printf("\r\n");
            #endif
            return 1;
        } 
        return messageOut;
    }

/* -----------------------------------------------------------------------------
Make all messages that make use of the funtion CANMessage and return the 
messages. 
----------------------------------------------------------------------------- */     
//CANMessage Latest_EPOS_msg(int NODE_ID){
//    return EPOS_MSGS.msg_1;
//    }
    
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*)Status_Word_Data);
    }
    
CANMessage EPOS::HomingMode(){
    return create_CAN_msg(0x600+NODE_ID,8,(char*)Homing_Mode_Data);
    }
    
CANMessage EPOS::HomingMethodPositive(){
    return create_CAN_msg(0x600+NODE_ID,8,(char*)Homing_Method_Data_Positive);
    }
    
CANMessage EPOS::HomingMethodNegative(){
    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);
    }
    
CANMessage EPOS::ClearFault(){
    return create_CAN_msg(0x600+NODE_ID,8,(char*)Clear_Fault_Data);
    }
    
CANMessage EPOS::PositionMode(){
    return create_CAN_msg(0x600+NODE_ID,8,(char*)Position_Mode_Data);
    }
    
CANMessage EPOS::GoToPosition(float quartercircles){
    
    CANMessage msg;
 
    msg.format = CANStandard;  // standard or extended ID (can be skipped for standard)
    msg.id = 0x600 + NODE_ID;
    msg.len = 8;
    
    /* -------------------------------------------------------------------------
    Unions allow one portion of memory to be accessed as different data types. 
    Its declaration and use is similar to the one of structures, but its 
    functionality is totally different.
    ------------------------------------------------------------------------- */
    union {
        int integer;
        unsigned char byte[4];
    } QC;
    
    QC.integer = quartercircles;
    
    /* -------------------------------------------------------------------------
    The data that is sent to determine how many quartercircles have to be turned, 
    consists of 4 bytes -> byte 0 = 0x23.
    The Position Mode Setting Value has object 0x2062-00(byte1=0x62, byte2=0x20,
    byte3=0x00).
    The data is put in byte 4, 5, 6 and 7 and comes from the union Quaters_Union
    ------------------------------------------------------------------------- */
    msg.data[0] = 0x23;                                                         //0x23 as the first byte: 4 bytes in total sent
    msg.data[1] = 0x62;                                                           
    msg.data[2] = 0x20;
    msg.data[3] = 0x00;
    msg.data[4] = QC.byte[0];
    msg.data[5] = QC.byte[1];
    msg.data[6] = QC.byte[2];
    msg.data[7] = QC.byte[3];   
    
    return msg;
}
    
/* -----------------------------------------------------------------------------
With the HOMING function you can home the motor. There are 2 differnt sorts of 
homing: 
    - 0xFD: -3: Current treshold positive speed (first in the direction of 
    the motor and then to the end to go to the defined position).
    - 0xFC: -4: Current treshold negative speed (this is what we are going 
    to use in the real boat: first to the end of then back in the direction 
    to the motor).
For homing different steps have to be executed in the right order. Foo is used 
as a counter to check if all the steps are executed. When can.write is 
succesfull (when there is a message on the CAN line) foo increases with 1. To 
check if all the messages are corrrect, we check it with pc.printf. In tera term
you do not always get a message if homing is succesfull or failed because the 
4th byte is already 0, but the 5th is 81 so you are not in the situations where
the output is succes or failed. 
    
can.read? 
----------------------------------------------------------------------------- */ 
void EPOS::Homing(){
    
    int foo = 0;                                                              //foo is een teller, als can.write lukt komt er een 1 uit en als het faalt een 0. Uiteindelijk moet foo in dit geval dus 5 zijn. 
    
    foo += can.write(ClearFault());
    Thread::wait(50);
    pc.printf("Waarde van foo is: %i \r\n", foo);
    
    foo += can.write(HomingMode());        
    Thread::wait(50);
    pc.printf("Waarde van foo is: %i \r\n", foo);
        
    foo += can.write(HomingMethodPositive());
    Thread::wait(50);
    pc.printf("Waarde van foo is: %i \r\n", foo);
        
    foo += can.write(Shutdown());      
    Thread::wait(50);
    pc.printf("Waarde van foo is: %i \r\n", foo);

    foo += can.write(SwitchOnAndEnable());
    Thread::wait(50);
    pc.printf("Waarde van foo is: %i \r\n", foo);
 
    foo += can.write(StartHoming());
    Thread::wait(50);
    pc.printf("Waarde van foo is: %i \r\n", foo);
        
    CANMessage msg;                                                         //msg nog vullen
        
    Thread::wait(250);
         
    for (int k=0; k<400; k++){                                              //while(iii<100){
        #ifdef EPOS_DEBUG
        pc.printf("Checking if homing NODE(%i) finished attempt :(%d) \r\n",NODE_ID,k);
        #endif
        
        int sw=0;
        sw +=can.write(StatusWord());
        pc.printf("Waarde van sw is: %i \r\n", sw);
            
        Thread::wait(100);
            
        //msg = Latest_EPOS_msg(NODE_ID);                                         //the NODE_ID is only usefull when you have more messages, then you can use a switch on the basis of the node_id. 
  
        int teller = 0;
        teller += can.read(msg);                                            //returns 0 if no message, 1 if message
        pc.printf("Waarde van teller is: %i \r\n", teller);
        
          
        //pc.printf("%02x \r\n",msg.data[1]);     //printen van de messages die je verstuurd
        //pc.printf("%02x \r\n",msg.data[2]);     //printen van de messages die je verstuurd
        //pc.printf("%02x \r\n",msg.data[4]);     //printen van de messages die je verstuurd
        //pc.printf("%02x \r\n",msg.data[5]);     //printen van de messages die je verstuurd
        
            
        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;                                                           //optional because it is a void?
        }
        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;
        }        
        
        Thread::wait(100);
    }                                                                           //end for statement
    
    #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;
    
/* -----------------------------------------------------------------------------
In this boolean funtion, the position is set and the axis is moving to the new 
absolute position with the maximum acceleration and maximum velocity without a 
particular traject. 
With tel we built in a test to see if all the can messages are written on the 
CAN line. 
----------------------------------------------------------------------------- */

void EPOS::StartPositionMode(){

    int tel=0;
    
    tel += can.write(ClearFault());
    Thread::wait(50);
    pc.printf("Waarde van tel is: %i \r\n", tel);
    
    tel += can.write(PositionMode());
    Thread::wait(50);
    pc.printf("Waarde van tel is: %i \r\n", tel);
        
    tel += can.write(Shutdown());      
    Thread::wait(50);
    pc.printf("Waarde van tel is: %i \r\n", tel);

    tel += can.write(SwitchOnAndEnable());
    Thread::wait(50);
    pc.printf("Waarde van tel is: %i \r\n", tel);
           
    //can.write(GoToPosition(quartercircles));                                             //send new position to the controllers
    //Thread::wait(100);    
}  
                                                                                //end of StartPositionMode