MorphGI
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:
- 6:f0a18e28a322
- Parent:
- 5:712e7634c779
- Child:
- 7:5b6a2cefbf3b
--- a/main.cpp Wed Aug 01 09:51:23 2018 +0000
+++ b/main.cpp Fri Aug 03 11:08:12 2018 +0000
@@ -1,8 +1,40 @@
#include "mbed.h"
#include "math.h"
+#include "mbed_events.h"
+// COMMS IMPORTS
+#include <sstream> // stringstream
+#include <vector> // vector
+#include <string.h> // strtok
//#include "mbed.h"
-#include "mbed_events.h"
+#include "EthernetInterface.h"
+#include "TCPServer.h"
+#include "TCPSocket.h"
+
+// COMMS SETTINGS
+const bool IS_DHCP = false;
+#define PORT 80
+
+// COMMS STRUCT DECLARATIONS
+struct msg_format {
+ double psi[3][3];
+ double duration;
+};
+struct comms_interfaces {
+ EthernetInterface eth; // Indicates which NetworkInterface the socket should be created on
+ TCPServer srv; // Provides the ability to accept incoming TCP connections
+ TCPSocket clt_sock; // Provides the ability to send a stream of data over TCP
+ SocketAddress clt_addr; // Represents the IP address and port pair of a unique network endpoint
+};
+
+// COMMS FUNCTION DECLARATIONS
+int setup_server(void);
+int accept_connection(void);
+void close_server(void);
+msg_format consume_message( unsigned char * msg );
+
+// COMMS GLOBAL VARIABLES
+struct comms_interfaces comms;
//ADC SPI stuff
#define PREAMBLE 0x06
@@ -26,17 +58,18 @@
//parameters to manually change
#define LOW_LEVEL_SPI_FREQUENCY 10000000
-#define PATH_SAMPLE_TIME_S 0.005
+#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.
-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_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};// covert 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 = 0.2;
+const double DBL_PATH_TOLERANCE = MAX_SPEED_MMPS * PATH_SAMPLE_TIME_S * 1.05; //path tolerance in mm with 5% tolerance
//-----These are the variables being shared between High-Level and Mid-Level-----------------------------------------------//
//
@@ -246,13 +279,13 @@
{
int ii;
- double dblPSI[3][3]; //the message from high-level containing the chamber lengths in meters in following format:
+ //double dblPSI[3][3]; //the message from high-level containing the chamber lengths in meters in following format:
/*
{L(front,1), L(front,2), L(front,3);
L(mid,1) , L(mid,2) , L(mid,3);
L(rear,1) , L(rear,2) , L(rear,3);}
*/
- double dblTargetTime_s; //the time in seconds desired to reach the target (>0...!)
+ //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).
@@ -261,66 +294,143 @@
//receive
- double dblTargetChambLen_mm[N_CHANNELS]; //the currenly assigned final target position (actuator will reach this at end of path)
+ // Error codes: https://github.com/ARMmbed/mbed-os/blob/master/features/netsocket/nsapi_types.h
+ // http://man7.org/linux/man-pages/man3/errno.3.html
+
+ int error_code;
+
+ error_code = setup_server();
+ if( error_code == -1 ) return;
+ error_code = accept_connection();
+ if( error_code == -1 ) return;
+
+ unsigned char recv_buffer[1024];
+ struct msg_format input;
+ char send_buffer[65];
+ char * message_to_send;
- bool kk = 0;
- while(1) {
+ while( true ) {
+ // RECEIVE MESSAGE
+ memset(recv_buffer, 0, sizeof(recv_buffer));
+ error_code = comms.clt_sock.recv(recv_buffer, 1024); // Blocks until data is received
+ if( error_code == NSAPI_ERROR_NO_CONNECTION ) { // -3004
+ printf("Client disconnected.\n\r");
+ close_server();
+ return;
+ } else if( error_code < 0 ) {
+ 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);
+ close_server();
+ return;
+ }
+ //printf("Message received.\r\n");
+ input = consume_message( recv_buffer );
+ //!! Set global variables to input with mutex
- if(kk == 0) {
- dblPSI[0][0] = (double) 0.20;
- dblTargetTime_s = 3.0;
- } else {
- dblPSI[0][0] = (double) 0.0;
- dblTargetTime_s = 3.0;
- }
- kk = !kk;
- printf("REPLAN, %f,\r\n", dblPSI[0][0] );
+ // 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);
//update front segment
- dblTargetChambLen_mm[0] = dblPSI[0][0]*1000;
- dblTargetChambLen_mm[1] = dblPSI[0][1]*1000;
- dblTargetChambLen_mm[2] = dblPSI[0][2]*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
- dblTargetChambLen_mm[6] = dblPSI[1][0]*1000;
+ 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[3] = dblPSI[2][0]*1000;
- dblTargetChambLen_mm[4] = dblPSI[2][1]*1000;
- dblTargetChambLen_mm[5] = dblPSI[2][2]*1000;
+ dblTargetChambLen_mm[3] = input.psi[2][0]*1000;
+ dblTargetChambLen_mm[4] = input.psi[2][1]*1000;
+ dblTargetChambLen_mm[5] = input.psi[2][2]*1000;
+ 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]) {
+ /*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;
+ 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 ) {
+ dblTargetActPos_mm[ii] = min( max( 0.0 , dblTargetActPos_mm[ii] ) , 25.0 );
+ isTimeChanged = 1;
+ }
+ }
+ double dblActPosChange;
+ short sgn;
+ for (ii = 0; ii< N_CHANNELS; ii++) {
+ //work out new velocities
+ dblActPosChange = dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii];
+ if( fabs(dblActPosChange) < DBL_PATH_TOLERANCE ) {
+ dblActPosChange = 0.0;
+ isTimeChanged = 1;
}
-
- mutPathIn.lock();//lock variables to avoid race condition
- dblTargetActPos_mm[ii] = dblTargetChambLen_mm[ii]*DBL_ACTUATOR_CONVERSION[ii];
-
+ if( input.duration < 0.000000001 ) {
+ sgn = (dblActPosChange > 0) ? 1 : ((dblActPosChange < 0) ? -1 : 0);
+ dblVelocity_mmps[ii] = sgn * MAX_SPEED_MMPS;
+ isTimeChanged = 1;
+ } else {
+ dblVelocity_mmps[ii] = dblActPosChange / input.duration;
+ }
+ //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;
+ }
+ isTimeChanged = 1;
+ }
+ double dblRecalculatedTime;
+ if( fabs(dblVelocity_mmps[ii]) < 0.000000001 ) {
+ dblRecalculatedTime = 0;
+ } else {
+ dblRecalculatedTime = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblVelocity_mmps[ii];
+ }
+ if( dblRecalculatedTime > dblMaxRecalculatedTime ) {
+ dblMaxRecalculatedTime = dblRecalculatedTime;
+ }
+ }
+ if( isTimeChanged ) {
+ 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]) / 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.
+ dblVelocity_mmps[ii] = (dblTargetActPos_mm[ii] - dblLinearPathCurrentPos_mm[ii]) / dblMaxRecalculatedTime;
+ }
+ }
- //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
+ //dblPressure_bar[ii] = 0.5;//read pressure from channel
+ //dblPosition_mtrs[ii] = 0.02; //read position from channel
//printf("%f\r\n", dblVelocity_mmps[0]);
- Thread::wait(7000);
- } // end of while(1)
+ mutPathIn.unlock(); //unlock mutex.
+
+ printf("Sending message...\r\n");
+ // SEND MESSAGE
+ memset(send_buffer, 0, sizeof(send_buffer));
+ _snprintf(send_buffer,64,"%f",dblMaxRecalculatedTime);
+ message_to_send = send_buffer;
+ error_code = comms.clt_sock.send(message_to_send, strlen(message_to_send));
+ if( error_code < 0 ) {
+ 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);
+ close_server();
+ return;
+ }
+ printf("Message sent.\r\n");
+ }
+
}
// lock mutex
@@ -337,7 +447,6 @@
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();
@@ -362,6 +471,10 @@
if(dblLinearPathCurrentPos_mm[jj] < 0.0) {
dblLinearPathCurrentPos_mm[jj] = 0.00;
}
+ /*if(dblLinearPathCurrentPos_mm[jj] > 0.0) {
+ dblLinearPathCurrentPos_mm[jj] = 0.00;
+ }*/
+
//check tolerance
if (fabs(dblLinearPathCurrentPos_mm[jj] - dblTargetActPos_mm[jj]) <= DBL_PATH_TOLERANCE) {
@@ -381,10 +494,10 @@
isDataReady[jj] = 1;//signal that data ready
} // end for
- //printf("%f, %d\r\n",dblSmoothPathCurrentPos_mm[0], intDemandPos_Tx[0]);
- mutChannel[0].lock();
- printf("%f, %f, %f\r\n",dblSmoothPathCurrentPos_mm[0],dblPressure_bar[0], dblPosition_mtrs[0]);
- mutChannel[0].unlock();
+ //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\r\n",dblLinearPathCurrentPos_mm[0]);
} // end while
}
@@ -409,7 +522,7 @@
}
//calculate some control variables
- //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.25);
@@ -418,7 +531,7 @@
pinReset = 1;//ready to go
//say something nice to the user.
- pc.baud(115200);
+ pc.baud(9600); //115200
printf("Hi, there! I'll be your mid-level controller for today.\r\n");
wait(3);
// Start the event queue
@@ -477,3 +590,111 @@
}
}
+
+int setup_server(void) {
+ int error_code;
+ printf("Starting TCP server...\n\r");
+ if( !IS_DHCP ) {
+ const char* ip = "10.101.79.135"; //"10.101.81.125"; // Get first 3 parts from "ifconfig" on PC
+ const char* mask = "255.255.255.0"; // Get from "ifconfig" on PC
+ const char* gateway = "10.101.79.1"; //"10.101.81.1"; // Get from "route -n" on PC
+ error_code = comms.eth.set_network(ip, mask, gateway); // Dont use DHCP
+ if( error_code < 0 ) {
+ printf("Error %i. Could not network interface to use a static IP address. "
+ "Perhaps the network is already connected?\n\r", error_code);
+ return -1;
+ }
+ }
+ error_code = comms.eth.connect();
+ if( error_code < 0 ) {
+ printf("Error %i. Could not start the Ethernet interface.\n\r", error_code);
+ return -1;
+ }
+ const char* server_ip = comms.eth.get_ip_address();
+ if( server_ip == NULL ) {
+ error_code = -1;
+ printf("Error %i. Ethernet interface is not yet connected.\n\r", error_code);
+ return -1;
+ }
+ printf("The target IP address is '%s'\n\r", server_ip);
+ error_code = comms.srv.open(&comms.eth); // Open the server on ethernet stack
+ if( error_code < 0 ) {
+ printf("Error %i. Could not open server socket.\n\r", error_code);
+ return -1;
+ }
+ error_code = comms.srv.bind(comms.eth.get_ip_address(), PORT); // Bind the HTTP port (TCP 80) to the server
+ if( error_code < 0 ) {
+ printf("Error %i. Could not bind server socket to port '%d'.\n\r", error_code, PORT);
+ return -1;
+ }
+ error_code = comms.srv.listen(1); // Listen for 1 simultaneous connection
+ if( error_code < 0 ) {
+ printf("Error %i. Could not put server socket into listening mode.\n\r", error_code);
+ return -1;
+ }
+ return 0;
+}
+
+
+int accept_connection(void) {
+ printf("Waiting to accept a connection on the TCP socket.\n\r");
+ int error_code;
+ error_code = comms.srv.accept(&comms.clt_sock, &comms.clt_addr); // Blocks until data is sent
+ if( error_code < 0 ) {
+ printf("Error %i. Could not create a network socket using the specified socket instance. "
+ "Perhaps the socket is set to non-blocking or timed out?\n\r", error_code);
+ return -1;
+ }
+ printf("Accepted %s:%d\n\r", comms.clt_addr.get_ip_address(), comms.clt_addr.get_port());
+ return 0;
+}
+
+
+void close_server(void) {
+ comms.clt_sock.close();
+ comms.srv.close();
+ comms.eth.disconnect();
+ return;
+}
+
+
+msg_format consume_message( unsigned char * msg ) {
+ // Break message string into chunks at each comma
+ vector<string> tokens;
+ char * delim = ",";
+ char * token = strtok((char *)msg, delim);
+ while(token != NULL) {
+ tokens.push_back(string(token));
+ //printf("%s\n\r",token);
+ token = strtok(NULL, delim);
+ }
+ // Cast into doubles and assign to variables
+ struct msg_format input;
+ stringstream(tokens[0]) >> input.psi[0][0];
+ stringstream(tokens[1]) >> input.psi[0][1];
+ stringstream(tokens[2]) >> input.psi[0][2];
+ stringstream(tokens[3]) >> input.psi[1][0];
+ stringstream(tokens[4]) >> input.psi[1][1];
+ stringstream(tokens[5]) >> input.psi[1][2];
+ stringstream(tokens[6]) >> input.psi[2][0];
+ stringstream(tokens[7]) >> input.psi[2][1];
+ stringstream(tokens[8]) >> input.psi[2][2];
+ stringstream(tokens[9]) >> input.duration;
+
+ if(input.psi[0][0] < 0.0) input.psi[0][0] = 0.0;
+ if(input.psi[0][1] < 0.0) input.psi[0][1] = 0.0;
+ if(input.psi[0][2] < 0.0) input.psi[0][2] = 0.0;
+ if(input.psi[1][0] < 0.0) input.psi[1][0] = 0.0;
+ if(input.psi[1][1] < 0.0) input.psi[1][1] = 0.0;
+ if(input.psi[1][2] < 0.0) input.psi[1][2] = 0.0;
+ if(input.psi[2][0] < 0.0) input.psi[2][0] = 0.0;
+ if(input.psi[2][1] < 0.0) input.psi[2][1] = 0.0;
+ if(input.psi[2][2] < 0.0) input.psi[2][2] = 0.0;
+ if(input.duration < 0.0) input.duration = 0.0;
+ /*if( input.psi[2][0] > 0.14 ) {
+ printf("Hi\n\r");
+ }*/
+ //throw std::invalid_argument( "received negative value" );
+ //return 0;
+ return input;
+} // End of consume_message()
\ No newline at end of file