Diff: main.cpp

Revision:

17:bbaf3e8440ad

Parent:

16:1e2804a4e5bd

Child:

18:6533fb7f5a91

--- a/main.cpp	Tue Sep 11 10:10:26 2018 +0000
+++ b/main.cpp	Thu Oct 04 15:27:15 2018 +0000
@@ -30,13 +30,39 @@
 Thread threadLowLevelSPI(osPriorityRealtime);
 Thread threadSmoothPathPlan(osPriorityNormal);
 Thread threadReceiveAndReplan(osPriorityBelowNormal);
+Thread threadSensorFeedback(osPriorityBelowNormal);
 
 Mutex mutPathIn;
 Semaphore semPathPlan(1);
+Semaphore semSensorData(1);
 
 Timer timer;
 Ticker PathCalculationTicker;
-Ticker SendPressuresTicker;
+Ticker SendSensorDataTicker;
+
+
+void sendSensorData() {
+    while( true ) {
+        semSensorData.wait();
+        double pressures[N_CHANNELS];
+        // !!! READ ADC PRESSURE ASSUMES THAT LL ARE ON FIRST
+        for(short int i=0; i<N_CHANNELS; i++) {
+            pressures[i] = llcomms.ReadADCPressure_bar(i);
+        }
+        int error_code = hlcomms.send_sensor_message(dblLinearPathCurrentPos_mm,pressures);
+        /*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? "
+                "Or the socket is set to non-blocking or timed out?\n\r", error_code);
+            hlcomms.close_server();
+            return;
+        }*/
+    }
+}
+
+void signalSendSensorData() {
+    semSensorData.release();
+}
 
 void startPathPlan() { // Plan a new linear path after receiving new target data
     semPathPlan.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
@@ -54,6 +80,8 @@
         return;
     }
     
+    SendSensorDataTicker.attach(&signalSendSensorData, 1); // Set up planning thread to recur at fixed intervals
+    
     int ii;
     struct msg_format input; //hlcomms.msg_format
     
@@ -71,22 +99,12 @@
             hlcomms.close_server();
             return;
         }
+        // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! RECV MUTEX
         input = hlcomms.process_message();
         
         // PROCESS INPUT
         double dblTargetChambLen_mm[N_CHANNELS]; // The currenly assigned final target position (actuator will reach this at end of path)
         if(IS_PRINT_OUTPUT) printf("REPLAN, %f\r\n",input.duration);
-        // Update front segment
-        /*dblTargetChambLen_mm[0] = input.psi[0][0]*1000;
-        dblTargetChambLen_mm[1] = input.psi[0][1]*1000;
-        dblTargetChambLen_mm[2] = input.psi[0][2]*1000;
-        // Update mid segment
-        dblTargetChambLen_mm[6] = input.psi[1][0]*1000;
-        dblTargetChambLen_mm[7] = dblTargetChambLen_mm[6]; // Same because two pumps are used
-        // Update rear segment
-        dblTargetChambLen_mm[3] = input.psi[2][0]*1000;
-        dblTargetChambLen_mm[4] = input.psi[2][1]*1000;
-        dblTargetChambLen_mm[5] = input.psi[2][2]*1000;*/
         
         //update rear Segment
         dblTargetChambLen_mm[3] = input.psi[0][0]*1000;
@@ -103,10 +121,6 @@
         dblTargetChambLen_mm[2] = input.psi[2][2]*1000;
 
         mutPathIn.lock(); // Lock variables to avoid race condition
