Basic Mid-Level control for the rebuilt MorphGI control unit, using PWM to communicate with the low level controllers.
Dependencies: ros_lib_kinetic
Diff: main.cpp
- Revision:
- 11:7029367a1840
- Parent:
- 10:1b6daba32452
- Child:
- 12:595ed862e52f
--- a/main.cpp Tue Aug 07 15:31:59 2018 +0000
+++ b/main.cpp Fri Aug 10 10:40:18 2018 +0000
@@ -4,41 +4,20 @@
#include "mbed.h"
#include "mbed_events.h"
// CUSTOM IMPORTS
+#include "MLOptions.h"
#include "HLComms.h"
#include "LLComms.h"
-// SIMPLE CHANNEL SELECTION
-#define ADC_PRESSURE 1
-#define ADC_POSITION 3
-
-#define N_CHANNELS 8 // Number of channels to control
-// 1-3: front segment; 4-6: rear segment; 7-8: mid segment
-
-// SPI SETTINGS
-#define LOW_LEVEL_SPI_FREQUENCY 10000000
-// PATH GENERATION SETTINGS
-#define PATH_SAMPLE_TIME_S 0.005 //0.109
-#define MAX_LENGTH_MM 100.0
-#define MAX_ACTUATOR_LENGTH 52.2
-#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.
-
-#define BAUD_RATE 9600 //115200
-
-// COMMS SETTINGS
-const short int SERVER_PORT = 80;
-
-//const double DBL_MAX_CHAMBER_LENGTHS_MM[N_CHANNELS] = {80.0,80.0,80.0,80.0,80.0,80.0,80.0,80.0};
-//const double DBL_ACTUATOR_CONVERSION[N_CHANNELS] = {0.30586,0.30586,0.30586,0.30586,0.30586,0.4588,0.4588}; // Convert from chamber lengths to actuator lengths
-const double DBL_ACTUATOR_CONVERSION[N_CHANNELS] = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0}; // Convert from chamber lengths to actuator
-const double DBL_SMOOTHING_FACTOR = 0.5; // 0<x<=1, where 1 is no smoothing
-const double DBL_PATH_TOLERANCE = MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; // Path tolerance in mm with 5% tolerance
+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
// 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
+double _dblVelocity_mmps[N_CHANNELS];
+double _dblLinearPathCurrentPos_mm[N_CHANNELS];
+
Serial pc(USBTX, USBRX); // tx, rx for usb debugging
LLComms llcomms(LOW_LEVEL_SPI_FREQUENCY);//(N_CHANNELS);
@@ -91,9 +70,9 @@
// 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);
+ //printf("REPLAN, %f\r\n",input.duration);
// Update front segment
- dblTargetChambLen_mm[0] = input.psi[0][0]*1000;
+ /*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
@@ -102,21 +81,29 @@
// 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;
+ dblTargetChambLen_mm[5] = input.psi[2][2]*1000;*/
+ dblTargetChambLen_mm[3] = input.psi[0][0]*1000;
+ dblTargetChambLen_mm[4] = input.psi[0][1]*1000;
+ dblTargetChambLen_mm[5] = 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[0] = input.psi[2][0]*1000;
+ dblTargetChambLen_mm[1] = input.psi[2][1]*1000;
+ 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];
+ }
+ mutPathIn.unlock(); // Unlock mutex
bool isTimeChanged = 0;
double dblMaxRecalculatedTime = input.duration;
- mutPathIn.lock(); // Lock variables to avoid race condition
for (ii = 0; ii< N_CHANNELS; ii++) {
//check to see if positions are achievable
- /*if(dblTargetChambLen_mm[ii]>DBL_MAX_CHAMBER_LENGTHS_MM[ii]) {
- dblTargetChambLen_mm[ii] = DBL_MAX_CHAMBER_LENGTHS_MM[ii];
- isTimeChanged = 1;
- }
- if(dblTargetChambLen_mm[ii]<0.0) {
- dblTargetChambLen_mm[ii] = 0.0;
- isTimeChanged = 1;
- }*/
dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*DBL_ACTUATOR_CONVERSION[ii];
//!! LIMIT CHAMBER LENGTHS TOO
if( dblTargetActPos_mm[ii]<0.0 || dblTargetActPos_mm[ii]>25.0 ) {
@@ -127,32 +114,47 @@
double dblActPosChange;
short sgn;
for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
- dblActPosChange = dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii];
+ /*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 = dblTargetActPos_mm[ii] - _dblLinearPathCurrentPos_mm[ii];
if( fabs(dblActPosChange) < DBL_PATH_TOLERANCE ) {
dblActPosChange = 0.0;
isTimeChanged = 1;
}
+ //IS BELOW
if( input.duration < 0.000000001 ) {
sgn = (dblActPosChange > 0) ? 1 : ((dblActPosChange < 0) ? -1 : 0);
- dblVelocity_mmps[ii] = sgn * MAX_SPEED_MMPS;
+ _dblVelocity_mmps[ii] = sgn * MAX_SPEED_MMPS;
isTimeChanged = 1;
} else {
- dblVelocity_mmps[ii] = dblActPosChange / input.duration;
+ _dblVelocity_mmps[ii] = dblActPosChange / input.duration;
}
+ //IS ABOVE
// 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;
+ 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;
+ _dblVelocity_mmps[ii] = -1*MAX_SPEED_MMPS;
}
isTimeChanged = 1;
}
double dblRecalculatedTime;
- if( fabs(dblVelocity_mmps[ii]) < 0.000000001 ) {
+ if( fabs(_dblVelocity_mmps[ii]) < 0.000000001 ) {
dblRecalculatedTime = 0;
} else {
- dblRecalculatedTime = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblVelocity_mmps[ii];
+ dblRecalculatedTime = dblActPosChange / _dblVelocity_mmps[ii];
}
if( dblRecalculatedTime > dblMaxRecalculatedTime ) {
dblMaxRecalculatedTime = dblRecalculatedTime;
@@ -162,14 +164,17 @@
if( dblMaxRecalculatedTime < input.duration ) {
dblMaxRecalculatedTime = input.duration;
}
- for (ii = 0; ii< N_CHANNELS; ii++) { // Work out new velocities
- dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblMaxRecalculatedTime;
+ 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] = _dblVelocity_mmps[ii];
+ }
}
}
- mutPathIn.unlock(); // Unlock mutex
- printf("Sending message...\r\n");
+ //printf("Sending message...\r\n");
// SEND MESSAGE
+ //dblMaxRecalculatedTime = 1.0;
hlcomms.make_message(&dblMaxRecalculatedTime);
error_code = hlcomms.send_message();
if( error_code < 0 ) {
@@ -179,7 +184,7 @@
hlcomms.close_server();
return;
}
- printf("Message sent.\r\n");
+ //printf("Message sent.\r\n");
}
}
@@ -207,19 +212,18 @@
// Calculate next step in linear path
mutPathIn.lock(); // Lock relevant mutex
- dblLinearPathCurrentPos_mm[jj] = dblLinearPathCurrentPos_mm[jj] + dblVelocity_mmps[jj]*dblMeasuredSampleTime;
- 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
}
+ dblLinearPathCurrentPos_mm[jj] = dblLinearPathCurrentPos_mm[jj] + dblVelocity_mmps[jj]*dblMeasuredSampleTime;
+ dblLinearPathCurrentPos_mm[jj] = max( 0.0 , dblLinearPathCurrentPos_mm[jj] );
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];
- llcomms.mutChannel[jj].lock(); // MUTEX LOCK
+ 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
llcomms.demandPosition[jj] = llcomms.demandPosition[jj] & 0x1FFF; // Ensure number is 13-bit
llcomms.mutChannel[jj].unlock(); // MUTEX UNLOCK
@@ -228,8 +232,8 @@
} // 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],
- dblLinearPathCurrentPos_mm[3],dblLinearPathCurrentPos_mm[4],dblLinearPathCurrentPos_mm[5],dblLinearPathCurrentPos_mm[6],dblLinearPathCurrentPos_mm[7]);*/
+ //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]);
} // end while