Diff: main.cpp

Revision:

6:8a0250a4877a

Parent:

5:52e3137c2831

--- a/main.cpp	Wed Oct 29 13:40:24 2014 +0000
+++ b/main.cpp	Wed Feb 11 16:18:15 2015 +0000
@@ -1,9 +1,39 @@
+/*
+ * Copyright (c) 2013, The Robot Studio
+ *  All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  Created on: Feb 27, 2013
+ *      Author: Cyril Jourdan (cyril.jourdan@therobotstudio.com)
+ */
+
 #define COMPILE_MAIN_CODE_TRS_SLAVE
 #ifdef  COMPILE_MAIN_CODE_TRS_SLAVE
 
-//include files
+/*** Includes ***/
 #include "include/eposCmd.h"
 
+/*** Defines ***/
 #define OPEN_ARROW          0x3C //< = 60
 #define CLOSE_ARROW         0x3E //< = 62
 #define NUMBER_OF_ARROWS    5
@@ -16,6 +46,7 @@
 #define CALIB_DELAY         1500
 #define NUMBER_OF_CALIB     1
 
+/*** Variables ***/
 SPISlave device(p5, p6, p7, p8); // mosi, miso, sclk, ssel
 DigitalIn sync_master(p11); //previously p25
 
@@ -40,47 +71,11 @@
 
 int counter = 0;
 
-//5 variables for median poti position
-//int16_t potiVal[NB_SAMPLES_MEDIAN];
-//int16_t sortVal[NB_SAMPLES_MEDIAN];
-
 int16_t forceVal[NB_SAMPLES_MEDIAN];
 int16_t sortForceVal[NB_SAMPLES_MEDIAN];
 //int16_t lastForceVal[NUMBER_EPOS2_BOARDS][NB_SAMPLES_MEDIAN];
 
-/*
-int16_t getMedianPotiVal(const int8_t nodeID) 
-{        
-    //read poti
-    for(int m=0; m<NB_SAMPLES_MEDIAN; m++)
-    {
-        getPotiPosition(nodeID);
-        wait_us(1000);
-        
-        potiVal[m] = potiPosArray[nodeID - 1];        
-        sortVal[m] = potiVal[m];
-
-        wait_us(100);
-    }
-    
-    //sort values
-    for(int m=1; m<NB_SAMPLES_MEDIAN; ++m)
-    {
-        int16_t n;
-        n = sortVal[m];
-        int p;
-
-        for(p = m-1; (p >=0) && (n<sortVal[p]); p--)
-        {
-            sortVal[p+1] = sortVal[p];
-        }
-        sortVal[p+1] = n;
-    }
-       
-    return sortVal[2];
-}
-*/
-
+/*** Functions ***/
 bool verifyChecksum() //check the data comming from the master over SPI
 {      
     for(int i=0; i<NUMBER_MAX_EPOS2_PER_SLAVE; i++)
@@ -197,7 +192,9 @@
                 boardStatus[nodeID-1] = 1;                
                 //first step : fault reset on controlword
                 //pc.printf("Node %d - STEP 1 - faultResetControlword\n", nodeID);
-                faultResetControlword(nodeID);                 
+                
+                //Debug for fault detection on brachii
+                //faultResetControlword(nodeID);                 
                 break;  
                 
             case COB_ID_SDO_SERVER_TO_CLIENT_DEFAULT : //SDO Acknoledgement frame
@@ -407,35 +404,48 @@
 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);
     }
     
-    wait_ms(CALIB_DELAY); 
+    ledchain[2] = 1; 
+    
+    wait_ms(20); 
         
     for(int j=1; j<=numberEpos2Boards; j++)
     {  
-        pc.printf("- Calibration number %d\n\r", j);  
+        //pc.printf("- Calibration number %d\n\r", j);  
         //trigger current in certain order
-        setCurrent(j, 150);
-        wait_ms(CALIB_DELAY);    
+        setCurrent(j, 100);
+        wait_ms(20);    
     }
-        
+    
+    ledchain[3] = 1; 
+        /*
     pc.printf("- Offsets will be recorded in 5 sec...");
     wait_ms(1000);
     pc.printf("4...");
+    ledchain[0] = 1;  
     wait_ms(1000);
+    ledchain[0] = 0;  
+    ledchain[1] = 1;  
     pc.printf("3...");
     wait_ms(1000);
+    ledchain[1] = 0;  
+    ledchain[2] = 1;  
     pc.printf("2...");
     wait_ms(1000);
+    ledchain[2] = 0;  
+    ledchain[3] = 1;  
     pc.printf("1...");
     wait_ms(1000);
+    ledchain[3] = 0;  
     pc.printf("0...");
-        
+        */
+        /*
     //save current encoder position as calib offset
     for(int i=0; i<numberEpos2Boards; i++)
     {    
@@ -447,58 +457,22 @@
         //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 case where motor start from zero
     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]);
-        pc.printf("nodeID %d : tempCalibEnc[%d]\n\r", i+1, tempCalibEnc[i]);
-        
-        //update the offsets   
-        calibOffset[i] = tempCalibEnc[i];
-    }   */
+    {    
+        calibOffset[i] = 0;        
+    }    
 }
 
+/*** Main ***/
 int main() 
 {          
     pc.baud(115200);
@@ -523,25 +497,16 @@
         
     pc.printf("--- Initialise EPOS2 boards ---\n\r");    
     
-  /* 
-    for(int i=1; i<=NUMBER_EPOS2_BOARDS; i++)
-    {
-        if(initEposBoard(i) = EPOS2_OK) numberEpos2Boards++; 
-    } 
-    */
+    ledchain[0] = 1; 
     
-    //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;   
+    ledchain[1] = 1; 
     
     pc.printf("--- Enable Interrupts ---\n\r"); 
     //attach the interrupt function
@@ -549,27 +514,7 @@
     __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
@@ -583,6 +528,11 @@
     
     //then start the main loop
     pc.printf("--- Start main loop ---\n\r"); 
+    
+    ledchain[0] = 0; 
+    ledchain[1] = 0; 
+    ledchain[2] = 0; 
+    ledchain[3] = 0; 
            
     while(1) 
     {