Diff: main.cpp

Revision:

5:712e7634c779

Parent:

4:303584310071

Child:

6:f0a18e28a322

--- a/main.cpp	Tue Jul 31 17:54:29 2018 +0000
+++ b/main.cpp	Wed Aug 01 09:51:23 2018 +0000
@@ -33,7 +33,7 @@
 #define MAX_SPEED_MMPS 24.3457
 #define PATH_TOLERANCE_MM 0.2 //how close the linear path must get to the target position before considering it a success.
 
-const double DBL_MAX_CHAMBER_LENGTHS_MM[N_CHANNELS] = {100.0,50.0,50.0,50.0,50.0,50.0,30.0,30.0};
+const double DBL_MAX_CHAMBER_LENGTHS_MM[N_CHANNELS] = {500.0,50.0,50.0,50.0,50.0,50.0,30.0,30.0};
 const double DBL_ACTUATOR_CONVERSION[N_CHANNELS] = {0.24176,0.24176,0.24176,0.24176,0.24176,0.36264,0.36264};// covert from chamber lengths to actuator lengths
 const double DBL_SMOOTHING_FACTOR = 0.5; // 0<x<=1, where 1 is no smoothing
 const double DBL_PATH_TOLERANCE = 0.2;
@@ -100,7 +100,8 @@
 
 bool isDataReady[N_CHANNELS]; // flag to indicate path data is ready for transmission to low level.
 
-
+double dblGlobalTest;
+int intGlobalTest;
 double ReadADCPosition_mtrs(int channel)
 {
     unsigned int outputA;
@@ -121,8 +122,13 @@
     outputA = outputA<<8;
     output = (outputA | outputB);
     output = 4095- output;
-    dblOutput = (double) ((output - 1504)/4095*0.1);
+    dblOutput = (double) (output);
+    dblOutput = dblOutput*0.0229 - 21.582;
+    //dblOutput = dblOutput - 21.78;
     return dblOutput;
+    //dblOutput = (double) ((output/4095)*100.0);
+//    dblOutput = dblOutput - 36.727717;
+//    return dblOutput;
 }
     
 double ReadADCPressure_bar(int channel)
@@ -145,7 +151,9 @@
     outputA = outputA<<8;
     output = (outputA | outputB);
     
-    dblOutput = (double) ((output-502)/4095*8.0);
+    dblOutput = (double)(output);
+    dblOutput = dblOutput-502.0;
+    dblOutput = dblOutput/4095.0*8.0;
     return dblOutput;
 }
 
@@ -164,8 +172,11 @@
     intPosSPI_Rx[channel] = spi.write(intDemandPos_Tx[channel]); //transmit & receive
     cs_LL[channel] = 1;//deselect chip
     isDataReady[channel] = 0;//data no longer ready, i.e. we now require new data
+    if(channel == 0) {
+        intGlobalTest = intPosSPI_Rx[channel];
+        dblGlobalTest = ((double) (intPosSPI_Rx[channel])/8191.0*52.2);
+    }
     
-
     
     //sort out received data
     chrErrorFlag[channel] = intPosSPI_Rx[channel]>>13;
@@ -228,6 +239,8 @@
     semPathPlan.release(); // Uses threadReceiveAndReplan which is below normal priority to ensure consistent transmission to LL
 }
 
+double somedumbthing;
+
 //this function will be called when a new transmission is received from high level
 void ReceiveAndReplan()
 {
@@ -240,9 +253,9 @@
              L(rear,1) ,   L(rear,2) ,    L(rear,3);}
             */
     double dblTargetTime_s; //the time in seconds desired to reach the target (>0...!)
-    
-    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).
+    //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).    
+
     
     //!!!!!!!!!!!!!!! If received messages faster than path replanning, will get increasingly behind time --> need to throw away some instructions
     
@@ -254,11 +267,11 @@
     while(1) {
         
         if(kk == 0) {
-            dblPSI[0][0] = (double) 10.0;
-            dblTargetTime_s = 1.5;
+            dblPSI[0][0] = (double) 0.20;
+            dblTargetTime_s = 3.0;
         } else { 
             dblPSI[0][0] = (double) 0.0; 
-            dblTargetTime_s = 2.0;
+            dblTargetTime_s = 3.0;
         }
         kk = !kk;
         printf("REPLAN, %f,\r\n", dblPSI[0][0] );
@@ -283,25 +296,25 @@
                 dblTargetChambLen_mm[ii] = 0.0; 
             }
             
