Diff: main.cpp

Revision:

2:eea12b149dba

Parent:

1:2a43cf183a62

Child:

3:c83291bf9fd2

--- a/main.cpp	Wed Jul 25 13:43:59 2018 +0000
+++ b/main.cpp	Tue Jul 31 10:44:10 2018 +0000
@@ -4,6 +4,21 @@
 //#include "mbed.h"
 #include "mbed_events.h"
 
+// COMMS
+/*#include "EthernetInterface.h"
+#include "TCPServer.h"
+#include "TCPSocket.h"
+#include <sstream> // stringstream
+#include <vector> // vector
+#include <string.h> // strtok
+const bool IS_DHCP = false;
+#define PORT 80
+struct msg_format {
+    double psi[3][3];
+    //unsigned short duration;
+    double duration;
+} input;*/
+
 //ADC SPI stuff
 #define PREAMBLE 0x06
 #define CHAN_1 0x30
@@ -37,7 +52,7 @@
 
 
 
-int ii,jj,kk; //counting varaibles
+int ii,jj; //counting varaibles
 
 //-----These are the variables being shared between High-Level and Mid-Level-----------------------------------------------//
 
@@ -61,9 +76,9 @@
 
 //path variables
 double dblVelocity_mmps[N_CHANNELS];//the linear path velocity (not sent to actuator)
-double dblLinearPath_mm[N_CHANNELS]; //the current position of the linear path (not sent to actuator)
-double dblSmoothPath_mm[N_CHANNELS]; //the current position of the smooth path (not sent to actuator)
-double dblTargetChambLen_mm[N_CHANNELS]; //the currenly assigned final target position (actuator will reach this at end of path)
+double dblLinearPathCurrentPos_mm[N_CHANNELS]; //the current position of the linear path (not sent to actuator)
+double dblSmoothPathCurrentPos_mm[N_CHANNELS]; //the current position of the smooth path (not sent to actuator)
+
 double dblTargetActLen_mm[N_CHANNELS];
 double dblPathToActuator[N_CHANNELS];//the target position for the actuator (sent to actuator)
 
@@ -242,6 +257,7 @@
 //this function will be called when a new transmission is received from high level
 void ReplanPath()
 {
+    
     //while(1)
     //{
         //semReplan.wait();//wait until called
@@ -252,7 +268,9 @@
 //        }
         
         mutPsi.lock();// lock mutex for PSI to ensure no conflict when receiving new messages while already replanning
+        //!!!!!!!!!!!!!!! If received messages faster than path replanning, will get increasingly behind time --> need to throw away some instructions
         
+        double dblTargetChambLen_mm[N_CHANNELS]; //the currenly assigned final target position (actuator will reach this at end of path)
         //update front segment
         dblTargetChambLen_mm[0] = dblPSI[0][0]*1000;
         dblTargetChambLen_mm[1] = dblPSI[0][1]*1000;
@@ -264,6 +282,9 @@
         dblTargetChambLen_mm[3] = dblPSI[2][0]*1000;
         dblTargetChambLen_mm[4] = dblPSI[2][1]*1000;
         dblTargetChambLen_mm[5] = dblPSI[2][2]*1000;
+       /* dblTargetChambLen_mm = { psi.
+        
+        dblTargetTime_s*/
         
         mutPsi.unlock();// unlock mutex for PSI to ensure no conflict when receiving new messages while already replanning
         
@@ -290,7 +311,7 @@
             dblTargetActLen_mm[ii] = dblTargetChambLen_mm[ii]*dblActuatorConversion[ii];
                 
             //work out new velocities
-            dblVelocity_mmps[ii] = (dblTargetActLen_mm[ii] - dblLinearPath_mm[ii]) / dblTargetTime_s;
+            dblVelocity_mmps[ii] = (dblTargetActLen_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblTargetTime_s;
             
             //check to see if velocities are achievable
             if(abs(dblVelocity_mmps[ii]) > MAX_SPEED_MMPS)
@@ -312,6 +333,14 @@
     
 }
 
+// lock mutex
+//  get variables
+// unlock mutex
+// do calculations
+// lock mutex
+//  set variables
+// unlock mutex 
+
 void CalculateSmoothPath()
 {
     double dblMeasuredSampleTime;
@@ -339,20 +368,20 @@
         {
             //calculate next step in linear path
             mutChannel[ii].lock();//lock relevant mutex
-            dblLinearPath_mm[ii] = dblLinearPath_mm[ii] + dblVelocity_mmps[ii]*dblMeasuredSampleTime; // PATH_SAMPLE_TIME_S SHOULD BE MEASURED
+            dblLinearPathCurrentPos_mm[ii] = dblLinearPathCurrentPos_mm[ii] + dblVelocity_mmps[ii]*dblMeasuredSampleTime; // PATH_SAMPLE_TIME_S SHOULD BE MEASURED
             
             //check tolerance
-            if (fabs(dblLinearPath_mm[ii] - dblTargetActLen_mm[ii]) <= dblPathTolerance)
+            if (fabs(dblLinearPathCurrentPos_mm[ii] - dblTargetActLen_mm[ii]) <= dblPathTolerance)
             {
                 dblVelocity_mmps[ii] = 0; //stop linear path generation when linear path is within tolerance of target position.
             }
             mutChannel[ii].unlock();//unlock relevant mutex
             
             //calculate next step in smooth path
-            dblSmoothPath_mm[ii] = dblSmoothingFactor*dblLinearPath_mm[ii] + (1.0-dblSmoothingFactor)*dblSmoothPath_mm[ii];
+            dblSmoothPathCurrentPos_mm[ii] = dblSmoothingFactor*dblLinearPathCurrentPos_mm[ii] + (1.0-dblSmoothingFactor)*dblSmoothPathCurrentPos_mm[ii];
             
             //convert to actuator distances
-            dblPathToActuator[ii] = dblSmoothPath_mm[ii];
+            dblPathToActuator[ii] = dblSmoothPathCurrentPos_mm[ii];
             
             intDemandPos_Tx[ii] = (int) (dblPathToActuator[ii]/MAX_ACTUATOR_LENGTH)*8191;//convert to a 13-bit number;
             intDemandPos_Tx[ii] = intDemandPos_Tx[ii] & 0x1FFF; //ensure number is 13-bit 
@@ -362,13 +391,13 @@
 
         }
         
-        //printf("%d, %f,%f,%f, %f\r\n",timer.read_ms(), dblTargetActLen_mm[0] ,dblVelocity_mmps[0], dblLinearPath_mm[0], dblSmoothPath_mm[0]);
+        //printf("%d, %f,%f,%f, %f\r\n",timer.read_ms(), dblTargetActLen_mm[0] ,dblVelocity_mmps[0], dblLinearPathCurrentPos_mm[0], dblSmoothPathCurrentPos_mm[0]);
     }
 }
 
-void SimulateDemand() // For testing purposes
+/*void SimulateDemand() // For testing purposes
 {
-    kk = 0;
+    int kk = 0;
     while(1)
     {
         mutChannel[0].lock();
@@ -393,7 +422,7 @@
     
         Thread::wait(7000);
     }
-}
+}*/