Diff: main.cpp

Revision:

29:10a5cf37a875

Parent:

28:8e0c502c1a50

Child:

31:08cb04eb75fc

--- a/main.cpp	Wed Feb 06 16:10:18 2019 +0000
+++ b/main.cpp	Fri Feb 08 18:36:15 2019 +0000
@@ -31,7 +31,7 @@
 void sendSensorData() {
     while( true ) {
         semSensorData.wait();
-        hlcomms.send_sensor_message(llcomms.positionSensor_mm,llcomms.pressureSensor_bar);
+        hlcomms.send_sensor_message(llcomms.positionSensor_uint,llcomms.pressureSensor_uint);
     }
 }
 
@@ -52,63 +52,49 @@
         
         // PROCESS INPUT
         
-        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_mm[0]*1000;
-        dblTarget_mm[5] = -1.0;
-        dblTarget_mm[6] = -1.0;
-        // Update mid segment
-        dblTarget_mm[4] = input.psi_mm[3]*1000;
-        dblTarget_mm[7] = dblTarget_mm[4]; // Same because two pumps are used
-        // Update front segment
-        dblTarget_mm[0] = input.psi_mm[6]*1000;
-        dblTarget_mm[1] = input.psi_mm[7]*1000;
-        dblTarget_mm[2] = input.psi_mm[8]*1000;
         /*input.psi_mm[5] = -1; // Disable additional mid actuator
         input.psi_mm[7] = -1; // Disable additional rear actuator
         input.psi_mm[8] = -1; // Disable additional rear actuator*/
 
-        // Lock mutex, preventing setDemandsForLL from running
-        mutPathIn.lock();        
         // Limit requested speed
+        double maxTimeToTarget_s;
         double limitedSpeed_mmps = min( max( 0.0 , input.speed_mmps ) , (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++) {
-            double dblCurrentPosition_mm = llcomms.positionSensor_mm[i];
-            if(dblTarget_mm[i]<0 || dblCurrentPosition_mm<0) { // If requested position is negative or the sensor feedback is erroneous
-                // 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
+        if( limitedSpeed_mmps > 0 ) {
+            // Lock mutex, preventing setDemandsForLL from running
+            mutPathIn.lock();
+            // For each actuator, limit the input position, calculate the position change, and select the absolute max
+            double dblDistanceToTarget_mm[N_CHANNELS] = { -1.0 };
+            double maxDistanceToTarget_mm = 0.0;
+            for(int i=0; i<N_CHANNELS; i++) {
+                double dblCurrentPosition_mm = llcomms.positionSensor_mm[i];
+                if(input.psi_mm[i]<0 || dblCurrentPosition_mm<0) { // If requested position is negative or the sensor feedback is erroneous
+                    continue;
+                }
                 // Limit actuator position
-                dblTarget_mm[i] = min( max( 0.0 , dblTarget_mm[i] ) , (double)MAX_ACTUATOR_LIMIT_MM );
+                input.psi_mm[i] = min( max( 0.0 , input.psi_mm[i] ) , (double)MAX_ACTUATOR_LIMIT_MM );
                 // Calculate actuator position change
-                dblDisplacementToTarget_mm[i] = dblTarget_mm[i] - dblCurrentPosition_mm;
+                dblDistanceToTarget_mm[i] = fabs(input.psi_mm[i] - dblCurrentPosition_mm);
                 // Select the max absolute actuator position change
-                if(fabs(dblDisplacementToTarget_mm[i])>maxDistanceToTarget_mm) {
-                    maxDistanceToTarget_mm = fabs(dblDisplacementToTarget_mm[i]);
+                if(dblDistanceToTarget_mm[i]>maxDistanceToTarget_mm) {
+                    maxDistanceToTarget_mm = dblDistanceToTarget_mm[i];
                 }
             }
+            // For max actuator position change, calculate the time to destination at the limited speed
+            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( dblDistanceToTarget_mm[i] <= 0.0 ) continue;
+                // Calculate velocity for each motor to synchronise movements to complete in max time
+                // Set dblDemandPosition_mm and dblDemandSpeed_mmps
+                dblDemandPosition_mm[i] = input.psi_mm[i];
+                dblDemandSpeed_mmps[i] = dblDistanceToTarget_mm[i] / maxTimeToTarget_s;
+            }
+            // Unlock mutex, allowing setDemandsForLL to run again
+            mutPathIn.unlock();
+        } else {
+            maxTimeToTarget_s = -1;
         }
-        // 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
-            //printf("%d\t%0.5f\t%0.5f\r\n",i,dblDisplacementToTarget_mm[i],FLT_PATH_TOLERANCE_MM);
-            //printf("%d\t%0.5f\t%0.5f\t%0.5f\r\n",i,dblTarget_mm[i],fabs(dblDisplacementToTarget_mm[i]),maxTimeToTarget_s);
-            if( fabs(dblDisplacementToTarget_mm[i]) < FLT_PATH_TOLERANCE_MM ) continue;
-            // Calculate velocity for each motor to synchronise movements to complete in max time
-            // Set dblDemandPosition_mm and dblDemandSpeed_mmps
-            //printf("%d\t%0.5f\t%0.5f\t%0.5f\r\n",i,dblTarget_mm[i],fabs(dblDisplacementToTarget_mm[i]),maxTimeToTarget_s);
-            dblDemandPosition_mm[i] = dblTarget_mm[i];
-            dblDemandSpeed_mmps[i] = fabs(dblDisplacementToTarget_mm[i]) / maxTimeToTarget_s;
-        }
-        // Unlock mutex, allowing setDemandsForLL to run again
-        mutPathIn.unlock();
         
         // SEND MESSAGE
         hlcomms.send_duration_message(maxTimeToTarget_s);