-        /*for(int j = 0; j<N_CHANNELS; j++) {
-            //_dblVelocity_mmps[j] = dblVelocity_mmps[j]; 
-            _dblLinearPathCurrentPos_mm[j] = dblLinearPathCurrentPos_mm[j];
-        }*/
         for(int j = 0; j<N_CHANNELS; j++) {
             dblADCCurrentPosition[j] = dblLinearPathCurrentPos_mm[j];
             //dblADCCurrentPosition[j] = llcomms.ReadADCPosition_mtrs(j); // Read position from channel
@@ -132,18 +146,22 @@
             // If sent a negative requested position, do NOT replan that actuator
             if( dblTargetChambLen_mm[ii] < 0.0 ) continue;
             
-            /*dblActPosChange = 1.0; // or = 0.0;
-            _dblVelocity_mmps[ii] = 0.0;*/ // DOESN'T CRASH
-            /*dblActPosChange = dblTargetActPos_mm[ii];
-            _dblVelocity_mmps[ii] = 0.0;*/ // DOESN'T CRASH
-            /*dblActPosChange = _dblLinearPathCurrentPos_mm[ii];*/ // DOESN'T CRASH
-            /*_dblVelocity_mmps[ii] = _dblLinearPathCurrentPos_mm[ii];*/ // DOESN'T CRASH
-            /*dblActPosChange = 0.0;
-            _dblVelocity_mmps[ii] = _dblLinearPathCurrentPos_mm[ii];*/ // DOESN'T CRASH
+            //dblActPosChange = 1.0; // or = 0.0;
+            //_dblVelocity_mmps[ii] = 0.0; // DOESN'T CRASH
+            
+            //dblActPosChange = dblTargetActPos_mm[ii];
+            //_dblVelocity_mmps[ii] = 0.0; // DOESN'T CRASH
+            
+            //dblActPosChange = _dblLinearPathCurrentPos_mm[ii]; // DOESN'T CRASH
+            
+            //_dblVelocity_mmps[ii] = _dblLinearPathCurrentPos_mm[ii]; // DOESN'T CRASH
+            
+            //dblActPosChange = 0.0;
+            //_dblVelocity_mmps[ii] = _dblLinearPathCurrentPos_mm[ii]; // DOESN'T CRASH
             
             // DOES CRASH but not with a return at the end of while OR if _ variables are declared globally
-            /*dblActPosChange = _dblLinearPathCurrentPos_mm[ii];
-            _dblVelocity_mmps[ii] = 0.0;*/
+            //dblActPosChange = _dblLinearPathCurrentPos_mm[ii];
+            //_dblVelocity_mmps[ii] = 0.0;
             
             dblActPosChange = dblTargetActPos_mm[ii] - dblADCCurrentPosition[ii];
             if( fabs(dblActPosChange) < FLT_PATH_TOLERANCE ) { // If actuator ii is already within tolerance
@@ -189,10 +207,10 @@
             _dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblADCCurrentPosition[ii]) / dblMaxRecalculatedTime;
             dblVelocity_mmps[ii] = _dblVelocity_mmps[ii];
         }
+        // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! MUTEX TO PROTECT SEND_BUFFER
         // SEND MESSAGE
         //dblMaxRecalculatedTime = 1.0;
-        hlcomms.make_message(&dblMaxRecalculatedTime);
-        error_code = hlcomms.send_message();
+        error_code = hlcomms.send_duration_message(&dblMaxRecalculatedTime);
         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? "
@@ -254,46 +272,9 @@
     } // end while
 }
 
-//short int whichVar = 0;
-void sendPressures() {
-    pc.printf("%f,%f\r\n",dblLinearPathCurrentPos_mm[1],llcomms.ReadADCPressure_bar(1));
-    /*switch(whichVar) {
-        case 0 : pc.printf("%f,",llcomms.ReadADCPressure_bar(???));
-                 break;
-        case 1 : pc.printf("%f,",dblLinearPathCurrentPos_mm[???]);
-                 break;
-        case 2 : pc.printf("%f,",llcomms.ReadADCPressure_bar(???));
-                 break;
-        case 3 : pc.printf("%f,",dblLinearPathCurrentPos_mm[???]);
-                 break;
-        case 4 : pc.printf("%f,",llcomms.ReadADCPressure_bar(???));
-                 break;
-        case 5 : pc.printf("%f,",dblLinearPathCurrentPos_mm[???]);
-                 break;
-        case 6 : pc.printf("%f,",llcomms.ReadADCPressure_bar(???));
-                 break;
-        case 7 : pc.printf("%f,",dblLinearPathCurrentPos_mm[???]);
-                 break;
-        case 0 : pc.printf("%f,",llcomms.ReadADCPressure_bar(???));
-                 break;
-        case 1 : pc.printf("%f,",dblLinearPathCurrentPos_mm[???]);
-                 break;
-        case 0 : pc.printf("%f,",llcomms.ReadADCPressure_bar(???));
-                 break;
-        case 1 : pc.printf("%f,",dblLinearPathCurrentPos_mm[???]);
-                 break;
-    }
-    if( whichVar < 8 ) {
-        whichVar++;
-    } else {
-        whichVar = 0;
-    }*/
-}
-
 int main() {
     pc.baud(BAUD_RATE);
-    printf("Sup bruvva! I'll be your mid-level controller for today.\r\n");
-    printf("Compiled at %s\r\n.",__TIME__);
+    printf("ML engage. Compiled at %s\r\n.",__TIME__);
     wait(3);
     
     timer.start();
@@ -301,9 +282,9 @@
     threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
     threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
     threadSmoothPathPlan.start(CalculateSmoothPath); // Start planning thread
+    threadSensorFeedback.start(sendSensorData); // Start sensor feedback thread
 
     PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); // Set up planning thread to recur at fixed intervals
-    SendPressuresTicker.attach(&sendPressures, 0.1); // Set up planning thread to recur at fixed intervals
     
     Thread::wait(1);
     while(1) {