Diff: main.cpp

Revision:

1:2a43cf183a62

Parent:

0:607bc887b6e0

Child:

2:eea12b149dba

--- a/main.cpp	Wed Jul 25 10:43:23 2018 +0000
+++ b/main.cpp	Wed Jul 25 13:43:59 2018 +0000
@@ -13,8 +13,10 @@
 
 #define DATA_MASK 0x0F
 
+//simple channel selection
 #define ADC_PRESSURE 1
 #define ADC_POSITION 3
+//---------------------
 
 #define N_CHANNELS 8 //number of channels to control
                         //1-3: front segment
@@ -65,17 +67,17 @@
 double dblTargetActLen_mm[N_CHANNELS];
 double dblPathToActuator[N_CHANNELS];//the target position for the actuator (sent to actuator)
 
-int intDemPos_Tx[N_CHANNELS]; //13-bit value to be sent to the actuator
+int intDemandPos_Tx[N_CHANNELS]; //13-bit value to be sent to the actuator
 int intPosSPI_Rx[N_CHANNELS]; //13 bit value received over SPI from the actuator
 int intPosADC_Rx[N_CHANNELS]; //12-bit ADC reading of potentiometer on actuator
 
 double dblPathTolerance; //how close the linear path must get to the target position before considering it a success.
 
-double dblActuatorConversion[N_CHANNELS] = {0.24176,0.24176,0.24176,0.24176,0.24176,0.36264,0.36264};
+double dblActuatorConversion[N_CHANNELS] = {0.24176,0.24176,0.24176,0.24176,0.24176,0.36264,0.36264};// covert from chamber lengths to actuator lengths
 
-double dblSmoothingFactor = 0.5;
+double dblSmoothingFactor = 0.5; // 0<x<=1, where 1 is no smoothing
 
-char chrErrorFlag[N_CHANNELS];
+char chrErrorFlag[N_CHANNELS];// 3 error bits from LL
 
 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
 
@@ -97,7 +99,8 @@
 
 Thread threadSimulateDemand(osPriorityHigh);
 
-Mutex mut[N_CHANNELS];
+Mutex mutChannel[N_CHANNELS];
+Mutex mutPsi;
 
 Semaphore semPathPlan(1);//
 
@@ -116,14 +119,15 @@
 
 
 
-
-double dblDemPosition[N_CHANNELS];
+/*
+double dblDemandPosition[N_CHANNELS];
 double dblPosition[N_CHANNELS];
 double dblPressure[N_CHANNELS];
+*/
 
 int ThreadID[N_CHANNELS];
 
-bool blnDataReady[N_CHANNELS];
+bool isDataReady[N_CHANNELS]; // flag to indicate path data is ready for transmission to low level.
 
 unsigned int ReadADCPosition(int channel)
 {
@@ -138,7 +142,7 @@
     spi.write(PREAMBLE);
     outputA = spi.write(CHAN_3);
     outputB = spi.write(0xFF);
-    cs_ADC[channel] = 1;
+    cs_ADC[channel]= 1;
     
     outputA = outputA & DATA_MASK;
     outputA = outputA<<8;
@@ -169,20 +173,24 @@
     return output;
 }
 
-void TransmitData(int channel) 
+void SendReceiveData(int channel) 
 {
     //get data from controller
     spi.format(16,2);
     spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
     
     cs_LL[channel] = 0;//select relevant chip
-    intPosSPI_Rx[channel] = spi.write(intDemPos_Tx[channel]); //transmit & receive
+    intPosSPI_Rx[channel] = spi.write(intDemandPos_Tx[channel]); //transmit & receive
     cs_LL[channel] = 1;//deselect chip
+    isDataReady[channel] = 0;//data no longer ready, i.e. we now require new data
+    
+    dblPressure_bar[channel] = ReadADCPressure(channel);
+    dblPosition_mtrs[channel] = ReadADCPosition(channel);
     
     //sort out received data
     chrErrorFlag[channel] = intPosSPI_Rx[channel]>>13;
     intPosSPI_Rx[channel] = intPosSPI_Rx[channel] & 0x1FFF;
-    dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*MAX_ACTUATOR_LENGTH/dblActuatorConversion[channel];
+    dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*(MAX_ACTUATOR_LENGTH/dblActuatorConversion[channel])/1000;
 }
 
 //common rise handler function 
