MorphGI / Mbed OS LoCoMoTE_MidLevel_7-0

Mid level control code

Dependencies:   ros_lib_kinetic

HLComms.cpp@17:bbaf3e8440ad, 2018-10-04 (annotated)

Committer:
WD40andTape
Date:
Thu Oct 04 15:27:15 2018 +0000
Revision:
17:bbaf3e8440ad
Parent:
14:54c3759e76ed
Child:
19:e5acb2183d4e
Sensor feedback through Ethernet.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WD40andTape 7:5b6a2cefbf3b 1 // HLComms.cpp
WD40andTape 7:5b6a2cefbf3b 2
WD40andTape 7:5b6a2cefbf3b 3 // Error codes: https://github.com/ARMmbed/mbed-os/blob/master/features/netsocket/nsapi_types.h
WD40andTape 7:5b6a2cefbf3b 4 // http://man7.org/linux/man-pages/man3/errno.3.html
WD40andTape 7:5b6a2cefbf3b 5
WD40andTape 7:5b6a2cefbf3b 6 #include "HLComms.h"
WD40andTape 7:5b6a2cefbf3b 7
WD40andTape 7:5b6a2cefbf3b 8 HLComms::HLComms(short int port) { // Constructor
WD40andTape 7:5b6a2cefbf3b 9 _port = port;
WD40andTape 7:5b6a2cefbf3b 10 }
WD40andTape 7:5b6a2cefbf3b 11
WD40andTape 7:5b6a2cefbf3b 12 int HLComms::setup_server(void) {
WD40andTape 7:5b6a2cefbf3b 13 int error_code;
dofydoink 12:595ed862e52f 14 if(IS_PRINT_OUTPUT) printf("Starting TCP server...\n\r");
WD40andTape 7:5b6a2cefbf3b 15 if( !IS_DHCP ) {
WD40andTape 17:bbaf3e8440ad 16 const char* ip = "192.168.1.5"; //"192.168.1.2"; //"10.101.81.135"; //"10.101.79.125"; // Get first 3 parts from "ifconfig" on PC
WD40andTape 7:5b6a2cefbf3b 17 const char* mask = "255.255.255.0"; // Get from "ifconfig" on PC
WD40andTape 17:bbaf3e8440ad 18 const char* gateway = "192.168.1.1"; //"10.101.81.1"; //"10.101.79.1"; // Get from "route -n" on PC
WD40andTape 7:5b6a2cefbf3b 19 error_code = interfaces.eth.set_network(ip, mask, gateway); // Dont use DHCP
WD40andTape 7:5b6a2cefbf3b 20 if( error_code < 0 ) {
dofydoink 12:595ed862e52f 21 if(IS_PRINT_OUTPUT) printf("Error %i. Could not network interface to use a static IP address. "
WD40andTape 7:5b6a2cefbf3b 22 "Perhaps the network is already connected?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 23 return -1;
WD40andTape 7:5b6a2cefbf3b 24 }
WD40andTape 7:5b6a2cefbf3b 25 }
WD40andTape 7:5b6a2cefbf3b 26 error_code = interfaces.eth.connect();
WD40andTape 7:5b6a2cefbf3b 27 if( error_code < 0 ) {
dofydoink 12:595ed862e52f 28 if(IS_PRINT_OUTPUT) printf("Error %i. Could not start the Ethernet interface.\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 29 return -1;
WD40andTape 7:5b6a2cefbf3b 30 }
WD40andTape 7:5b6a2cefbf3b 31 const char* server_ip = interfaces.eth.get_ip_address();
WD40andTape 7:5b6a2cefbf3b 32 if( server_ip == NULL ) {
WD40andTape 7:5b6a2cefbf3b 33 error_code = -1;
dofydoink 12:595ed862e52f 34 if(IS_PRINT_OUTPUT) printf("Error %i. Ethernet interface is not yet connected.\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 35 return -1;
WD40andTape 7:5b6a2cefbf3b 36 }
dofydoink 12:595ed862e52f 37 if(IS_PRINT_OUTPUT) printf("The target IP address is '%s'\n\r", server_ip);
WD40andTape 7:5b6a2cefbf3b 38 error_code = interfaces.srv.open(&interfaces.eth); // Open the server on ethernet stack
WD40andTape 7:5b6a2cefbf3b 39 if( error_code < 0 ) {
dofydoink 12:595ed862e52f 40 if(IS_PRINT_OUTPUT) printf("Error %i. Could not open server socket.\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 41 return -1;
WD40andTape 7:5b6a2cefbf3b 42 }
WD40andTape 7:5b6a2cefbf3b 43 error_code = interfaces.srv.bind(interfaces.eth.get_ip_address(), _port); // Bind the HTTP port (TCP 80) to the server
WD40andTape 7:5b6a2cefbf3b 44 if( error_code < 0 ) {
dofydoink 12:595ed862e52f 45 if(IS_PRINT_OUTPUT) printf("Error %i. Could not bind server socket to port '%d'.\n\r", error_code, _port);
WD40andTape 7:5b6a2cefbf3b 46 return -1;
WD40andTape 7:5b6a2cefbf3b 47 }
WD40andTape 7:5b6a2cefbf3b 48 error_code = interfaces.srv.listen(1); // Listen for 1 simultaneous connection
WD40andTape 7:5b6a2cefbf3b 49 if( error_code < 0 ) {
dofydoink 12:595ed862e52f 50 if(IS_PRINT_OUTPUT) printf("Error %i. Could not put server socket into listening mode.\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 51 return -1;
WD40andTape 7:5b6a2cefbf3b 52 }
WD40andTape 7:5b6a2cefbf3b 53 return 0;
WD40andTape 7:5b6a2cefbf3b 54 }
WD40andTape 7:5b6a2cefbf3b 55
WD40andTape 7:5b6a2cefbf3b 56
WD40andTape 7:5b6a2cefbf3b 57 int HLComms::accept_connection(void) {
dofydoink 12:595ed862e52f 58 if(IS_PRINT_OUTPUT) printf("Waiting to accept a connection on the TCP socket.\n\r");
WD40andTape 7:5b6a2cefbf3b 59 int error_code;
WD40andTape 7:5b6a2cefbf3b 60 error_code = interfaces.srv.accept(&interfaces.clt_sock, &interfaces.clt_addr); // Blocks until data is sent
WD40andTape 7:5b6a2cefbf3b 61 if( error_code < 0 ) {
dofydoink 12:595ed862e52f 62 if(IS_PRINT_OUTPUT) printf("Error %i. Could not create a network socket using the specified socket instance. "
WD40andTape 7:5b6a2cefbf3b 63 "Perhaps the socket is set to non-blocking or timed out?\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 64 return -1;
WD40andTape 7:5b6a2cefbf3b 65 }
dofydoink 12:595ed862e52f 66 if(IS_PRINT_OUTPUT) printf("Accepted %s:%d\n\r", interfaces.clt_addr.get_ip_address(), interfaces.clt_addr.get_port());
WD40andTape 7:5b6a2cefbf3b 67 return 0;
WD40andTape 7:5b6a2cefbf3b 68 }
WD40andTape 7:5b6a2cefbf3b 69
WD40andTape 7:5b6a2cefbf3b 70
WD40andTape 7:5b6a2cefbf3b 71 void HLComms::close_server(void) {
dofydoink 12:595ed862e52f 72 if(IS_PRINT_OUTPUT) printf("Closing server...\n\r");
WD40andTape 7:5b6a2cefbf3b 73 interfaces.clt_sock.close();
WD40andTape 7:5b6a2cefbf3b 74 interfaces.srv.close();
WD40andTape 7:5b6a2cefbf3b 75 interfaces.eth.disconnect();
WD40andTape 7:5b6a2cefbf3b 76 return;
WD40andTape 7:5b6a2cefbf3b 77 }
WD40andTape 7:5b6a2cefbf3b 78
WD40andTape 7:5b6a2cefbf3b 79
WD40andTape 9:cd3607ba5643 80 int HLComms::receive_message(void) {
WD40andTape 9:cd3607ba5643 81 memset(recv_buffer, 0, sizeof(recv_buffer));
WD40andTape 17:bbaf3e8440ad 82 int error_code = interfaces.clt_sock.recv(recv_buffer, sizeof(recv_buffer)-1); // Blocks until data is received
dofydoink 12:595ed862e52f 83 if(IS_PRINT_OUTPUT) printf("Message received.\r\n");
WD40andTape 9:cd3607ba5643 84 return error_code;
WD40andTape 9:cd3607ba5643 85 }
WD40andTape 9:cd3607ba5643 86
WD40andTape 9:cd3607ba5643 87
WD40andTape 9:cd3607ba5643 88 msg_format HLComms::process_message(void) { //HLComms::msg_format
WD40andTape 9:cd3607ba5643 89 unsigned char * msg = recv_buffer;
WD40andTape 7:5b6a2cefbf3b 90 // Break message string into chunks at each comma
WD40andTape 7:5b6a2cefbf3b 91 vector<string> tokens;
WD40andTape 7:5b6a2cefbf3b 92 char * delim = ",";
WD40andTape 7:5b6a2cefbf3b 93 char * token = strtok((char *)msg, delim);
WD40andTape 7:5b6a2cefbf3b 94 while(token != NULL) {
WD40andTape 7:5b6a2cefbf3b 95 tokens.push_back(string(token));
dofydoink 12:595ed862e52f 96 //if(IS_PRINT_OUTPUT) printf("%s\n\r",token);
WD40andTape 7:5b6a2cefbf3b 97 token = strtok(NULL, delim);
WD40andTape 7:5b6a2cefbf3b 98 }
WD40andTape 7:5b6a2cefbf3b 99 // Cast into doubles and assign to variables
WD40andTape 7:5b6a2cefbf3b 100 struct msg_format input;
WD40andTape 7:5b6a2cefbf3b 101 stringstream(tokens[0]) >> input.psi[0][0];
WD40andTape 7:5b6a2cefbf3b 102 stringstream(tokens[1]) >> input.psi[0][1];
WD40andTape 7:5b6a2cefbf3b 103 stringstream(tokens[2]) >> input.psi[0][2];
WD40andTape 7:5b6a2cefbf3b 104 stringstream(tokens[3]) >> input.psi[1][0];
WD40andTape 7:5b6a2cefbf3b 105 stringstream(tokens[4]) >> input.psi[1][1];
WD40andTape 7:5b6a2cefbf3b 106 stringstream(tokens[5]) >> input.psi[1][2];
WD40andTape 7:5b6a2cefbf3b 107 stringstream(tokens[6]) >> input.psi[2][0];
WD40andTape 7:5b6a2cefbf3b 108 stringstream(tokens[7]) >> input.psi[2][1];
WD40andTape 7:5b6a2cefbf3b 109 stringstream(tokens[8]) >> input.psi[2][2];
WD40andTape 7:5b6a2cefbf3b 110 stringstream(tokens[9]) >> input.duration;
WD40andTape 7:5b6a2cefbf3b 111
WD40andTape 14:54c3759e76ed 112 /*if(input.psi[0][0] < 0.0) input.psi[0][0] = 0.0;
WD40andTape 7:5b6a2cefbf3b 113 if(input.psi[0][1] < 0.0) input.psi[0][1] = 0.0;
WD40andTape 7:5b6a2cefbf3b 114 if(input.psi[0][2] < 0.0) input.psi[0][2] = 0.0;
WD40andTape 7:5b6a2cefbf3b 115 if(input.psi[1][0] < 0.0) input.psi[1][0] = 0.0;
WD40andTape 7:5b6a2cefbf3b 116 if(input.psi[1][1] < 0.0) input.psi[1][1] = 0.0;
WD40andTape 7:5b6a2cefbf3b 117 if(input.psi[1][2] < 0.0) input.psi[1][2] = 0.0;
WD40andTape 7:5b6a2cefbf3b 118 if(input.psi[2][0] < 0.0) input.psi[2][0] = 0.0;
WD40andTape 7:5b6a2cefbf3b 119 if(input.psi[2][1] < 0.0) input.psi[2][1] = 0.0;
WD40andTape 7:5b6a2cefbf3b 120 if(input.psi[2][2] < 0.0) input.psi[2][2] = 0.0;
WD40andTape 14:54c3759e76ed 121 if(input.duration < 0.0) input.duration = 0.0;*/
WD40andTape 7:5b6a2cefbf3b 122 /*if( input.psi[2][0] > 0.14 ) {
dofydoink 12:595ed862e52f 123 if(IS_PRINT_OUTPUT) printf("Hi\n\r");
WD40andTape 7:5b6a2cefbf3b 124 }*/
WD40andTape 7:5b6a2cefbf3b 125 //throw std::invalid_argument( "received negative value" );
WD40andTape 7:5b6a2cefbf3b 126 //return 0;
WD40andTape 7:5b6a2cefbf3b 127 return input;
WD40andTape 9:cd3607ba5643 128 } // End of consume_message()
WD40andTape 9:cd3607ba5643 129
WD40andTape 17:bbaf3e8440ad 130 int HLComms::send_duration_message(double *dblTime) {
WD40andTape 17:bbaf3e8440ad 131 send_mutex.lock();
WD40andTape 9:cd3607ba5643 132 memset(send_buffer, 0, sizeof(send_buffer));
WD40andTape 17:bbaf3e8440ad 133 _snprintf(send_buffer,sizeof(send_buffer),"0,%f",*dblTime);
WD40andTape 17:bbaf3e8440ad 134 int error_code = send_message();
WD40andTape 17:bbaf3e8440ad 135 send_mutex.unlock();
WD40andTape 17:bbaf3e8440ad 136 return error_code;
WD40andTape 17:bbaf3e8440ad 137 }
WD40andTape 17:bbaf3e8440ad 138
WD40andTape 17:bbaf3e8440ad 139 int HLComms::send_sensor_message(double positions[], double pressures[]) {
WD40andTape 17:bbaf3e8440ad 140 send_mutex.lock();
WD40andTape 17:bbaf3e8440ad 141 memset(send_buffer, 0, sizeof(send_buffer));
WD40andTape 17:bbaf3e8440ad 142 _snprintf(send_buffer,sizeof(send_buffer),"1,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f,%0.5f",
WD40andTape 17:bbaf3e8440ad 143 positions[0],positions[1],positions[2],positions[3],positions[4],positions[5],positions[6],positions[7],
WD40andTape 17:bbaf3e8440ad 144 pressures[0],pressures[1],pressures[2],pressures[3],pressures[4],pressures[5],pressures[6],pressures[7]);
WD40andTape 17:bbaf3e8440ad 145 int error_code = send_message();
WD40andTape 17:bbaf3e8440ad 146 send_mutex.unlock();
WD40andTape 17:bbaf3e8440ad 147 return error_code;
WD40andTape 9:cd3607ba5643 148 }
WD40andTape 9:cd3607ba5643 149
WD40andTape 9:cd3607ba5643 150 int HLComms::send_message(void) {
dofydoink 12:595ed862e52f 151 if(IS_PRINT_OUTPUT) printf("Sending message...\r\n");
WD40andTape 9:cd3607ba5643 152 char * msg = send_buffer;
WD40andTape 9:cd3607ba5643 153 int error_code = interfaces.clt_sock.send(msg, strlen(msg));
dofydoink 12:595ed862e52f 154 if(IS_PRINT_OUTPUT) printf("Message sent.\r\n");
WD40andTape 9:cd3607ba5643 155 return error_code;
WD40andTape 9:cd3607ba5643 156 }