Diff: Daan_Test1_maxon.cpp

Revision:

2:1117ce9d1867

Parent:

1:822c609fb2ce

--- a/Daan_Test1_maxon.cpp	Fri Apr 05 12:12:24 2019 +0000
+++ b/Daan_Test1_maxon.cpp	Wed Apr 10 09:33:36 2019 +0000
@@ -6,23 +6,34 @@
 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
+//struct EPOS_MESSAGES EPOS_MSGS;
+
+/* -----------------------------------------------------------------------------
+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.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.
+            messageOut.data[i] = DATA[i];                                       // repeat for each byte.
         }
     
-        if(can.write(messageOut)) {
+        if(can.write(messageOut)) {                                             //can.write() gives a 1 or 0 as output
             #ifdef EPOS_DEBUG
-            pc.printf("sending message : ");
+            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("%02x ",messageOut.data[ii]);                         
             }
             pc.printf("\r\n");
             #endif
@@ -30,96 +41,222 @@
         } 
         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;
+//    }
     
-    //Maken van alle CAN messages 
-    CANMessage EPOS::Shutdown(){
+CANMessage EPOS::Shutdown(){
     return create_CAN_msg(0x600+NODE_ID,8,(char*)Shutdown_Data);
     }
     
-    CANMessage EPOS::SwitchOnAndEnable(){
+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::StatusWord(){
+    return create_CAN_msg(0x600 + NODE_ID,4,(char*)Status_Word_Data);
     }
     
-    CANMessage EPOS::Homing_Mode(){
+CANMessage EPOS::HomingMode(){
     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::HomingMethodPositive(){
+    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::HomingMethodNegative(){
+    return create_CAN_msg(0x600+NODE_ID,8,(char*)Homing_Method_Data_Negative);
     }
     
-    CANMessage EPOS::StartHoming(){
+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);
+    }
     
-    void EPOS::Homing(){
+CANMessage EPOS::PositionMode(){
+    return create_CAN_msg(0x600+NODE_ID,8,(char*)Position_Mode_Data);
+    }
+    
+CANMessage EPOS::GoToPosition(int quartercircles){
+    
+    CANMessage msg;
+ 
+    msg.format = CANStandard;  // standard or extended ID (can be skipped for standard)
+    msg.id = 0x600 + NODE_ID;
+    msg.len = 8;
     
-        int foo=0;                      
+    /* -------------------------------------------------------------------------
+    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;
     
-        foo += can.write(Homing_Mode());
-        Thread::wait(50);
-
-        foo += can.write(Homing_Method_positive());
-        Thread::wait(50);
+    /* -------------------------------------------------------------------------
+    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(Shutdown());      
-        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());
+    foo += can.write(HomingMethodPositive());
+    Thread::wait(50);
+    pc.printf("Waarde van foo is: %i \r\n", foo);
         
-            Thread::wait(100);
+    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(200);
+         
+    for (int k=0; k<100; 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;
-            }
-            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;
-            }
+            
+        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;
+        }        
         
-            iii++;
-            Thread::wait(100);
-        }
+        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;
     
-        #ifdef EPOS_DEBUG
-        printf("\n Homing NODE(%i) timed out \r\n Resetting Device\r\n",NODE_ID);
-        #endif
+    tel += can.write(ClearFault());
+    Thread::wait(50);
+    pc.printf("Waarde van tel is: %i \r\n", tel);
     
-        //can.write(ReSet1());
-        //can.write(ReSet2());
-        //EPOS_HOME[NODE_ID-1]=0;;
-    } 
+    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);
+           
+    float quadc;
+    quadc = (int)(-300000);                                                     //determine the quadcounts for EPOS
+    can.write(GoToPosition(quadc));                                             //send new position to the controllers
+    Thread::wait(100);
     
-       
\ No newline at end of file
+}  
+                                                                                //end of StartPositionMode
\ No newline at end of file