@@ -190,11 +198,10 @@
 void common_rise_handler(int channel)
 {
     //check if data is ready for tranmission
-    if (blnDataReady[channel])
+    if (isDataReady[channel])
     {
         // Add transmit task to event queue
-        blnDataReady[channel] = 0;//data no longer ready
-        ThreadID[channel] = queue.call(TransmitData,channel);//schedule transmission
+        ThreadID[channel] = queue.call(SendReceiveData,channel);//schedule transmission
     }
 }
 
@@ -227,9 +234,9 @@
 void fall6(void) { common_fall_handler(6); }
 void fall7(void) { common_fall_handler(7); }
 
-void startPathPlan()
+void startPathPlan() // Plan a new linear path after receiving new target data
 {
-    semPathPlan.release();
+    semPathPlan.release(); // Uses threadReplan which is below normal priority to ensure consistent transmission to LL
 }
 
 //this function will be called when a new transmission is received from high level
@@ -239,10 +246,12 @@
     //{
         //semReplan.wait();//wait until called
         //printf("replan!\r\n");
-        for (ii = 0; ii < N_CHANNELS; ii++)
-        {
-            mut[ii].lock();
-        }
+        //for (ii = 0; ii < N_CHANNELS; ii++)
+//        {
+//            mutChannel[ii].lock();
+//        }
+        
+        mutPsi.lock();// lock mutex for PSI to ensure no conflict when receiving new messages while already replanning
         
         //update front segment
         dblTargetChambLen_mm[0] = dblPSI[0][0]*1000;
@@ -256,14 +265,16 @@
         dblTargetChambLen_mm[4] = dblPSI[2][1]*1000;
         dblTargetChambLen_mm[5] = dblPSI[2][2]*1000;
         
-        for (ii = 0; ii < N_CHANNELS; ii++)
-        {
-            mut[ii].unlock();
-        }
+        mutPsi.unlock();// unlock mutex for PSI to ensure no conflict when receiving new messages while already replanning
+        
+//        for (ii = 0; ii < N_CHANNELS; ii++)
+//        {
+//            mutChannel[ii].unlock();
+//        }
 
         for (ii = 0; ii< N_CHANNELS; ii++)
         {
-            mut[ii].lock();//lock variables to avoid race condition
+            mutChannel[ii].lock();//lock variables to avoid race condition
             
             //check to see if positions are achievable
             if(dblTargetChambLen_mm[ii]>dblMaxChamberLengths_mm[ii]) 
@@ -294,15 +305,16 @@
                 }
             }
             
-            mut[ii].unlock(); //unlock mutex.
+            mutChannel[ii].unlock(); //unlock mutex.
         }
     //}
     
     
 }
 
