Diff: main.cpp

Revision:

3:c16d726670b2

Parent:

2:7ab1d5918efe

Child:

4:396433e30e64

--- a/main.cpp	Fri Mar 08 17:54:03 2013 +0000
+++ b/main.cpp	Fri Mar 29 17:00:02 2013 +0000
@@ -14,10 +14,12 @@
 #define RNE 0x04
 
 SPISlave device(p5, p6, p7, p8); // mosi, miso, sclk, ssel
-DigitalIn sync_master(p25);
+DigitalIn sync_master(p11); //previously p25
 
 DigitalOut logicPin(p26); //to record with Logic analyser on an event, pin high.
-//Timer timer;
+
+uint8_t numberEpos2Boards = 1;
+
 char dataChecksum = 0x00;
 char cmdChecksum = 0x00;
 
@@ -150,7 +152,7 @@
     }
     
     //check nodeID first
-    if((nodeID >= 0) && (nodeID <= NUMBER_EPOS2_BOARDS))
+    if((nodeID >= 1) && (nodeID <= numberEpos2Boards)) //was nodeID >= 0 before ???
     {        
         switch(cobID)
         {       
@@ -267,34 +269,39 @@
         uint8_t node_ID = readBufferSPI[i][0];
         uint8_t node_mode = readBufferSPI[i][1];
         int32_t value = readBufferSPI[i][2] + (readBufferSPI[i][3]<<8) + (readBufferSPI[i][4]<<16) + (readBufferSPI[i][5]<<24);
+        
+        if((node_ID >= 1) && (node_ID <= numberEpos2Boards)) 
+        {       
+            if(node_mode != 0xFF) 
+            {       
+                switch (node_mode)
+                {                
+                    //WARNING : changing mode requires 1 cycle, so "else" case has been added, commands needs to be sent at least twice to be applied when the mode change.
+                    
+                    case POSITION: //first change modes of motors that will be triggered later   
+                        if(activMode[node_ID-1] != POSITION) setModeOfOperationPDO(node_ID, VALUE_POSITION_MODE);         
+                        else setPosition(node_ID, value);                    
+                        break;
+                    case CURRENT: //first change modes of motors that will be triggered later (like CURRENT mode needs some time to be active) 
+                        //pc.printf("setCurrent(%d, %d)\n", motorCmdBuffer[i].nodeID, motorCmdBuffer[i].value);
+                        if(activMode[node_ID-1] != CURRENT) setModeOfOperationPDO(node_ID, VALUE_CURRENT_MODE);
+                        else setCurrent(node_ID, value);
+                        break;
+                    case VELOCITY: //first change modes of motors that will be triggered later            
+                        if(activMode[node_ID-1] != VELOCITY) setModeOfOperationPDO(node_ID, VALUE_VELOCITY_MODE);         
+                        else setVelocity(node_ID, value);
+                        break;
+                    default:
+                        break;
+                }
                 
-        if(node_mode != 0xFF) 
-        {       /*
-            switch (node_mode)
-            {                
-                case POSITION: //first change modes of motors that will be triggered later   
-                    if(activMode[node_ID-1] != POSITION) setModeOfOperationPDO(node_ID, VALUE_POSITION_MODE);         
-                    setPosition(node_ID, value);                    
-                    break;
-                case CURRENT: //first change modes of motors that will be triggered later (like CURRENT mode needs some time to be active) 
-                    //pc.printf("setCurrent(%d, %d)\n", motorCmdBuffer[i].nodeID, motorCmdBuffer[i].value);
-                    if(activMode[node_ID-1] != CURRENT) setModeOfOperationPDO(node_ID, VALUE_CURRENT_MODE);
-                    setCurrent(node_ID, value);
-                    break;
-                case VELOCITY: //first change modes of motors that will be triggered later            
-                    if(activMode[node_ID-1] != VELOCITY) setModeOfOperationPDO(node_ID, VALUE_VELOCITY_MODE);         
-                    setVelocity(node_ID, value);
-                    break;
-                default:
-                    break;
+                ledchain[3] = 1;    //switch on when cmd is applied
+                cmdPlayLED = true;
             }
-            */
-            ledchain[3] = 1;    //switch on when cmd is applied
-            cmdPlayLED = true;
-        }
-        else //idle mode 0xFF
-        {
-            if(!cmdPlayLED) ledchain[3] = 0;    //switch off when slave is idle, i.e. all cmd in a set are 0xFF
+            else //idle mode 0xFF
+            {
+                if(!cmdPlayLED) ledchain[3] = 0;    //switch off when slave is idle, i.e. all cmd in a set are 0xFF
+            }
         }
         
         wait_us(10);
@@ -303,21 +310,20 @@
 
 void updateTxSPIBuffer()
 {
-    for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
+    for(int i=1; i<=numberEpos2Boards; i++)
     {
-        uint8_t node_id = i+1;
-        //if(node_id!=5)
-        //{                        
-            getPosition(node_id);
-            wait_us(200);   
-            getCurrent(node_id);
-            wait_us(200);
-            //if((node_id >= 2) && (node_id <= 9)) getMedianForceVal(node_id);                                           
-        //}            
+        //uint8_t node_id = i;
+                              
+        getPosition(i);
+        wait_us(200);   
+        getCurrent(i);
+        wait_us(200);  
+        //getForce(i);
+        wait_us(200);                 
     }
       
     //build the motorDataSet_msg          
-    for(int i=0; i<NUMBER_EPOS2_BOARDS; i++)
+    for(int i=0; i<numberEpos2Boards; i++)
     {    
         uint8_t node_id = i+1;
                     
@@ -351,7 +357,7 @@
             readBufferSPI[i][j] = 0x00;            
         }
     }
-    
+    /*
     for(int n=0; n<NUMBER_MAX_EPOS2_PER_SLAVE; n++)
     {
         //position
@@ -367,13 +373,13 @@
         //force       
         writeBufferSPI[n][6] = 0xD0+n;
         writeBufferSPI[n][7] = 0x00;      
-    }
+    }*/
 }
 
 int main() 
 {          
     pc.baud(115200);
-    pc.printf("*** Start Slave Main ***\n\r");
+    pc.printf("*** Start Slave Main - DCX Left Arm ***\n\r");
     
     logicPin = 0;
     
@@ -394,13 +400,22 @@
         
     pc.printf("--- Initialise EPOS2 boards ---\n\r");    
     
-    /*
+  /* 
     for(int i=1; i<=NUMBER_EPOS2_BOARDS; i++)
     {
-        initEposBoard(i); 
+        if(initEposBoard(i) = EPOS2_OK) numberEpos2Boards++; 
     } 
     */
     
+    //int i=1;
+    while(initEposBoard(numberEpos2Boards) == EPOS2_OK)
+    {
+        numberEpos2Boards++; 
+        //i++;
+    } 
+    
+    numberEpos2Boards--; //because it has been increment one time too much at the end of the while loop
+    
     //initEposBoard(0);
         
     //ledchain[0] = 1;   
@@ -414,7 +429,7 @@
 
     //gather first pack of data
     //get the sensor values       
-    //updateTxSPIBuffer();      
+    updateTxSPIBuffer();      
       
     //update checksum
     calculateSPIChecksum();    
@@ -543,7 +558,7 @@
             if(device.receive()) 
             {
                 cmdChecksum = device.read();
-                //pc.printf("cmdChecksum 0x%02X\n", cmdChecksum);
+                //pc.printf("%02X\n", cmdChecksum);
                 cmdValid = verifyChecksum();                 
                 checksumReceived = true; //exit while loop
             }
@@ -555,10 +570,17 @@
         __enable_irq();
      
         logicPin = 0; 
-     
-      /*     
+        /*
+        //print some data:
+        for(int n=0; n<1; n++)
+        {
+            pc.printf("%02X %02X %02X %02X %02X %02X %02X %02X\n", readBufferSPI[n][0], readBufferSPI[n][1], readBufferSPI[n][2], readBufferSPI[n][3], readBufferSPI[n][4], readBufferSPI[n][5], readBufferSPI[n][6], readBufferSPI[n][7]);
+        }  
+        */
+        //pc.printf("%02X\n", readBufferSPI[0][4]);
+           
         wait_us(30);
-        logicPin = 1;
+      /*  logicPin = 1;
         wait_us(10);
         logicPin = 0;
         wait_us(20);
@@ -580,7 +602,7 @@
           */              
         //get the sensor values ready to be sent for the next loop   
         //update the writeBufferSPI
-        //updateTxSPIBuffer();    //test with known data first
+        updateTxSPIBuffer();    //test with known data first
         
         //compute checksum        
         calculateSPIChecksum();