Diff: main.cpp

Revision:

28:8e0c502c1a50

Parent:

27:6853ee8ffefd

Child:

29:10a5cf37a875

--- a/main.cpp	Tue Jan 29 14:58:45 2019 +0000
+++ b/main.cpp	Wed Feb 06 16:10:18 2019 +0000
@@ -14,7 +14,7 @@
 
 Serial pc(USBTX, USBRX); // tx, rx for usb debugging
 LLComms llcomms;
-HLComms hlcomms(SERVER_PORT);
+HLComms hlcomms(HL_COMMS_FREQ_HZ);
 
 Thread threadLowLevelSPI(osPriorityRealtime);
 Thread threadSetDemands(osPriorityNormal);
@@ -28,18 +28,10 @@
 Ticker setDemandsTicker;
 Ticker SendSensorDataTicker;
 
-
 void sendSensorData() {
     while( true ) {
         semSensorData.wait();
-        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? "
-                "Or the socket is set to non-blocking or timed out?\n\r", error_code);
-            hlcomms.close_server();
-            return;
-        }*/
+        hlcomms.send_sensor_message(llcomms.positionSensor_mm,llcomms.pressureSensor_bar);
     }
 }
 
@@ -50,56 +42,36 @@
 // This function will be called when a new transmission is received from high level
 void ReceiveAndReplan() {
     
-    int error_code;
-    error_code = hlcomms.setup_server();
-    if( error_code == -1 ) return;
-    error_code = hlcomms.accept_connection();
-    if( error_code == -1 ) {
-        hlcomms.close_server();
-        return;
-    }
-    
     SendSensorDataTicker.attach(&signalSendSensorData, 1/(float)SENSOR_FEEDBACK_HZ); // Set up planning thread to recur at fixed intervals
     
-    struct msg_format input; //hlcomms.msg_format
+    struct demands_struct input;
     
     while( true ) {
-        // RECEIVE MESSAGE
-        error_code = hlcomms.receive_message();
-        if( error_code == NSAPI_ERROR_NO_CONNECTION ) { // -3004
-            if(IS_PRINT_OUTPUT) printf("Client disconnected.\n\r");
-            hlcomms.close_server();
-            return;
-        } else 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 connected to a remote host? "
-                "Or the socket is set to non-blocking or timed out?\n\r", error_code);
-            hlcomms.close_server();
-            return;
-        }
-        // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! RECV MUTEX
-        input = hlcomms.process_message();
+        hlcomms.newData.wait();
+        input = hlcomms.get_demands();
         
         // PROCESS INPUT
-        if(IS_PRINT_OUTPUT) printf("REPLAN, %f\r\n",input.speed);
         
         double dblTarget_mm[N_CHANNELS]; // The currenly assigned final target position (actuator will reach this at end of path)
         // Update rear segment
-        dblTarget_mm[3] = input.psi[0][0]*1000;
-        dblTarget_mm[5] = 0.0; //input.psi[0][1]*1000;
-        dblTarget_mm[6] = 0.0; //input.psi[0][2]*1000;
+        dblTarget_mm[3] = input.psi_mm[0]*1000;
+        dblTarget_mm[5] = -1.0;
+        dblTarget_mm[6] = -1.0;
         // Update mid segment
-        dblTarget_mm[4] = input.psi[1][0]*1000;
+        dblTarget_mm[4] = input.psi_mm[3]*1000;
         dblTarget_mm[7] = dblTarget_mm[4]; // Same because two pumps are used
         // Update front segment
-        dblTarget_mm[0] = input.psi[2][0]*1000;
-        dblTarget_mm[1] = input.psi[2][1]*1000;
-        dblTarget_mm[2] = input.psi[2][2]*1000;
+        dblTarget_mm[0] = input.psi_mm[6]*1000;
+        dblTarget_mm[1] = input.psi_mm[7]*1000;
+        dblTarget_mm[2] = input.psi_mm[8]*1000;
+        /*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*/
 
         // Lock mutex, preventing setDemandsForLL from running
         mutPathIn.lock();        
         // Limit requested speed
-        double limitedSpeed_mmps = min( max( 0.0 , input.speed ) , (double)MAX_SPEED_MMPS );
+        double limitedSpeed_mmps = min( max( 0.0 , input.speed_mmps ) , (double)MAX_SPEED_MMPS );
         // For each actuator, limit the input position, calculate the position change, and select the absolute max
         double dblDisplacementToTarget_mm[N_CHANNELS];
         double maxDistanceToTarget_mm = 0.0;
@@ -139,14 +111,7 @@
         mutPathIn.unlock();
         
         // SEND MESSAGE
-        error_code = hlcomms.send_duration_message(&maxTimeToTarget_s);
-        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;
-        }
+        hlcomms.send_duration_message(maxTimeToTarget_s);
     }
 
 }
@@ -155,7 +120,6 @@
     semLLcomms.release(); // Uses threadSetDemands which is normal priority
 }
 
-Timer timerA;
 void setDemandsForLL() {
     
     while(1) {
@@ -163,9 +127,6 @@
         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_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
@@ -181,8 +142,6 @@
     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