File content as of revision 6:f0a18e28a322:

#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 "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
#define CHAN_1 0x30
#define CHAN_2 0x70
#define CHAN_3 0xB0
#define CHAN_4 0xF0

#define DATA_MASK 0x0F

//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
                
//parameters to manually change                       
#define LOW_LEVEL_SPI_FREQUENCY 10000000

#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] = {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 = 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-----------------------------------------------//

//
Semaphore semReplan(1);// this must be set every time new high-level transmissions are received to allow replanning to take place!
//--------------------------------------------------------------------------------------------------------------------------//

//boolean flag to indicate that new target information has been received
//bool blnReplan;//set this when new transmission is received over ethernet

//path variables
double dblVelocity_mmps[N_CHANNELS] = { 0.0 };//the linear path velocity (not sent to actuator) KEEP GLOBAL
double dblLinearPathCurrentPos_mm[N_CHANNELS]={ 0.0 }; //the current position of the linear path (not sent to actuator) //KEEP GLOBAL
double dblTargetActPos_mm[N_CHANNELS] = { 0.0 }; //The final target position for the actuator KEEP GLOBAL

int intDemandPos_Tx[N_CHANNELS]; //13-bit value to be sent to the actuator KEEP GLOBAL

char chrErrorFlag[N_CHANNELS];// 3 error bits from LL KEEP GLOBAL

//pin declarations----------------
Serial pc(USBTX, USBRX); // tx, rx for usb debugging

InterruptIn pinGate6(PE_11); //this pin HAS TO BE defined before SPI set up. No Clue Why.

SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk

DigitalOut cs_LL[N_CHANNELS] = {PD_15, PE_10, PD_14, PD_11, PE_7, PD_12, PF_10, PD_13};//chip select for low level controller
DigitalOut cs_ADC[N_CHANNELS] = {PG_12, PG_9, PE_1, PG_0, PD_0, PD_1, PF_0, PF_1}; //chip select for ADC

DigitalOut pinCheck(PE_5);

//these interrupt pins have to be declared AFTER SPI declaration. No Clue Why.
InterruptIn pinGate0(PF_11);
InterruptIn pinGate1(PG_14);
InterruptIn pinGate2(PF_15);
InterruptIn pinGate3(PF_12);
InterruptIn pinGate4(PF_3);
InterruptIn pinGate5(PF_13);
//InterruptIn pinGate6(PE_11); //see above nonsense
InterruptIn pinGate7(PE_13);

DigitalOut pinReset(PD_2); //reset pin for all controllers.


EventQueue queue(32 * EVENTS_EVENT_SIZE);

Thread t(osPriorityRealtime);

Thread threadReceiveAndReplan(osPriorityBelowNormal);
Thread threadSmoothPathPlan(osPriorityNormal);

Thread threadSimulateDemand(osPriorityHigh);

Mutex mutChannel[N_CHANNELS];
Mutex mutPathIn;

Semaphore semPathPlan(1);//

Timer timer;//timers are nice

int ThreadID[N_CHANNELS];

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;
    unsigned int outputB;
    int output;
    double dblOutput;

    spi.format(8,0);
    spi.frequency(1000000);
    
    cs_ADC[channel] = 0;
    spi.write(PREAMBLE);
    outputA = spi.write(CHAN_3);
    outputB = spi.write(0xFF);
    cs_ADC[channel]= 1;
    
    outputA = outputA & DATA_MASK;
    outputA = outputA<<8;
    output = (outputA | outputB);
    output = 4095- output;
    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)
{
    unsigned int outputA;
    unsigned int outputB;
    int output;
    double dblOutput;

    spi.format(8,0);
    spi.frequency(1000000);
    
    cs_ADC[channel] = 0;
    spi.write(PREAMBLE);
    outputA = spi.write(CHAN_1);
    outputB = spi.write(0xFF);
    cs_ADC[channel] = 1;
    
    outputA = outputA & DATA_MASK;
    outputA = outputA<<8;
    output = (outputA | outputB);
    
    dblOutput = (double)(output);
    dblOutput = dblOutput-502.0;
    dblOutput = dblOutput/4095.0*8.0;
    return dblOutput;
}

void SendReceiveData(int channel) 
{
    
    
    int intPosSPI_Rx[N_CHANNELS]; //13 bit value received over SPI from the actuator
    
    
    //get data from controller
    spi.format(16,2);
    spi.frequency(LOW_LEVEL_SPI_FREQUENCY);
    mutChannel[channel].lock();//lock mutex for specific Channel
    cs_LL[channel] = 0;//select relevant chip
    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;
    
    intPosSPI_Rx[channel] = intPosSPI_Rx[channel] & 0x1FFF;
    //dblPosition_mtrs[channel] = (double)intPosSPI_Rx[channel]/8191*(MAX_ACTUATOR_LENGTH/DBL_ACTUATOR_CONVERSION[channel])/1000;
    mutChannel[channel].unlock();//unlock mutex for specific channel
    //printf("%d, %d\r\n",intPosSPI_Rx[0],intDemandPos_Tx[0]);
    
}