-            //mutPathIn.lock();//lock variables to avoid race condition
-            
-            dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*DBL_ACTUATOR_CONVERSION[ii];
-            
-            //work out new velocities
-            dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblTargetTime_s;
+            mutPathIn.lock();//lock variables to avoid race condition
+                dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*DBL_ACTUATOR_CONVERSION[ii];
+                
+                //work out new velocities
+                dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblTargetTime_s;
+                
+                //check to see if velocities are achievable
+                if(abs(dblVelocity_mmps[ii]) > MAX_SPEED_MMPS) {
+                    if(dblVelocity_mmps[ii]>0) {
+                        dblVelocity_mmps[ii] = MAX_SPEED_MMPS;
+                    } else {
+                        dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
+                    }
+                }
+            mutPathIn.unlock(); //unlock mutex.
             
-            //check to see if velocities are achievable
-            if(abs(dblVelocity_mmps[ii]) > MAX_SPEED_MMPS) {
-                if(dblVelocity_mmps[ii]>0) {
-                    dblVelocity_mmps[ii] = MAX_SPEED_MMPS;
-                } else {
-                    dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
-                }
-            }
-            //mutPathIn.unlock(); //unlock mutex.
             
-            dblPressure_bar[ii] = ReadADCPressure_bar(ii);//read pressure from channel
-            dblPosition_mtrs[ii] = ReadADCPosition_mtrs(ii); //read position from channel
+            //dblPressure_bar[ii] = 0.5;//read pressure from channel
+//            dblPosition_mtrs[ii] = 0.02; //read position from channel
             
             //send info back to HL
         } // end of for
