Diff: main.cpp

Revision:

21:0b10d8e615d1

Parent:

20:b2139294c547

Child:

22:82871f00f89d

--- a/main.cpp	Tue Nov 27 16:53:06 2018 +0000
+++ b/main.cpp	Tue Dec 11 15:45:01 2018 +0000
@@ -8,47 +8,37 @@
 #include "HLComms.h"
 #include "LLComms.h"
 
-//DigitalOut pinTesty(PB_8);
-
 // Maximum achievable mm path tolerance plus additional % tolerance
-const float FLT_PATH_TOLERANCE = MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * (1.0f+FLT_PERCENT_PATH_TOLERANCE);
+const float FLT_PATH_TOLERANCE = MAX_SPEED_MMPS * (1/LL_DEMANDS_FREQ_HZ) * (1.0f+FLT_PERCENT_PATH_TOLERANCE);
 
-// PATH VARIABLES
-double dblVelocity_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
-double dblLinearPathCurrentPos_mm[N_CHANNELS]= { 0.0 }; // The current position of the linear path (not sent to actuator)
-double dblTargetActPos_mm[N_CHANNELS] = { 0.0 }; // The final target position for the actuator
-
-// These have to be declared in the global scope or we get a stack overflow. No idea why.
-double _dblVelocity_mmps[N_CHANNELS];
-//double _dblLinearPathCurrentPos_mm[N_CHANNELS];
+// DEMAND VARIABLES
+double dblDemandVelocity_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
+double dblDemandPosition_mtrs[N_CHANNELS] = { 0.0 }; // The final target position for the actuator
+// SENSOR VARIABLES
+double dblPosition_mtrs[N_CHANNELS]; // The actual chamber lengths in meters given as the change in length relative to neutral (should always be >=0)
+double dblPressure_bar[N_CHANNELS]; // The pressure in a given chamber in bar (1 bar  = 100,000 Pa)
 
 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
 LLComms llcomms;
 HLComms hlcomms(SERVER_PORT);
 
 Thread threadLowLevelSPI(osPriorityRealtime);
-Thread threadSmoothPathPlan(osPriorityNormal);
+Thread threadSetDemands(osPriorityNormal);
 Thread threadReceiveAndReplan(osPriorityBelowNormal);
 Thread threadSensorFeedback(osPriorityBelowNormal);
 
 Mutex mutPathIn;
-Semaphore semPathPlan(1);
+Semaphore semLLcomms(1);
 Semaphore semSensorData(1);
 