//common rise handler function 

void common_rise_handler(int channel)
{
    pinCheck = 1;
    //check if data is ready for tranmission
    if (isDataReady[channel])
    {
        // Add transmit task to event queue
        ThreadID[channel] = queue.call(SendReceiveData,channel);//schedule transmission
    }
}




//common_fall handler functions
void common_fall_handler(int channel) 
{
    pinCheck = 0;
    //cancel relevant queued event    
    queue.cancel(ThreadID[channel]);
}

//stub rise functions
void rise0(void) { common_rise_handler(0); }
void rise1(void) { common_rise_handler(1); }
void rise2(void) { common_rise_handler(2); }
void rise3(void) { common_rise_handler(3); }
void rise4(void) { common_rise_handler(4); }
void rise5(void) { common_rise_handler(5); }
void rise6(void) { common_rise_handler(6); }
void rise7(void) { common_rise_handler(7); }

//stub fall functions
void fall0(void) { common_fall_handler(0); }
void fall1(void) { common_fall_handler(1); }
void fall2(void) { common_fall_handler(2); }
void fall3(void) { common_fall_handler(3); }
void fall4(void) { common_fall_handler(4); }
void fall5(void) { common_fall_handler(5); }
void fall6(void) { common_fall_handler(6); }
void fall7(void) { common_fall_handler(7); }

void startPathPlan() // Plan a new linear path after receiving new target data
{
    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()
{
    int ii;

    //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 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
    
    //receive
    
    // 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;
    
    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
        
        // 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] = 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] = input.psi[1][0]*1000;
        dblTargetChambLen_mm[7] = dblTargetChambLen_mm[6]; //same because two pumps are used
        //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;

        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 ) {
                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;
            }
            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]) / dblMaxRecalculatedTime;
            }
        }
            
            
        //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]);
        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
//  get variables
// unlock mutex
// do calculations
// lock mutex
//  set variables
// unlock mutex

void CalculateSmoothPath() {
    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(); 
        } else {
            dblMeasuredSampleTime = PATH_SAMPLE_TIME_S;
        }
        timer.reset();
            
        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);
            
            //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;
                }
                /*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[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[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],
            dblLinearPathCurrentPos_mm[3],dblLinearPathCurrentPos_mm[4],dblLinearPathCurrentPos_mm[5],dblLinearPathCurrentPos_mm[6],dblLinearPathCurrentPos_mm[7]);*/
        //printf("%f\r\n",dblLinearPathCurrentPos_mm[0]);
    } // end while
}



Ticker PathCalculationTicker;

int main() 
{
    int ii;
    
    //initialise relevant variables
    for(ii = 0; ii<N_CHANNELS; ii++)
    {
        
        //all chip selects in off state
        cs_LL[ii] = 1;
        cs_ADC[ii] = 1;
        
        //data ready flags set to not ready
        isDataReady[ii] = 0;
    }
    
    //calculate some control variables
    
    
    pinReset = 1; //initialise reset pin to not reset the controllers.
    wait(0.25);
    pinReset=0; //reset controllers to be safe
    wait(0.25);
    pinReset = 1;//ready to go
    
    //say something nice to the user.
    pc.baud(9600); //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));
    
    //set up the interrupts

    //set up rise interrupts MIGHT NOT NEED TO BE POINTERS
    //
    //pinGate[0].rise(&testRiseFunction);
    //testPinGate.rise(&testRiseFunction);//<working
    //testPinGate.rise(&rise0);
    
    pinGate0.rise(&rise0);
    
    pinGate1.rise(&rise1);
    pinGate2.rise(&rise2);
    pinGate3.rise(&rise3);
    pinGate4.rise(&rise4);
    pinGate5.rise(&rise5);
    pinGate6.rise(&rise6);
    pinGate7.rise(&rise7);
    
    
    //set up fall interrupts MIGHT NOT NEED TO BE POINTERS
    //
    //pinGate[0].fall(&testFallFunction);
    //testPinGate.fall(&testFallFunction); <working
    //testPinGate.fall(&fall0);
    
    pinGate0.fall(&fall0);
    
    pinGate1.fall(&fall1);
    pinGate2.fall(&fall2);
    pinGate3.fall(&fall3);
    pinGate4.fall(&fall4);
    pinGate5.fall(&fall5);
    pinGate6.fall(&fall6);
    pinGate7.fall(&fall7);
    
    
    timer.start();
    
    //threadSimulateDemand.start(SimulateDemand);
    threadReceiveAndReplan.start(ReceiveAndReplan);//start Replanning thread
    //ReceiveAndReplan();
    threadSmoothPathPlan.start(CalculateSmoothPath); //start planning thread
    
    PathCalculationTicker.attach(&startPathPlan, PATH_SAMPLE_TIME_S); //set up planning thread to recur at fixed intervals
    
     Thread::wait(1);  
    
      
    while(1) {
        Thread::wait(osWaitForever); 
    }
}


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()