Diff: main.cpp

Revision:

13:a373dfc57b89

Parent:

12:595ed862e52f

Child:

14:54c3759e76ed

--- a/main.cpp	Mon Aug 13 09:16:29 2018 +0000
+++ b/main.cpp	Wed Aug 29 10:47:02 2018 +0000
@@ -8,6 +8,8 @@
 #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);
 
@@ -25,8 +27,8 @@
 LLComms llcomms;
 
 Thread threadLowLevelSPI(osPriorityRealtime);
+Thread threadSmoothPathPlan(osPriorityNormal);
 Thread threadReceiveAndReplan(osPriorityBelowNormal);
-Thread threadSmoothPathPlan(osPriorityNormal);
 
 Mutex mutPathIn;
 Semaphore semPathPlan(1);
@@ -107,20 +109,24 @@
         
         mutPathIn.unlock(); // Unlock mutex
 
-        bool isTimeChanged = 0;
+        //bool isTimeChanged = 0;
         double dblMaxRecalculatedTime = input.duration;
+        // Convert from requested chamber to actuator space and limit actuator positions
         for (ii = 0; ii< N_CHANNELS; ii++) {
+            // If sent a negative requested position, do NOT replan that actuator
+            //if( dblTargetChambLen_mm[ii] < 0.0 ) continue;
             //check to see if positions are achievable
             dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*FLT_ACTUATOR_CONVERSION[ii];
+            dblTargetActPos_mm[ii] = min( max( 0.0 , dblTargetActPos_mm[ii] ) , 25.0 );
             //!! LIMIT CHAMBER LENGTHS TOO
-            if( dblTargetActPos_mm[ii]<0.0 || dblTargetActPos_mm[ii]>25.0 ) {
-                dblTargetActPos_mm[ii] = min( max( 0.0 , dblTargetActPos_mm[ii] ) , 25.0 );
-                isTimeChanged = 1;
-            }
         }
+        // Calculate achievable velocities, and hence time, for the requested move to complete within tolerance
         double dblActPosChange;
         short sgn;
         for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
+            // If sent a negative requested position, do NOT replan that actuator
+            //if( dblTargetChambLen_mm[ii] < 0.0 ) continue;
+            
             /*dblActPosChange = 1.0; // or = 0.0;
             _dblVelocity_mmps[ii] = 0.0;*/ // DOESN'T CRASH
             /*dblActPosChange = dblTargetActPos_mm[ii];
@@ -135,46 +141,51 @@
             _dblVelocity_mmps[ii] = 0.0;*/
             
             dblActPosChange = dblTargetActPos_mm[ii] - dblADCCurrentPosition[ii];
-            if( fabs(dblActPosChange) < FLT_PATH_TOLERANCE ) {
+            if( fabs(dblActPosChange) < FLT_PATH_TOLERANCE ) { // If actuator ii is already within tolerance
                 dblActPosChange = 0.0;
-                isTimeChanged = 1;
+                //isTimeChanged = 1;
             }
             //IS BELOW
-            if( input.duration < 0.000000001 ) {
+            if( input.duration < 0.000000001 ) { // If max (safe) velocity was requested
                 sgn = (dblActPosChange > 0) ? 1 : ((dblActPosChange < 0) ? -1 : 0);
                 _dblVelocity_mmps[ii] = sgn * MAX_SPEED_MMPS;
-                isTimeChanged = 1;
+                //isTimeChanged = 1;
             } else {
                 _dblVelocity_mmps[ii] = dblActPosChange / input.duration;
             }
             //IS ABOVE
-            // Check to see if velocities are achievable
+            // Check to see if velocities are achievable -- this can move into the else section of the above if statement
             if(abs(_dblVelocity_mmps[ii]) > MAX_SPEED_MMPS) {
                 if(_dblVelocity_mmps[ii]>0) {
                     _dblVelocity_mmps[ii] = MAX_SPEED_MMPS;
                 } else {
                     _dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
                 }
-                isTimeChanged = 1;
+                //isTimeChanged = 1;
             }
+            // Recalculate the move's time after altering the position and/or velocity
             double dblRecalculatedTime;
             if( fabs(_dblVelocity_mmps[ii]) < 0.000000001 ) {
                 dblRecalculatedTime = 0;
             } else {
                 dblRecalculatedTime = dblActPosChange / _dblVelocity_mmps[ii];
             }
+            // Find the maximum time for any actuator's move for synchronization
             if( dblRecalculatedTime > dblMaxRecalculatedTime ) {
                 dblMaxRecalculatedTime = dblRecalculatedTime;
             }
         }
-        if( isTimeChanged ) {
-            if( dblMaxRecalculatedTime >= 0.000000001 ) {
-                for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
-                    _dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblADCCurrentPosition[ii]) / dblMaxRecalculatedTime;
-                    dblVelocity_mmps[ii] = _dblVelocity_mmps[ii];
-                }
-            }
+        // Finally recalculate all of the velocities based upon this maximum time for synchronization
+        // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!MUTEX FOR dblVelocity_mmps and dblTargetActPos_mm??
+        //if( dblMaxRecalculatedTime >= 0.000000001 ) { // isTimeChanged && 
+        for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
+            // If sent a negative requested position, do NOT replan that actuator
+            //if( dblTargetChambLen_mm[ii] < 0.0 ) continue;
+            
+            _dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblADCCurrentPosition[ii]) / dblMaxRecalculatedTime;
+            dblVelocity_mmps[ii] = _dblVelocity_mmps[ii];
         }
+        // !!!!!!!!! IF TIME ISN'T CHANGED THEN VELOCITY ISN'T UPDATED.....................................................................................
         // SEND MESSAGE
         //dblMaxRecalculatedTime = 1.0;
         hlcomms.make_message(&dblMaxRecalculatedTime);
@@ -198,7 +209,7 @@
     //double dblPressure_bar[N_CHANNELS]; // The pressure in a given chamber in bar (1 bar  = 100,000 Pa)
     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(); 
@@ -218,7 +229,7 @@
                 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] = max( 0.0 , dblLinearPathCurrentPos_mm[jj] );
+            dblLinearPathCurrentPos_mm[jj] = min( max( 0.0 , dblLinearPathCurrentPos_mm[jj] ) , 25.0 );
             mutPathIn.unlock(); // Unlock relevant mutex
             
             // Calculate next step in smooth path
@@ -236,13 +247,14 @@
         //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() {    
+int main() {
     pc.baud(BAUD_RATE);
-    printf("Hi, there! I'll be your mid-level controller for today.\r\n");
+    printf("Sup bruvva! I'll be your mid-level controller for today.\r\n");
+    printf("Compiled at %s\r\n.",__TIME__);
     wait(3);
     
     timer.start();