Diff: Daan_Test1_maxon.cpp

Revision:

1:3d46a0660d05

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Daan_Test1_maxon.cpp	Fri Apr 12 09:55:53 2019 +0000
@@ -0,0 +1,260 @@
+#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                                                                              
+                                                                                
\ No newline at end of file