MorphGI
Mid level control code
Dependencies: ros_lib_kinetic
Diff: main.cpp
- Revision:
- 12:595ed862e52f
- Parent:
- 11:7029367a1840
- Child:
- 13:a373dfc57b89
--- a/main.cpp Fri Aug 10 10:40:18 2018 +0000
+++ b/main.cpp Mon Aug 13 09:16:29 2018 +0000
@@ -4,22 +4,25 @@
#include "mbed.h"
#include "mbed_events.h"
// CUSTOM IMPORTS
-#include "MLOptions.h"
+#include "MLSettings.h"
#include "HLComms.h"
#include "LLComms.h"
-const double DBL_PATH_TOLERANCE = MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * (1.0+DBL_PERCENT_PATH_TOLERANCE); // Path tolerance in mm with 5% tolerance
+// Maximum achievable mm path tolerance plus additional % tolerance
+const float FLT_PATH_TOLERANCE = MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * (1.0f+FLT_PERCENT_PATH_TOLERANCE);
// PATH VARIABLES
double dblVelocity_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
double dblLinearPathCurrentPos_mm[N_CHANNELS]={ 0.0 }; // The current position of the linear path (not sent to actuator)
double dblTargetActPos_mm[N_CHANNELS] = { 0.0 }; // The final target position for the actuator
+// These have to be declared in the global scope or we get a stack overflow. No idea why.
double _dblVelocity_mmps[N_CHANNELS];
-double _dblLinearPathCurrentPos_mm[N_CHANNELS];
+//double _dblLinearPathCurrentPos_mm[N_CHANNELS];
+double dblADCCurrentPosition[N_CHANNELS];
Serial pc(USBTX, USBRX); // tx, rx for usb debugging
-LLComms llcomms(LOW_LEVEL_SPI_FREQUENCY);//(N_CHANNELS);
+LLComms llcomms;
Thread threadLowLevelSPI(osPriorityRealtime);
Thread threadReceiveAndReplan(osPriorityBelowNormal);
@@ -55,22 +58,21 @@
// RECEIVE MESSAGE
error_code = hlcomms.receive_message();
if( error_code == NSAPI_ERROR_NO_CONNECTION ) { // -3004
- printf("Client disconnected.\n\r");
+ if(IS_PRINT_OUTPUT) printf("Client disconnected.\n\r");
hlcomms.close_server();
return;
} else if( error_code < 0 ) {
- printf("Error %i. Could not send data over the TCP socket. "
+ 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;
}
- //printf("Message received.\r\n");
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)
- //printf("REPLAN, %f\r\n",input.duration);
+ 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;
@@ -94,17 +96,22 @@
dblTargetChambLen_mm[2] = input.psi[2][2]*1000;
mutPathIn.lock(); // Lock variables to avoid race condition
- for(int j = 0; j<N_CHANNELS; j++) {
+ /*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
}
+
mutPathIn.unlock(); // Unlock mutex
bool isTimeChanged = 0;
double dblMaxRecalculatedTime = input.duration;
for (ii = 0; ii< N_CHANNELS; ii++) {
//check to see if positions are achievable
- dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*DBL_ACTUATOR_CONVERSION[ii];
+ dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*FLT_ACTUATOR_CONVERSION[ii];
//!! LIMIT CHAMBER LENGTHS TOO
if( dblTargetActPos_mm[ii]<0.0 || dblTargetActPos_mm[ii]>25.0 ) {
dblTargetActPos_mm[ii] = min( max( 0.0 , dblTargetActPos_mm[ii] ) , 25.0 );
@@ -127,8 +134,8 @@
/*dblActPosChange = _dblLinearPathCurrentPos_mm[ii];
_dblVelocity_mmps[ii] = 0.0;*/
- dblActPosChange = dblTargetActPos_mm[ii] - _dblLinearPathCurrentPos_mm[ii];
- if( fabs(dblActPosChange) < DBL_PATH_TOLERANCE ) {
+ dblActPosChange = dblTargetActPos_mm[ii] - dblADCCurrentPosition[ii];
+ if( fabs(dblActPosChange) < FLT_PATH_TOLERANCE ) {
dblActPosChange = 0.0;
isTimeChanged = 1;
}
@@ -161,30 +168,24 @@
}
}
if( isTimeChanged ) {
- if( dblMaxRecalculatedTime < input.duration ) {
- dblMaxRecalculatedTime = input.duration;
- }
if( dblMaxRecalculatedTime >= 0.000000001 ) {
for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
- _dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - _dblLinearPathCurrentPos_mm[ii]) / dblMaxRecalculatedTime;
+ _dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblADCCurrentPosition[ii]) / dblMaxRecalculatedTime;
dblVelocity_mmps[ii] = _dblVelocity_mmps[ii];
}
}
}
-
- //printf("Sending message...\r\n");
// SEND MESSAGE
//dblMaxRecalculatedTime = 1.0;
hlcomms.make_message(&dblMaxRecalculatedTime);
error_code = hlcomms.send_message();
if( error_code < 0 ) {
- printf("Error %i. Could not send data over the TCP socket. "
+ 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;
}
- //printf("Message sent.\r\n");
}
}
@@ -213,7 +214,7 @@
// Calculate next step in linear path
mutPathIn.lock(); // Lock relevant mutex
// Check tolerance
- if (fabs(dblLinearPathCurrentPos_mm[jj] - dblTargetActPos_mm[jj]) <= DBL_PATH_TOLERANCE) {
+ if (fabs(dblLinearPathCurrentPos_mm[jj] - dblTargetActPos_mm[jj]) <= FLT_PATH_TOLERANCE) {
dblVelocity_mmps[jj] = 0.0; // Stop linear path generation when linear path is within tolerance of target position
}
dblLinearPathCurrentPos_mm[jj] = dblLinearPathCurrentPos_mm[jj] + dblVelocity_mmps[jj]*dblMeasuredSampleTime;
@@ -221,7 +222,7 @@
mutPathIn.unlock(); // Unlock relevant mutex
// Calculate next step in smooth path
- dblSmoothPathCurrentPos_mm[jj] = DBL_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[jj] + (1.0-DBL_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[jj];
+ dblSmoothPathCurrentPos_mm[jj] = FLT_SMOOTHING_FACTOR*dblLinearPathCurrentPos_mm[jj] + (1.0f-FLT_SMOOTHING_FACTOR)*dblSmoothPathCurrentPos_mm[jj];
dblSmoothPathCurrentPos_mm[jj] = max( 0.0 , dblSmoothPathCurrentPos_mm[jj] );
llcomms.mutChannel[jj].lock(); // MUTEX LOCK
llcomms.demandPosition[jj] = (int) ((dblSmoothPathCurrentPos_mm[jj]/MAX_ACTUATOR_LENGTH)*8191);// Convert to a 13-bit number
@@ -231,10 +232,10 @@
llcomms.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, %f, %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[0],dblLinearPathCurrentPos_mm[1],dblLinearPathCurrentPos_mm[2],
+ //if(IS_PRINT_OUTPUT) printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]);
+ //if(IS_PRINT_OUTPUT) printf("%f, %f, %f, %f, %f, %f, %f, %f\r\n",dblLinearPathCurrentPos_mm[0],dblLinearPathCurrentPos_mm[1],dblLinearPathCurrentPos_mm[2],
// dblLinearPathCurrentPos_mm[3],dblLinearPathCurrentPos_mm[4],dblLinearPathCurrentPos_mm[5],dblLinearPathCurrentPos_mm[6],dblLinearPathCurrentPos_mm[7]);
- //printf("%f\r\n",dblLinearPathCurrentPos_mm[0]);
+ //if(IS_PRINT_OUTPUT) printf("%f\r\n",dblLinearPathCurrentPos_mm[0]);
} // end while
}