Diff: main.cpp

Revision:

4:396433e30e64

Parent:

3:c16d726670b2

Child:

5:52e3137c2831

--- a/main.cpp	Fri Mar 29 17:00:02 2013 +0000
+++ b/main.cpp	Fri Apr 05 19:06:48 2013 +0000
@@ -13,9 +13,13 @@
 #define TFE 0x01
 #define RNE 0x04
 
+#define CALIB_DELAY         1500
+#define NUMBER_OF_CALIB     3
+
 SPISlave device(p5, p6, p7, p8); // mosi, miso, sclk, ssel
 DigitalIn sync_master(p11); //previously p25
 
+DigitalIn calibDonePin(p23);
 DigitalOut logicPin(p26); //to record with Logic analyser on an event, pin high.
 
 uint8_t numberEpos2Boards = 1;
@@ -26,6 +30,14 @@
 uint8_t writeBufferSPI[NUMBER_MAX_EPOS2_PER_SLAVE][NUMBER_BYTES_PER_MSG];
 uint8_t readBufferSPI[NUMBER_MAX_EPOS2_PER_SLAVE][NUMBER_BYTES_PER_MSG];
 
+int32_t tempCalibEnc[NUMBER_OF_CALIB][NUMBER_MAX_EPOS2_PER_SLAVE];
+int32_t meanCalibEnc[NUMBER_MAX_EPOS2_PER_SLAVE];
+int32_t distanceFromMean[NUMBER_OF_CALIB][NUMBER_MAX_EPOS2_PER_SLAVE];
+int32_t minDistFromMean[NUMBER_MAX_EPOS2_PER_SLAVE];
+int32_t maxDistFromMean[NUMBER_MAX_EPOS2_PER_SLAVE];
+
+int32_t calibOffset[NUMBER_MAX_EPOS2_PER_SLAVE];
+
 int counter = 0;
 
 //5 variables for median poti position
@@ -280,7 +292,7 @@
                     
                     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);                    
+                        else setPosition(node_ID, value + calibOffset[node_ID-1]);                    
                         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);
@@ -326,13 +338,22 @@
     for(int i=0; i<numberEpos2Boards; i++)
     {    
         uint8_t node_id = i+1;
-                    
+        
+        int32_t position = encPosition[i] - calibOffset[i];
+        
+        //position
+        writeBufferSPI[i][0] = position;
+        writeBufferSPI[i][1] = position>>8;
+        writeBufferSPI[i][2] = position>>16;
+        writeBufferSPI[i][3] = position>>24;
+        
+                   /* 
         //position
         writeBufferSPI[i][0] = encPosition[node_id-1];
         writeBufferSPI[i][1] = encPosition[node_id-1]>>8;
         writeBufferSPI[i][2] = encPosition[node_id-1]>>16;
         writeBufferSPI[i][3] = encPosition[node_id-1]>>24;
-        
+        */
         //current
         writeBufferSPI[i][4] = avgCurrent[node_id-1];
         writeBufferSPI[i][5] = avgCurrent[node_id-1]>>8;
@@ -357,6 +378,7 @@
             readBufferSPI[i][j] = 0x00;            
         }
     }
+    
     /*
     for(int n=0; n<NUMBER_MAX_EPOS2_PER_SLAVE; n++)
     {
@@ -376,6 +398,125 @@
     }*/
 }
 
