Diff: main.cpp

Revision:

26:7c59002c9cd7

Parent:

25:88e6cccde856

Child:

27:6853ee8ffefd

--- a/main.cpp	Mon Dec 17 15:09:44 2018 +0000
+++ b/main.cpp	Mon Jan 28 21:24:48 2019 +0000
@@ -9,11 +9,11 @@
 #include "LLComms.h"
 
 // Maximum achievable mm path tolerance plus additional % tolerance
-const float FLT_PATH_TOLERANCE = MAX_SPEED_MMPS * (1/LL_DEMANDS_FREQ_HZ) * (1.0f+FLT_PERCENT_PATH_TOLERANCE);
+//const float FLT_PATH_TOLERANCE_MM = MAX_SPEED_MMPS * (1/LL_DEMANDS_FREQ_HZ) * (1.0f+FLT_PERCENT_PATH_TOLERANCE);
 
 // DEMAND VARIABLES
-double dblDemandVelocity_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
-double dblDemandPosition_mtrs[N_CHANNELS] = { 0.0 }; // The final target position for the actuator
+double dblDemandSpeed_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
+double dblDemandPosition_mm[N_CHANNELS] = { 0.0 }; // The final target position for the actuator
 
 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
 LLComms llcomms;
@@ -35,7 +35,7 @@
 void sendSensorData() {
     while( true ) {
         semSensorData.wait();
-        int error_code = hlcomms.send_sensor_message(llcomms.positionSensor_m,llcomms.pressureSensor_bar);
+        int error_code = hlcomms.send_sensor_message(llcomms.positionSensor_mm,llcomms.pressureSensor_bar);
         /*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? "
@@ -107,18 +107,17 @@
         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) {
+            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
                 // Limit actuator position
-                dblTarget_mm[i] = min( max( 0.0 , dblTarget_mm[i] ) , (double)MAX_ACTUATOR_LIMIT ); // !!! USE A CONSTANT FOR THIS
+                dblTarget_mm[i] = min( max( 0.0 , dblTarget_mm[i] ) , (double)MAX_ACTUATOR_LIMIT_MM );
                 // Calculate actuator position change
-                double dblCurrentPosition = llcomms.positionSensor_m[i];
-                dblDisplacementToTarget_mm[i] = dblTarget_mm[i] - dblCurrentPosition;
+                dblDisplacementToTarget_mm[i] = dblTarget_mm[i] - dblCurrentPosition_mm;
                 // Select the max absolute actuator position change
                 if(fabs(dblDisplacementToTarget_mm[i])>maxDistanceToTarget_mm) {
                     maxDistanceToTarget_mm = fabs(dblDisplacementToTarget_mm[i]);
@@ -130,11 +129,14 @@
         // 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;
+            //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_mtrs and dblDemandVelocity_mmps
-            dblDemandPosition_mtrs[i] = dblTarget_mm[i];
-            dblDemandVelocity_mmps[i] = dblDisplacementToTarget_mm[i] / maxTimeToTarget_s;
+            // 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();
@@ -156,22 +158,25 @@
     semLLcomms.release(); // Uses threadSetDemands which is normal priority
 }
 
+Timer timerA;
 void setDemandsForLL() {
     
     while(1) {
         semLLcomms.wait();
-        
         mutPathIn.lock(); // Lock relevant mutex
         for(short int i=0; i<N_CHANNELS; i++) { // For each LL
             llcomms.mutChannel[i].lock(); // MUTEX LOCK
-            llcomms.demandPosition[i] = dblDemandPosition_mtrs[i];
-            llcomms.demandSpeed[i] = dblDemandVelocity_mmps[i];
+            //llcomms.demandPosition_mm[i] = 2*sin(timerA.read())+4;
+            //llcomms.demandSpeed_mmps[i] = 4.0;
+            //printf("%d\t%0.5f\t%0.5f\r\n",i,dblDemandPosition_mm[i],dblDemandSpeed_mmps[i]);
+            llcomms.demandPosition_mm[i] = dblDemandPosition_mm[i];
+            llcomms.demandSpeed_mmps[i] = dblDemandSpeed_mmps[i];
             llcomms.mutChannel[i].unlock(); // MUTEX UNLOCK
             llcomms.isDataReady[i] = 1; // Signal that data ready
         } // end for
         mutPathIn.unlock(); // Unlock relevant mutex
     } // end while(1)
-        
+
 }
 
 int main() {
@@ -179,6 +184,8 @@
     printf("ML engage. Compiled at %s\r\n.",__TIME__);
     wait(3);
     
+    timerA.start();
+    
     threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
     threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
     threadSetDemands.start(setDemandsForLL); // Start planning thread