Diff: main.cpp

Revision:

19:e5acb2183d4e

Parent:

18:6533fb7f5a91

Child:

20:b2139294c547

--- a/main.cpp	Wed Oct 10 13:36:57 2018 +0000
+++ b/main.cpp	Tue Nov 27 16:52:06 2018 +0000
@@ -21,7 +21,6 @@
 // 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];
-double dblADCCurrentPosition[N_CHANNELS];
 
 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
 LLComms llcomms;
@@ -82,7 +81,6 @@
     
     SendSensorDataTicker.attach(&signalSendSensorData, 0.1); // Set up planning thread to recur at fixed intervals
     
-    int ii;
     struct msg_format input; //hlcomms.msg_format
     
     while( true ) {
@@ -103,31 +101,63 @@
         input = hlcomms.process_message();
         
         // PROCESS INPUT
-        double dblTargetChambLen_mm[N_CHANNELS]; // The currenly assigned final target position (actuator will reach this at end of path)
-        if(IS_PRINT_OUTPUT) printf("REPLAN, %f\r\n",input.duration);
+        if(IS_PRINT_OUTPUT) printf("REPLAN, %f\r\n",input.speed);
         
-        //update rear Segment
-        dblTargetChambLen_mm[3] = input.psi[0][0]*1000;
-        dblTargetChambLen_mm[5] = 0.0;//input.psi[0][1]*1000;//not used
-        dblTargetChambLen_mm[6] = 0.0;//input.psi[0][2]*1000;//not used
-        
-        //update mid segment
-        dblTargetChambLen_mm[4] = input.psi[1][0]*1000;
-        dblTargetChambLen_mm[7] = dblTargetChambLen_mm[4]; // Same because two pumps are used
-
+        double dblTarget_mm[N_CHANNELS]; // The currenly assigned final target position (actuator will reach this at end of path)
+        // Update rear segment
+        dblTarget_mm[3] = input.psi[0][0]*1000;
+        dblTarget_mm[5] = 0.0; //input.psi[0][1]*1000;
+        dblTarget_mm[6] = 0.0; //input.psi[0][2]*1000;
+        // Update mid segment
+        dblTarget_mm[4] = input.psi[1][0]*1000;
+        dblTarget_mm[7] = dblTarget_mm[4]; // Same because two pumps are used
         // Update front segment
-        dblTargetChambLen_mm[0] = input.psi[2][0]*1000;
-        dblTargetChambLen_mm[1] = input.psi[2][1]*1000;
-        dblTargetChambLen_mm[2] = input.psi[2][2]*1000;
+        dblTarget_mm[0] = input.psi[2][0]*1000;
+        dblTarget_mm[1] = input.psi[2][1]*1000;
+        dblTarget_mm[2] = input.psi[2][2]*1000;
 
-        mutPathIn.lock(); // Lock variables to avoid race condition
-        for(int j = 0; j<N_CHANNELS; j++) {
-            dblADCCurrentPosition[j] = dblLinearPathCurrentPos_mm[j];
-            //dblADCCurrentPosition[j] = llcomms.ReadADCPosition_mtrs(j); // Read position from channel
+        // Lock mutex, preventing CalculateSmoothPath from running
+        mutPathIn.lock();        
+        // Limit requested speed
+        double limitedSpeed_mmps = min( max( 0.0 , input.speed ) , (double)MAX_SPEED_MMPS );
+        // For each actuator, limit the input position, calculate the position change, and select the absolute max
+        double dblDisplacementToTarget_mm[N_CHANNELS];
+        double maxDistanceToTarget_mm = 0.0;
+        for(int i=0; i<N_CHANNELS; i++) {
+            // If requested position is negative
+            if(dblTarget_mm[i]<0) {
+                // Set actuator position change to 0
+                dblDisplacementToTarget_mm[i] = 0.0;
+            } else { // Requested position is positive
+                // ? 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
+                // Calculate actuator position change
+                double dblCurrentPosition = dblLinearPathCurrentPos_mm[i];
+                //dblCurrentPosition[i] = llcomms.ReadADCPosition_mtrs(i); // Read position from channel
+                dblDisplacementToTarget_mm[i] = dblTarget_mm[i] - dblCurrentPosition;
+                // Select the max absolute actuator position change
+                if(fabs(dblDisplacementToTarget_mm[i])>maxDistanceToTarget_mm) {
+                    maxDistanceToTarget_mm = fabs(dblDisplacementToTarget_mm[i]);
+                }
+            }
         }
+        // For max actuator position change, calculate the time to destination at the limited speed
+        double maxTimeToTarget_s = fabs(maxDistanceToTarget_mm) / limitedSpeed_mmps;
+        // For each actuator, replan target position and velocity as required
+        for(int i=0; i<N_CHANNELS; i++) {
+            // 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;
+        }
+        // Unlock mutex, allowing CalculateSmoothPath to run again
+        mutPathIn.unlock();
         
-        mutPathIn.unlock(); // Unlock mutex
-
+/*
         //bool isTimeChanged = 0;
         double dblMaxRecalculatedTime = input.duration;
         // Convert from requested chamber to actuator space and limit actuator positions
@@ -207,8 +237,9 @@
             _dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblADCCurrentPosition[ii]) / dblMaxRecalculatedTime;
             dblVelocity_mmps[ii] = _dblVelocity_mmps[ii];
         }
+*/
         // SEND MESSAGE
-        error_code = hlcomms.send_duration_message(&dblMaxRecalculatedTime);
+        error_code = hlcomms.send_duration_message(&maxTimeToTarget_s);
         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? "