+void calibrate()
+{
+    pc.printf("- Start Calibration\n\r");
+    
+    //set all boards to current mode
+    for(int i=1; i<=numberEpos2Boards; i++)
+    { 
+        setModeOfOperationPDO(i, VALUE_CURRENT_MODE);
+    }
+        
+    for(int j=0; j<NUMBER_OF_CALIB; j++)
+    {  
+        pc.printf("- Calibration number %d\n\r", j);  
+        //trigger current in certain order
+        setCurrent(7, 70);
+        wait_ms(CALIB_DELAY);    
+        setCurrent(6, 100);
+        wait_ms(CALIB_DELAY);
+        setCurrent(11, 100);
+        wait_ms(CALIB_DELAY);
+        setCurrent(1, 100);
+        wait_ms(CALIB_DELAY);
+        setCurrent(3, 100);
+        wait_ms(CALIB_DELAY);
+        setCurrent(4, 100);
+        wait_ms(CALIB_DELAY);    
+        setCurrent(5, 70);
+        wait_ms(CALIB_DELAY);
+        setCurrent(2, 70);
+        wait_ms(CALIB_DELAY);
+        setCurrent(8, 70);
+        wait_ms(CALIB_DELAY);
+        setCurrent(9, 70);
+        wait_ms(CALIB_DELAY);
+        setCurrent(10, 100);
+        wait_ms(CALIB_DELAY);    
+        setCurrent(12, 70);
+        wait_ms(CALIB_DELAY);
+        setCurrent(13, 70);
+        wait_ms(CALIB_DELAY);
+        setCurrent(15, -100);
+        wait_ms(CALIB_DELAY);
+        setCurrent(14, 100);
+        wait_ms(CALIB_DELAY);
+        
+        pc.printf("- Offsets will be recorded in 5 sec...");
+        wait_ms(1000);
+        pc.printf("4...");
+        wait_ms(1000);
+        pc.printf("3...");
+        wait_ms(1000);
+        pc.printf("2...");
+        wait_ms(1000);
+        pc.printf("1...");
+        wait_ms(1000);
+        pc.printf("0...");
+        
+        //save current encoder position as calib offset
+        for(int i=0; i<numberEpos2Boards; i++)
+        {    
+            int nodeid = i+1;
+            getPosition(nodeid);
+            wait_us(500);  
+            tempCalibEnc[j][i] = encPosition[i];
+            
+            //pc.printf("%d\n\r", calibOffset[i]);
+        }
+        pc.printf("OK\n\r");
+        
+        pc.printf("current to zero\n\r");
+        for(int i=0; i<numberEpos2Boards; i++)
+        {    
+            int nodeid = i+1;
+            setCurrent(nodeid, 0);
+            wait_us(500);          
+        }
+        
+        pc.printf("wait 5 sec now\n\r");
+        wait_ms(5000);
+        pc.printf("will restart now\n\r");
+        wait_ms(1000);
+    }//end for NUMBER_OF_CALIB times
+    
+    pc.printf("\n\rCompute mean, min and max offsets\n\r");
+    for(int i=0; i<numberEpos2Boards; i++)
+    { 
+        meanCalibEnc[i] = 0;
+        minDistFromMean[i] = 100000;
+        maxDistFromMean[i] = 0;
+
+        for(int j=0; j<NUMBER_OF_CALIB; j++)
+        { 
+            meanCalibEnc[i] += tempCalibEnc[j][i];
+        }
+        meanCalibEnc[i] /=NUMBER_OF_CALIB;
+    }
+    
+    for(int j=0; j<NUMBER_OF_CALIB; j++)
+    { 
+        for(int i=0; i<numberEpos2Boards; i++)
+        { 
+            distanceFromMean[j][i] = tempCalibEnc[j][i] - meanCalibEnc[i];
+            
+            if(distanceFromMean[j][i] < minDistFromMean[i]) minDistFromMean[i] = distanceFromMean[j][i];
+            if(distanceFromMean[j][i] > maxDistFromMean[i]) maxDistFromMean[i] = distanceFromMean[j][i];
+            
+            
+        }        
+    }
+    
+    for(int i=0; i<numberEpos2Boards; i++)
+    {
+        pc.printf("nodeID %d : min[%d] max[%d]\n\r", i+1, minDistFromMean[i], maxDistFromMean[i]);
+        
+        //update the offsets   
+        calibOffset[i] = meanCalibEnc[i];
+    }   
+}
+
 int main() 
 {          
     pc.baud(115200);
@@ -425,6 +566,30 @@
     cantoepos.attach(&interrupt);
     __enable_irq();
     
+    pc.printf("--- Calibrate Arm ---\n\r");
+  /*  
+    //test
+    pc.printf("test home mode\n\r");   
+    setModeOfOperationPDO(1, VALUE_CURRENT_MODE);
+    wait_ms(100);
+    pc.printf("set current\n\r"); 
+    setCurrent(1, 100);
+    wait_ms(2000); 
+    getPosition(1);
+    wait_ms(100);      
+    pc.printf("getPosition before home %d\n\r", encPosition[0]);    
+    pc.printf("set OBJECT_HOME_POSITION\n\r");
+    setModeOfOperationPDO(1, 6);
+    setObjectSDO(1, OBJECT_HOME_POSITION, encPosition[0]);
+    wait_ms(100); 
+    getPosition(1);
+    wait_ms(100);      
+    pc.printf("getPosition after home %d\n\r", encPosition[0]);
+    //test
+    */
+    //if(calibDonePin) 
+    calibrate();   
+    
     device.reply(0x62); //Prime SPI with first reply
 
     //gather first pack of data