-Timer timer;
-Ticker PathCalculationTicker;
+Ticker setDemandsTicker;
 Ticker SendSensorDataTicker;
 
 
 void sendSensorData() {
     while( true ) {
         semSensorData.wait();
-        double pressures[N_CHANNELS];
-        // !!! READ ADC PRESSURE ASSUMES THAT LL ARE ON FIRST
-        for(short int i=0; i<N_CHANNELS; i++) {
-            pressures[i] = llcomms.ReadADCPressure_bar(i);
-        }
-        int error_code = hlcomms.send_sensor_message(dblLinearPathCurrentPos_mm,pressures);
+        int error_code = hlcomms.send_sensor_message(dblPosition_mtrs,dblPressure_bar);
         /*if( error_code < 0 ) {
             if(IS_PRINT_OUTPUT) printf("Error %i. Could not send data over the TCP socket. "
                 "Perhaps the server socket is not bound or not set to listen for connections? "
@@ -63,8 +53,8 @@
     semSensorData.release();
 }
 
-void startPathPlan() { // Plan a new linear path after receiving new target data
-    semPathPlan.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
+void startLLcomms() { // Plan a new linear path after receiving new target data
+    semLLcomms.release(); // Uses threadReceiveAndReplan 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
@@ -116,7 +106,7 @@
         dblTarget_mm[1] = input.psi[2][1]*1000;
         dblTarget_mm[2] = input.psi[2][2]*1000;
 
-        // Lock mutex, preventing CalculateSmoothPath from running
+        // Lock mutex, preventing setDemandsForLL from running
         mutPathIn.lock();        
         // Limit requested speed
         double limitedSpeed_mmps = min( max( 0.0 , input.speed ) , (double)MAX_SPEED_MMPS );
@@ -132,10 +122,9 @@
                 // ? Limit requested chamber lengths
                 // ? Convert from chamber length to actuator space
                 // Limit actuator position
-                dblTarget_mm[i] = min( max( 0.0 , dblTarget_mm[i] ) , 40.0 ); // !!! USE A CONSTANT FOR THIS
+                dblTarget_mm[i] = min( max( 0.0 , dblTarget_mm[i] ) , (double)MAX_ACTUATOR_LIMIT ); // !!! USE A CONSTANT FOR THIS
                 // Calculate actuator position change
-                double dblCurrentPosition = dblLinearPathCurrentPos_mm[i];
-                //dblCurrentPosition[i] = llcomms.ReadADCPosition_mtrs(i); // Read position from channel
+                double dblCurrentPosition = dblPosition_mtrs[i];
                 dblDisplacementToTarget_mm[i] = dblTarget_mm[i] - dblCurrentPosition;
                 // Select the max absolute actuator position change
                 if(fabs(dblDisplacementToTarget_mm[i])>maxDistanceToTarget_mm) {
@@ -150,11 +139,11 @@
             // If requested actuator position change is already within tolerance, do NOT replan that actuator
             if( fabs(dblDisplacementToTarget_mm[i]) < FLT_PATH_TOLERANCE ) continue;
             // Calculate velocity for each motor to synchronise movements to complete in max time
-            // Set dblTargetActPos_mm and dblVelocity_mmps
-            dblTargetActPos_mm[i] = dblTarget_mm[i];
-            dblVelocity_mmps[i] = dblDisplacementToTarget_mm[i] / maxTimeToTarget_s;
+            // Set dblDemandPosition_mtrs and dblDemandVelocity_mmps
+            dblDemandPosition_mtrs[i] = dblTarget_mm[i];
+            dblDemandVelocity_mmps[i] = dblDisplacementToTarget_mm[i] / maxTimeToTarget_s;
         }
-        // Unlock mutex, allowing CalculateSmoothPath to run again
+        // Unlock mutex, allowing setDemandsForLL to run again
         mutPathIn.unlock();
         
         // SEND MESSAGE
@@ -170,54 +159,30 @@
 
 }
 
-void CalculateSmoothPath() {
-    int jj;
-    double dblMeasuredSampleTime;
-    double dblSmoothPathCurrentPos_mm[N_CHANNELS] = { 0.0 }; // The current position of the smooth path (not sent to actuator)
-    //double dblPosition_mtrs[N_CHANNELS]; // The actual chamber lengths in meters given as the change in length relative to neutral (should always be >=0)
-    //double dblPressure_bar[N_CHANNELS]; // The pressure in a given chamber in bar (1 bar  = 100,000 Pa)
+void setDemandsForLL() {
+    
     while(1) {
-        semPathPlan.wait();
-        //pinTesty = 1;
-        // 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(jj = 0; jj < N_CHANNELS; jj++) {
-            //dblPressure_bar[jj] = ReadADCPressure_bar(jj); // Read pressure from channel
-            //dblPosition_mtrs[jj] = ReadADCPosition_mtrs(jj); // Read position from channel
-            
-            // Calculate next step in linear path
-            mutPathIn.lock(); // Lock relevant mutex
-            // Check tolerance
-            if (fabs(dblLinearPathCurrentPos_mm[jj] - dblTargetActPos_mm[jj]) <= FLT_PATH_TOLERANCE) {
-                dblVelocity_mmps[jj] = 0.0; // Stop linear path generation when linear path is within tolerance of target position
-            }
-            dblLinearPathCurrentPos_mm[jj] = dblLinearPathCurrentPos_mm[jj] + dblVelocity_mmps[jj]*dblMeasuredSampleTime;
-            dblLinearPathCurrentPos_mm[jj] = min( max( 0.0 , dblLinearPathCurrentPos_mm[jj] ) , 40.0 );
-            mutPathIn.unlock(); // Unlock relevant mutex
-            
-            // Calculate next step in smooth path
-            dblSmoothPathCurrentPos_mm[jj] = FLT_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[jj] + (1.0f-FLT_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[jj];
-            dblSmoothPathCurrentPos_mm[jj] = max( 0.0 , dblSmoothPathCurrentPos_mm[jj] );
-            llcomms.mutChannel[jj].lock(); // MUTEX LOCK            
-            llcomms.demandPosition[jj] = (int) ((dblSmoothPathCurrentPos_mm[jj]/MAX_ACTUATOR_LENGTH)*8191);// Convert to a 13-bit number
-            llcomms.demandPosition[jj] = llcomms.demandPosition[jj] & 0x1FFF; // Ensure number is 13-bit
-            llcomms.mutChannel[jj].unlock(); // MUTEX UNLOCK
+        semLLcomms.wait();
+        
+        mutPathIn.lock(); // Lock relevant mutex
+        for(short int i=0; i<N_CHANNELS; i++) { // For each LL
+            llcomms.mutChannel[i].lock(); // MUTEX LOCK
+// DO IT IN ONE BLOCK
+// SEND AND RECEIVE DATA AT SAME TIME
+// PASS RECEIVED DATA BACK
+// Send position to LL, receive position and store in global
+            llcomms.demandPosition[i] = (int) ((dblDemandPosition_mtrs[i]/MAX_ACTUATOR_LENGTH)*8191); // Convert to a 13-bit number
+            llcomms.demandPosition[i] = llcomms.demandPosition[i] & 0x1FFF; // Ensure number is 13-bit
+// Send speed to LL, receive pressure and store in global
+            llcomms.demandSpeed[i] = (int) ((dblDemandVelocity_mmps[i]/MAX_ACTUATOR_SPEED)*8191);// Convert to a 13-bit number
+            llcomms.demandSpeed[i] = llcomms.demandSpeed[i] & 0x1FFF; // Ensure number is 13-bit
+            llcomms.mutChannel[i].unlock(); // MUTEX UNLOCK
+        } // end for
+        mutPathIn.unlock(); // Unlock relevant mutex
           
-            llcomms.isDataReady[jj] = 1; // Signal that data ready
-        } // end for
+        llcomms.isDataReady[i] = 1; // Signal that data ready
+    } // end while
         
-        //if(IS_PRINT_OUTPUT) printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]);
-        //if(IS_PRINT_OUTPUT) printf("%f, %f, %f, %f, %f, %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[0],dblLinearPathCurrentPos_mm[1],dblLinearPathCurrentPos_mm[2],
-        //  dblLinearPathCurrentPos_mm[3],dblLinearPathCurrentPos_mm[4],dblLinearPathCurrentPos_mm[5],dblLinearPathCurrentPos_mm[6],dblLinearPathCurrentPos_mm[7]);
-        //if(IS_PRINT_OUTPUT) printf("%f\r\n",dblLinearPathCurrentPos_mm[0]);
-        //pinTesty = 0;
-    } // end while
 }
 
 int main() {
@@ -225,14 +190,12 @@
     printf("ML engage. Compiled at %s\r\n.",__TIME__);
     wait(3);
     
-    timer.start();
-
     threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
     threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
-    threadSmoothPathPlan.start(CalculateSmoothPath); // Start planning thread
+    threadSetDemands.start(setDemandsForLL); // Start planning thread
     threadSensorFeedback.start(sendSensorData); // Start sensor feedback thread
 
-    PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); // Set up planning thread to recur at fixed intervals
+    setDemandsTicker.attach(&startLLcomms, 1/LL_DEMANDS_FREQ_HZ); // Set up LL comms thread to recur at fixed intervals
     
     Thread::wait(1);
     while(1) {