Diff: main.cpp

Revision:

5:52e3137c2831

Parent:

4:396433e30e64

Child:

6:8a0250a4877a

--- a/main.cpp	Fri Apr 05 19:06:48 2013 +0000
+++ b/main.cpp	Wed Oct 29 13:40:24 2014 +0000
@@ -14,7 +14,7 @@
 #define RNE 0x04
 
 #define CALIB_DELAY         1500
-#define NUMBER_OF_CALIB     3
+#define NUMBER_OF_CALIB     1
 
 SPISlave device(p5, p6, p7, p8); // mosi, miso, sclk, ssel
 DigitalIn sync_master(p11); //previously p25
@@ -341,6 +341,8 @@
         
         int32_t position = encPosition[i] - calibOffset[i];
         
+        int16_t force = getForce(11); //get hand force
+        
         //position
         writeBufferSPI[i][0] = position;
         writeBufferSPI[i][1] = position>>8;
@@ -359,8 +361,12 @@
         writeBufferSPI[i][5] = avgCurrent[node_id-1]>>8;
         
         //force       
-        writeBufferSPI[i][6] = 0;
-        writeBufferSPI[i][7] = 0;
+        //force       
+        writeBufferSPI[i][6] = force;
+        writeBufferSPI[i][7] = force>>8;
+        //writeBufferSPI[i][6] = 0;
+        //writeBufferSPI[i][7] = 0;
+        //pc.printf("%d\n", force); 
                 
         //pc.printf("[%d] pos=%d cur=%d\n", node_id, encPosition[node_id-1], avgCurrent[node_id-1]);                        
         //force = getMedianForceVal(node_id); //medForce[node_id-1];                  
@@ -407,80 +413,54 @@
     { 
         setModeOfOperationPDO(i, VALUE_CURRENT_MODE);
     }
+    
+    wait_ms(CALIB_DELAY); 
         
-    for(int j=0; j<NUMBER_OF_CALIB; j++)
+    for(int j=1; j<=numberEpos2Boards; 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);
+        setCurrent(j, 150);
         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...");
         
-        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);  
+        calibOffset[i] = encPosition[i];
         
-        //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("%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);    
+    /*
     pc.printf("\n\rCompute mean, min and max offsets\n\r");
     for(int i=0; i<numberEpos2Boards; i++)
     { 
@@ -507,14 +487,16 @@
             
         }        
     }
-    
+    */
+    /*
     for(int i=0; i<numberEpos2Boards; i++)
     {
-        pc.printf("nodeID %d : min[%d] max[%d]\n\r", i+1, minDistFromMean[i], maxDistFromMean[i]);
+       // pc.printf("nodeID %d : min[%d] max[%d]\n\r", i+1, minDistFromMean[i], maxDistFromMean[i]);
+        pc.printf("nodeID %d : tempCalibEnc[%d]\n\r", i+1, tempCalibEnc[i]);
         
         //update the offsets   
-        calibOffset[i] = meanCalibEnc[i];
-    }   
+        calibOffset[i] = tempCalibEnc[i];
+    }   */
 }
 
 int main()