Diff: main.cpp

Revision:

31:08cb04eb75fc

Parent:

29:10a5cf37a875

Child:

32:8c59c536a2a6

--- a/main.cpp	Fri Feb 15 10:29:23 2019 +0000
+++ b/main.cpp	Wed Mar 06 10:28:59 2019 +0000
@@ -51,53 +51,50 @@
         input = hlcomms.get_demands();
         
         // PROCESS INPUT
-        
-        /*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*/
-
-        // Limit requested speed
-        double maxTimeToTarget_s;
-        double limitedSpeed_mmps = min( max( 0.0 , input.speed_mmps ) , (double)MAX_SPEED_MMPS );
-        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;
+        double maxTimesToTarget_s[3];
+        for(short int segNum=0; segNum<3; segNum++) {
+            // Limit requested speed
+            double limitedSpeed_mmps = min( max( 0.0 , input.speeds_mmps[segNum] ) , (double)MAX_SPEED_MMPS );
+            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[3] = { -1.0 };
+                double maxDistanceToTarget_mm = 0.0;
+                for(int i=segNum*3; i<segNum*3+3; 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
+                    input.psi_mm[i] = min( max( 0.0 , input.psi_mm[i] ) , (double)MAX_ACTUATOR_LIMIT_MM );
+                    // Calculate actuator position change
+                    dblDistanceToTarget_mm[i] = fabs(input.psi_mm[i] - dblCurrentPosition_mm);
+                    // Select the max absolute actuator position change
+                    if(dblDistanceToTarget_mm[i]>maxDistanceToTarget_mm) {
+                        maxDistanceToTarget_mm = dblDistanceToTarget_mm[i];
+                    }
                 }
-                // Limit actuator position
-                input.psi_mm[i] = min( max( 0.0 , input.psi_mm[i] ) , (double)MAX_ACTUATOR_LIMIT_MM );
-                // Calculate actuator position change
-                dblDistanceToTarget_mm[i] = fabs(input.psi_mm[i] - dblCurrentPosition_mm);
-                // Select the max absolute actuator position change
-                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
+                maxTimesToTarget_s[segNum] = fabs(maxDistanceToTarget_mm) / limitedSpeed_mmps;
+                // For each actuator, replan target position and velocity as required
+                for(int i=segNum*3; i<segNum*3+3; 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] / maxTimesToTarget_s[segNum];
                 }
+                // Unlock mutex, allowing setDemandsForLL to run again
+                mutPathIn.unlock();
+            } else {
+                maxTimesToTarget_s[segNum] = -1;
             }
-            // 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;
         }
         
         // SEND MESSAGE
-        hlcomms.send_duration_message(maxTimeToTarget_s);
+        hlcomms.send_durations_message(maxTimesToTarget_s);
     }
 
 }
@@ -125,7 +122,7 @@
 
 int main() {
     pc.baud(BAUD_RATE);
-    printf("ML engage. Compiled at %s\r\n.",__TIME__);
+    //printf("ML engage. Compiled at %s\r\n.",__TIME__);
     wait(3);
     
     threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue