Diff: main.cpp

Revision:

22:82871f00f89d

Parent:

21:0b10d8e615d1

Child:

23:61526647cc8a

--- a/main.cpp	Tue Dec 11 15:45:01 2018 +0000
+++ b/main.cpp	Wed Dec 12 08:41:06 2018 +0000
@@ -8,15 +8,14 @@
 #include "HLComms.h"
 #include "LLComms.h"
 
+//!!!!!!!!!!!!!!!!!!!! CHECK POSITION SENSOR IS FROM 0 AND IN METERS
+
 // 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);
 
 // 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
-// SENSOR VARIABLES
-double dblPosition_mtrs[N_CHANNELS]; // The actual chamber lengths in meters given as the change in length relative to neutral (should always be >=0)
-double dblPressure_bar[N_CHANNELS]; // The pressure in a given chamber in bar (1 bar  = 100,000 Pa)
 
 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
 LLComms llcomms;
@@ -38,7 +37,7 @@
 void sendSensorData() {
     while( true ) {
         semSensorData.wait();
-        int error_code = hlcomms.send_sensor_message(dblPosition_mtrs,dblPressure_bar);
+        int error_code = hlcomms.send_sensor_message(llcomms.positionSensor_m,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? "
@@ -53,8 +52,8 @@
     semSensorData.release();
 }
 
-void startLLcomms() { // Plan a new linear path after receiving new target data
-    semLLcomms.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
+void startLLcomms() { // Send new demands to LL after receiving new target data
+    semLLcomms.release(); // Uses threadSetDemands which is normal priority
 }
 
 // This function will be called when a new transmission is received from high level
@@ -69,7 +68,7 @@
         return;
     }
     
-    SendSensorDataTicker.attach(&signalSendSensorData, 0.1); // Set up planning thread to recur at fixed intervals
+    SendSensorDataTicker.attach(&signalSendSensorData, 1/(float)SENSOR_FEEDBACK_HZ); // Set up planning thread to recur at fixed intervals
     
     struct msg_format input; //hlcomms.msg_format
     
@@ -124,7 +123,7 @@
                 // Limit actuator position
                 dblTarget_mm[i] = min( max( 0.0 , dblTarget_mm[i] ) , (double)MAX_ACTUATOR_LIMIT ); // !!! USE A CONSTANT FOR THIS
                 // Calculate actuator position change
-                double dblCurrentPosition = dblPosition_mtrs[i];
+                double dblCurrentPosition = llcomms.positionSensor_m[i];
                 dblDisplacementToTarget_mm[i] = dblTarget_mm[i] - dblCurrentPosition;
                 // Select the max absolute actuator position change
                 if(fabs(dblDisplacementToTarget_mm[i])>maxDistanceToTarget_mm) {
@@ -167,20 +166,15 @@
         mutPathIn.lock(); // Lock relevant mutex
         for(short int i=0; i<N_CHANNELS; i++) { // For each LL
             llcomms.mutChannel[i].lock(); // MUTEX LOCK
-// DO IT IN ONE BLOCK
-// SEND AND RECEIVE DATA AT SAME TIME
-// PASS RECEIVED DATA BACK
-// Send position to LL, receive position and store in global
             llcomms.demandPosition[i] = (int) ((dblDemandPosition_mtrs[i]/MAX_ACTUATOR_LENGTH)*8191); // Convert to a 13-bit number
             llcomms.demandPosition[i] = llcomms.demandPosition[i] & 0x1FFF; // Ensure number is 13-bit
-// Send speed to LL, receive pressure and store in global
             llcomms.demandSpeed[i] = (int) ((dblDemandVelocity_mmps[i]/MAX_ACTUATOR_SPEED)*8191);// Convert to a 13-bit number
             llcomms.demandSpeed[i] = llcomms.demandSpeed[i] & 0x1FFF; // Ensure number is 13-bit
             llcomms.mutChannel[i].unlock(); // MUTEX UNLOCK
+            llcomms.isDataReady[i] = 1; // Signal that data ready
         } // end for
         mutPathIn.unlock(); // Unlock relevant mutex
-          
-        llcomms.isDataReady[i] = 1; // Signal that data ready
+
     } // end while
         
 }