Diff: main.cpp

Revision:

32:8c59c536a2a6

Parent:

31:08cb04eb75fc

Child:

33:9877ca32e43c

--- a/main.cpp	Wed Mar 06 10:28:59 2019 +0000
+++ b/main.cpp	Fri Mar 08 12:44:29 2019 +0000
@@ -49,27 +49,34 @@
     while( true ) {
         hlcomms.newData.wait();
         input = hlcomms.get_demands();
-        
         // PROCESS INPUT
-        double maxTimesToTarget_s[3];
+        double maxTimesToTarget_s[3] = { -1.0 };
+        //[8,7,6,4,3,-1,-1,0,-1]
+        //FRONT = channels 0,1,2
+        //MID = channels 3,4(,5)
+        //REAR = channels (6,)7(,8)
+        // Lock mutex, preventing setDemandsForLL from running
+        mutPathIn.lock();
         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();
+            if( input.speeds_mmps[segNum] > 0 ) {
+                double limitedSpeed_mmps = min( max( 0.0 , input.speeds_mmps[segNum] ) , (double)MAX_SPEED_MMPS );
                 // 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
+                for(short int i=0; i<3; i++) {
+                    short int channel = segNum*3+i;
+                    if(channel==8) { // !!!!!!!!!!!!!!!! This is horrible
+                        continue;
+                    }
+                    double dblCurrentPosition_mm = llcomms.positionSensor_mm[channel];
+                    if(input.psi_mm[channel]<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 );
+                    input.psi_mm[channel] = min( max( 0.0 , input.psi_mm[channel] ) , (double)MAX_ACTUATOR_LIMIT_MM );
                     // Calculate actuator position change
-                    dblDistanceToTarget_mm[i] = fabs(input.psi_mm[i] - dblCurrentPosition_mm);
+                    dblDistanceToTarget_mm[i] = fabs(input.psi_mm[channel] - dblCurrentPosition_mm);
                     // Select the max absolute actuator position change
                     if(dblDistanceToTarget_mm[i]>maxDistanceToTarget_mm) {
                         maxDistanceToTarget_mm = dblDistanceToTarget_mm[i];
@@ -78,20 +85,21 @@
                 // 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++) {
+                for(short int i=0; i<3; i++) {
+                    short int channel = segNum*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];
+                    dblDemandPosition_mm[channel] = input.psi_mm[channel];
+                    dblDemandSpeed_mmps[channel] = dblDistanceToTarget_mm[i] / maxTimesToTarget_s[segNum];
                 }
-                // Unlock mutex, allowing setDemandsForLL to run again
-                mutPathIn.unlock();
             } else {
-                maxTimesToTarget_s[segNum] = -1;
+                maxTimesToTarget_s[segNum] = -1.0;
             }
         }
+        // Unlock mutex, allowing setDemandsForLL to run again
+        mutPathIn.unlock();
         
         // SEND MESSAGE
         hlcomms.send_durations_message(maxTimesToTarget_s);