-void CalculatePath()
+void CalculateSmoothPath()
 {
+    double dblMeasuredSampleTime;
     while(1)
     {
         semPathPlan.wait();
@@ -311,17 +323,30 @@
 //            blnReplan = 0;//remove flag
 //            ReplanPath(dblPSI, dblTargetTime_s);
 //        }
+        
+        // If run time is more than 50 us from expected, calculate from measured time step
+        if (fabs(PATH_SAMPLE_TIME_S*1000000 - timer.read_us()) > 50)
+        {
+            dblMeasuredSampleTime = timer.read();
+        }
+        else
+        {
+            dblMeasuredSampleTime = PATH_SAMPLE_TIME_S;
+        }
+        timer.reset();    
+            
         for(ii = 0; ii < N_CHANNELS; ii++)
         {
             //calculate next step in linear path
-            mut[ii].lock();//lock relevant mutex
-            dblLinearPath_mm[ii] = dblLinearPath_mm[ii] + dblVelocity_mmps[ii]*PATH_SAMPLE_TIME_S;
+            mutChannel[ii].lock();//lock relevant mutex
+            dblLinearPath_mm[ii] = dblLinearPath_mm[ii] + dblVelocity_mmps[ii]*dblMeasuredSampleTime; // PATH_SAMPLE_TIME_S SHOULD BE MEASURED
+            
             //check tolerance
-            if (abs(dblLinearPath_mm[ii] - dblTargetActLen_mm[ii]) <= dblPathTolerance)
+            if (fabs(dblLinearPath_mm[ii] - dblTargetActLen_mm[ii]) <= dblPathTolerance)
             {
                 dblVelocity_mmps[ii] = 0; //stop linear path generation when linear path is within tolerance of target position.
             }
-            mut[ii].unlock();//unlock relevant mutex
+            mutChannel[ii].unlock();//unlock relevant mutex
             
             //calculate next step in smooth path
             dblSmoothPath_mm[ii] = dblSmoothingFactor*dblLinearPath_mm[ii] + (1.0-dblSmoothingFactor)*dblSmoothPath_mm[ii];
@@ -329,22 +354,24 @@
             //convert to actuator distances
             dblPathToActuator[ii] = dblSmoothPath_mm[ii];
             
-            intDemPos_Tx[ii] = (int) dblPathToActuator[ii]/MAX_ACTUATOR_LENGTH*8191;//convert to a 13-bit number;
-            intDemPos_Tx[ii] = intDemPos_Tx[ii] & 0x1FFF; //ensure number is 13-bit 
+            intDemandPos_Tx[ii] = (int) (dblPathToActuator[ii]/MAX_ACTUATOR_LENGTH)*8191;//convert to a 13-bit number;
+            intDemandPos_Tx[ii] = intDemandPos_Tx[ii] & 0x1FFF; //ensure number is 13-bit 
             
           
-            blnDataReady[ii] = 1;//signal that data ready
+            isDataReady[ii] = 1;//signal that data ready
 
         }
-        printf("%d, %f,%f,%f, %f\r\n",timer.read_ms(), dblTargetActLen_mm[0] ,dblVelocity_mmps[0], dblLinearPath_mm[0], dblSmoothPath_mm[0]);
+        
+        //printf("%d, %f,%f,%f, %f\r\n",timer.read_ms(), dblTargetActLen_mm[0] ,dblVelocity_mmps[0], dblLinearPath_mm[0], dblSmoothPath_mm[0]);
     }
 }
 
-void SimulateDemand()
+void SimulateDemand() // For testing purposes
 {
+    kk = 0;
     while(1)
     {
-        mut[0].lock();
+        mutChannel[0].lock();
         if(kk == 0)
         {
             dblPSI[0][0] = (double) 10.0;
@@ -361,7 +388,7 @@
         
         //semReplan.release();
         
-        mut[0].unlock();
+        mutChannel[0].unlock();
         ReplanPath();
     
         Thread::wait(7000);
@@ -382,11 +409,11 @@
         cs_ADC[ii] = 1;
         
         //data ready flags set to not ready
-        blnDataReady[ii] = 0;
+        isDataReady[ii] = 0;
     }
     
     //calculate some control variables
-    dblPathTolerance = 0.1;//MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; //path tolerance. 
+    dblPathTolerance = 0.1;//MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; //path tolerance in mm. 
     
     pinReset = 1; //initialise reset pin to not reset the controllers.
     
@@ -420,20 +447,18 @@
     pinGate[7].fall(&fall7);
     
     timer.start();
-    kk = 0;
     
     threadSimulateDemand.start(SimulateDemand);
-    threadPathPlan.start(CalculatePath); //start planning thread
+    threadPathPlan.start(CalculateSmoothPath); //start planning thread
+    
     PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); //set up planning thread to recur at fixed intervals
+    
     threadReplan.start(ReplanPath);//start Replanning thread 
      Thread::wait(1);  
      
       
-    while(1) { 
-    
-    
-    
-    Thread::wait(osWaitForever); 
+    while(1) {
+        Thread::wait(osWaitForever); 
     }
 }