Revision 2:1117ce9d1867, committed 2019-04-10

Comitter:

DanielleKruijver

Date:

Wed Apr 10 09:33:36 2019 +0000

Parent:

1:822c609fb2ce

Commit message:

Maxon motoren homen en daarna 300.000qc draaien

Changed in this revision

--- 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

--- a/Daan_Test1_maxon.h	Fri Apr 05 12:12:24 2019 +0000
+++ b/Daan_Test1_maxon.h	Wed Apr 10 09:33:36 2019 +0000
@@ -2,35 +2,58 @@
 #ifndef EPOSS
 #define EPOSS
 
-// EPOS commands that you need to home, you can find those in chapter 8 from the application node. 
+/* -----------------------------------------------------------------------------
+EPOS commands that you need to home, you can find those in chapter 8.3 from the 
+application node. 
+----------------------------------------------------------------------------- */
 const char Shutdown_Data[8] = {0x2B,0x40,0x60,0x00,0x06,0x00,0x00,0x00};        //voorkomen dat je niet nog in een andere mode zit
     
 const char Switch_On_And_Enable_Data[8] = {0x2B,0x40,0x60,0x00,0x0F,0x00,0x00,0x00}; //enable de motorcontroller (groene ledje constant aan)
 
 const char Homing_Mode_Data[8] = {0x2F,0x60,0x60,0x00,0x06,0x00,0x00,0x00};     //aan de 0x6060-00 geef je aan dat je nu gaat homen wat de waarde 06 heeft in de data bytes
     
-const char Homing_Method_Data_positive[8] = {0x2F,0x98,0x60,0x00,0xFD,0x00,0x00,0x00};  //bij de index 0x6098-00 geef je aan welke homing method je gaat gebruiken: 0xFD = -3 = current treshold positive speed
+const char Homing_Method_Data_Positive[8] = {0x2F,0x98,0x60,0x00,0xFD,0x00,0x00,0x00};  //bij de index 0x6098-00 geef je aan welke homing method je gaat gebruiken: 0xFD = -3 = current treshold positive speed
 
-const char Homing_Method_Data_negative[8] = {0x2F,0x98,0x60,0x00,0xFC,0x00,0x00,0x00};  //bij de index 0x6098-00 geef je aan welke homing method je gaat gebruiken: 0xFC = -4 = current treshold negative speed
+const char Homing_Method_Data_Negative[8] = {0x2F,0x98,0x60,0x00,0xFC,0x00,0x00,0x00};  //bij de index 0x6098-00 geef je aan welke homing method je gaat gebruiken: 0xFC = -4 = current treshold negative speed
     
 const char Start_Homing_Data[8] = {0x2B,0x40,0x60,0x00,0x1F,0x00,0x00,0x00}; //om te beginnen met homen geef je het controlword de waarde 0x001F om het homen te starten 
 
-const char StatusWord_Data[4] = {0x40,0x41,0x60,0x00};  //0x40, want ccs is 2: je upload iets van de controller om te kijken of het gelukt is. 
-    
-/** Sends a CAN Message to the controller
+const char Status_Word_Data[4] = {0x40,0x41,0x60,0x00};  //0x40, want ccs is 2: je upload iets van de controller om te kijken of het gelukt is. 
+
+const char Clear_Fault_Data[8] = {0x2B,0x40,0x60,0x00,0x80,0x00,0x00,0x00}; //Clear fault: 0x0080 versturen naar 0x6040-00    
+
+/* -----------------------------------------------------------------------------
+Additional EPOS commands that you need for going to a position, you can find 
+those in chapter 8.7 from the application node. 
+----------------------------------------------------------------------------- */
+const char Position_Mode_Data[8] = {0x2F,0x60,0x60,0x00,0xFF,0x00,0x00,0x00};
+
+/* -----------------------------------------------------------------------------
+Sends a CAN Message to the controller
 Returns 1 for succes. May be slow. a faster send message is build into Goto_Pos
 @param COB_ID
 @param Message length
 @param Data string
-*/
+----------------------------------------------------------------------------- */
 CANMessage create_CAN_msg(int COB_ID, int LENGTH, char * DATA);
     
+//A struct in which msg_1 is saved
+//struct EPOS_MESSAGES {
+//    CANMessage msg_1;
+//};
+
+//CANMessage Latest_EPOS_msg(int NODE_ID);   
+ 
 // The EPOS class
 class EPOS{
     
     public:
     
     int NODE_ID;   // the CAN adress of this controller
+    
+    EPOS(int node_id){
+        NODE_ID=node_id;
+        };
 
     /** puts the EPOS in shutdown state */
     CANMessage Shutdown();
@@ -42,22 +65,31 @@
     CANMessage StatusWord();
     
     /** puts the EPOS in homing mode */
-    CANMessage Homing_Mode();
+    CANMessage HomingMode();
     
     /** sets the homing method to current mode positive speed */
-    CANMessage Homing_Method_positive();
+    CANMessage HomingMethodPositive();
     
     /** sets the homing method to current mode positive speed */
-    CANMessage Homing_Method_negative();
+    CANMessage HomingMethodNegative();
     
     /** puts the EPOS in start homing state */
     CANMessage StartHoming();
+     
+    /** Resets the fault condition on the EPOS */
+    CANMessage ClearFault();
     
-    /** Sets the EPOS home method to current pos */
-    CANMessage Homing_Method_current_pos();
+    /** Function that defines a message with the right amount of quartercircles */
+    CANMessage GoToPosition(int quartercircles);
+    
+    /** Function that defines a message with the right amount of quartercircles */
+    CANMessage PositionMode();
 
     /** Homes the EPOS with Current mode */
     void Homing();
+    
+    /** With this function the axis moves to the new absolute position */
+    void StartPositionMode();
 };
 
 #endif
\ No newline at end of file

--- a/main.cpp	Fri Apr 05 12:12:24 2019 +0000
+++ b/main.cpp	Wed Apr 10 09:33:36 2019 +0000
@@ -3,18 +3,23 @@
 #include "Daan_Test1_maxon.h"
 
 RawSerial pc(SERIAL_TX,SERIAL_RX);
-CAN can(PB_5,PB_6);                                                             //NOG CHECKEN!!!
+CAN can(PB_8,PB_9);                                                             //NOG CHECKEN!!!
 
-EPOS epos;
-Thread thread;
+EPOS epos(1);
+Thread thread1;
+Thread thread2;
 
 int main() {
     pc.baud(921600);
     can.frequency(250000);                                                      //NOG AANPASSEN IN EPOS COMPUTER PROGRAMMA ZODAT DE CAN FREQUENCY OP MOTORCONTROLLER OVEREENKOMT MET DIE VAN DE MASTER!! -> value 3
     pc.printf("startup: \r\n");   
+
+    thread1.start(&epos,&EPOS::Homing);                                         //start de functie Homing
+    Thread::wait(100); 
+    thread1.join();
     
-    thread.start(&epos,&EPOS::Homing);                                          //start de functie Homing
+    thread2.start(&epos,&EPOS::StartPositionMode);                                         
     Thread::wait(100); 
-    
+    thread2.join();
     
 }
\ No newline at end of file