@@ -319,50 +332,59 @@
 // unlock mutex
 
 void CalculateSmoothPath() {
-    int ii;
+    int jj;
     double dblMeasuredSampleTime;
     double dblSmoothPathCurrentPos_mm[N_CHANNELS] = { 0.0 }; //the current position of the smooth path (not sent to actuator)
+    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).    
     
     while(1) {
         semPathPlan.wait();
         
         // If run time is more than 50 us from expected, calculate from measured time step
         if (fabs(PATH_SAMPLE_TIME_S*1000000 - timer.read_us()) > 50) {
-            dblMeasuredSampleTime = timer.read();
+            dblMeasuredSampleTime = timer.read(); 
         } else {
             dblMeasuredSampleTime = PATH_SAMPLE_TIME_S;
         }
         timer.reset();
             
-        for(ii = 0; ii < N_CHANNELS; ii++) {
-            if(ii == 0) printf("BEFORE: %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[0],dblVelocity_mmps[0],dblMeasuredSampleTime);
-            //calculate next step in linear path
-            //mutPathIn.lock();//lock relevant mutex
-            dblLinearPathCurrentPos_mm[ii] = dblLinearPathCurrentPos_mm[ii] + dblVelocity_mmps[ii]*dblMeasuredSampleTime; // PATH_SAMPLE_TIME_S SHOULD BE MEASURED
+        for(jj = 0; jj < N_CHANNELS; jj++) {
+            
+            dblPressure_bar[jj] = ReadADCPressure_bar(jj);//read pressure from channel
+            dblPosition_mtrs[jj] = ReadADCPosition_mtrs(jj); //read position from channel
+            //DEBUGGING
+            //if(jj == 0) printf("BEFORE: %f, %f, %f\r\n",dblVelocity_mmps[0],dblPosition_mtrs[0], dblMeasuredSampleTime);
             
-            if(dblLinearPathCurrentPos_mm[ii] < 0.0) {
-                dblLinearPathCurrentPos_mm[ii] = 0.00;
-            }
-            
-            //check tolerance
-            if (fabs(dblLinearPathCurrentPos_mm[ii] - dblTargetActPos_mm[ii]) <= DBL_PATH_TOLERANCE) {
-                dblVelocity_mmps[ii] = 0.0; //stop linear path generation when linear path is within tolerance of target position.
-            }
-            //mutPathIn.unlock();//unlock relevant mutex
+            //calculate next step in linear path
+            mutPathIn.lock();//lock relevant mutex
+                dblLinearPathCurrentPos_mm[jj] = dblLinearPathCurrentPos_mm[jj] + dblVelocity_mmps[jj]*dblMeasuredSampleTime; // PATH_SAMPLE_TIME_S SHOULD BE MEASURED
+                if(dblLinearPathCurrentPos_mm[jj] < 0.0) {
+                    dblLinearPathCurrentPos_mm[jj] = 0.00;
+                }
+                
+                //check tolerance
+                if (fabs(dblLinearPathCurrentPos_mm[jj] - dblTargetActPos_mm[jj]) <= DBL_PATH_TOLERANCE) {
+                    dblVelocity_mmps[jj] = 0.0; //stop linear path generation when linear path is within tolerance of target position.
+                }
+            mutPathIn.unlock();//unlock relevant mutex
             
             //calculate next step in smooth path
-            dblSmoothPathCurrentPos_mm[ii] = DBL_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[ii] + (1.0-DBL_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[ii];
-            //mutChannel[ii].lock();
-            intDemandPos_Tx[ii] = (int) ((dblSmoothPathCurrentPos_mm[ii]/MAX_ACTUATOR_LENGTH)*8191);//convert to a 13-bit number;
-            intDemandPos_Tx[ii] = intDemandPos_Tx[ii] & 0x1FFF; //ensure number is 13-bit 
-            //mutChannel[ii].unlock();
-            if(ii == 0) printf("AFTER: %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[ii],dblVelocity_mmps[ii],dblMeasuredSampleTime);
+            dblSmoothPathCurrentPos_mm[jj] = DBL_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[jj] + (1.0-DBL_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[jj];
+            
+            mutChannel[jj].lock(); //MUTEX LOCK
+                intDemandPos_Tx[jj] = (int) ((dblSmoothPathCurrentPos_mm[jj]/MAX_ACTUATOR_LENGTH)*8191);//convert to a 13-bit number;
+                intDemandPos_Tx[jj] = intDemandPos_Tx[jj] & 0x1FFF; //ensure number is 13-bit 
+            mutChannel[jj].unlock(); //MUTEX UNLOCK
+            //if(jj == 0) printf("AFTER: %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[jj],dblVelocity_mmps[jj],dblMeasuredSampleTime);
           
-            isDataReady[ii] = 1;//signal that data ready
+            isDataReady[jj] = 1;//signal that data ready
 
         } // end for
         //printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]); 
-        //printf("%f,%f, %f, %f\r\n", dblTargetActPos_mm[0], dblSmoothPathCurrentPos_mm[0],dblPressure_bar[0], dblPosition_mtrs[0]);
+        mutChannel[0].lock();
+        printf("%f, %f, %f\r\n",dblSmoothPathCurrentPos_mm[0],dblPressure_bar[0], dblPosition_mtrs[0]);
+        mutChannel[0].unlock();
     } // end while
 }
 
@@ -390,15 +412,15 @@
     //dblPathTolerance = 0.1;//MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; //path tolerance in mm. 
     
     pinReset = 1; //initialise reset pin to not reset the controllers.
-    wait(0.1);
+    wait(0.25);
     pinReset=0; //reset controllers to be safe
-    wait(0.1);
+    wait(0.25);
     pinReset = 1;//ready to go
     
     //say something nice to the user.
-    pc.baud(9600);
+    pc.baud(115200);
     printf("Hi, there! I'll be your mid-level controller for today.\r\n");
-    
+    wait(3);
     // Start the event queue
     t.start(callback(&queue, &EventQueue::dispatch_forever));
     
@@ -411,7 +433,7 @@
     //testPinGate.rise(&rise0);
     
     pinGate0.rise(&rise0);
-    /*
+    
     pinGate1.rise(&rise1);
     pinGate2.rise(&rise2);
     pinGate3.rise(&rise3);
@@ -419,7 +441,7 @@
     pinGate5.rise(&rise5);
     pinGate6.rise(&rise6);
     pinGate7.rise(&rise7);
-    */
+    
     
     //set up fall interrupts MIGHT NOT NEED TO BE POINTERS
     //
@@ -428,7 +450,7 @@
     //testPinGate.fall(&fall0);
     
     pinGate0.fall(&fall0);
-    /*
+    
     pinGate1.fall(&fall1);
     pinGate2.fall(&fall2);
     pinGate3.fall(&fall3);
@@ -436,7 +458,7 @@
     pinGate5.fall(&fall5);
     pinGate6.fall(&fall6);
     pinGate7.fall(&fall7);
-    */
+    
     
     timer.start();