MorphGI / Mbed OS MGI_Rebuild_MidLevel_9_0-Basic_PWM

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ros_lib_kinetic

HLComms.cpp@11:7029367a1840, 2018-08-10 (annotated)

Committer:
dofydoink
Date:
Fri Aug 10 10:40:18 2018 +0000
Revision:
11:7029367a1840
Parent:
9:cd3607ba5643
Child:
12:595ed862e52f
Tested and working with 1 actuator.

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;
WD40andTape 7:5b6a2cefbf3b 14 printf("Starting TCP server...\n\r");
WD40andTape 7:5b6a2cefbf3b 15 if( !IS_DHCP ) {
dofydoink 11:7029367a1840 16 const char* ip = "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
dofydoink 11:7029367a1840 18 const char* gateway = "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 ) {
WD40andTape 7:5b6a2cefbf3b 21 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 ) {
WD40andTape 7:5b6a2cefbf3b 28 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;
WD40andTape 7:5b6a2cefbf3b 34 printf("Error %i. Ethernet interface is not yet connected.\n\r", error_code);
WD40andTape 7:5b6a2cefbf3b 35 return -1;
WD40andTape 7:5b6a2cefbf3b 36 }
WD40andTape 7:5b6a2cefbf3b 37 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 ) {
WD40andTape 7:5b6a2cefbf3b 40 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 ) {
WD40andTape 7:5b6a2cefbf3b 45 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 ) {
WD40andTape 7:5b6a2cefbf3b 50 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) {
WD40andTape 7:5b6a2cefbf3b 58 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 ) {
WD40andTape 7:5b6a2cefbf3b 62 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 }
WD40andTape 7:5b6a2cefbf3b 66 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) {
WD40andTape 9:cd3607ba5643 72 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 9:cd3607ba5643 82 int error_code = interfaces.clt_sock.recv(recv_buffer, 1024); // Blocks until data is received
WD40andTape 9:cd3607ba5643 83 return error_code;
WD40andTape 9:cd3607ba5643 84 }
WD40andTape 9:cd3607ba5643 85
WD40andTape 9:cd3607ba5643 86
WD40andTape 9:cd3607ba5643 87 msg_format HLComms::process_message(void) { //HLComms::msg_format
WD40andTape 9:cd3607ba5643 88 unsigned char * msg = recv_buffer;
WD40andTape 7:5b6a2cefbf3b 89 // Break message string into chunks at each comma
WD40andTape 7:5b6a2cefbf3b 90 vector<string> tokens;
WD40andTape 7:5b6a2cefbf3b 91 char * delim = ",";
WD40andTape 7:5b6a2cefbf3b 92 char * token = strtok((char *)msg, delim);
WD40andTape 7:5b6a2cefbf3b 93 while(token != NULL) {
WD40andTape 7:5b6a2cefbf3b 94 tokens.push_back(string(token));
WD40andTape 7:5b6a2cefbf3b 95 //printf("%s\n\r",token);
WD40andTape 7:5b6a2cefbf3b 96 token = strtok(NULL, delim);
WD40andTape 7:5b6a2cefbf3b 97 }
WD40andTape 7:5b6a2cefbf3b 98 // Cast into doubles and assign to variables
WD40andTape 7:5b6a2cefbf3b 99 struct msg_format input;
WD40andTape 7:5b6a2cefbf3b 100 stringstream(tokens[0]) >> input.psi[0][0];
WD40andTape 7:5b6a2cefbf3b 101 stringstream(tokens[1]) >> input.psi[0][1];
WD40andTape 7:5b6a2cefbf3b 102 stringstream(tokens[2]) >> input.psi[0][2];
WD40andTape 7:5b6a2cefbf3b 103 stringstream(tokens[3]) >> input.psi[1][0];
WD40andTape 7:5b6a2cefbf3b 104 stringstream(tokens[4]) >> input.psi[1][1];
WD40andTape 7:5b6a2cefbf3b 105 stringstream(tokens[5]) >> input.psi[1][2];
WD40andTape 7:5b6a2cefbf3b 106 stringstream(tokens[6]) >> input.psi[2][0];
WD40andTape 7:5b6a2cefbf3b 107 stringstream(tokens[7]) >> input.psi[2][1];
WD40andTape 7:5b6a2cefbf3b 108 stringstream(tokens[8]) >> input.psi[2][2];
WD40andTape 7:5b6a2cefbf3b 109 stringstream(tokens[9]) >> input.duration;
WD40andTape 7:5b6a2cefbf3b 110
WD40andTape 7:5b6a2cefbf3b 111 if(input.psi[0][0] < 0.0) input.psi[0][0] = 0.0;
WD40andTape 7:5b6a2cefbf3b 112 if(input.psi[0][1] < 0.0) input.psi[0][1] = 0.0;
WD40andTape 7:5b6a2cefbf3b 113 if(input.psi[0][2] < 0.0) input.psi[0][2] = 0.0;
WD40andTape 7:5b6a2cefbf3b 114 if(input.psi[1][0] < 0.0) input.psi[1][0] = 0.0;
WD40andTape 7:5b6a2cefbf3b 115 if(input.psi[1][1] < 0.0) input.psi[1][1] = 0.0;
WD40andTape 7:5b6a2cefbf3b 116 if(input.psi[1][2] < 0.0) input.psi[1][2] = 0.0;
WD40andTape 7:5b6a2cefbf3b 117 if(input.psi[2][0] < 0.0) input.psi[2][0] = 0.0;
WD40andTape 7:5b6a2cefbf3b 118 if(input.psi[2][1] < 0.0) input.psi[2][1] = 0.0;
WD40andTape 7:5b6a2cefbf3b 119 if(input.psi[2][2] < 0.0) input.psi[2][2] = 0.0;
WD40andTape 7:5b6a2cefbf3b 120 if(input.duration < 0.0) input.duration = 0.0;
WD40andTape 7:5b6a2cefbf3b 121 /*if( input.psi[2][0] > 0.14 ) {
WD40andTape 7:5b6a2cefbf3b 122 printf("Hi\n\r");
WD40andTape 7:5b6a2cefbf3b 123 }*/
WD40andTape 7:5b6a2cefbf3b 124 //throw std::invalid_argument( "received negative value" );
WD40andTape 7:5b6a2cefbf3b 125 //return 0;
WD40andTape 7:5b6a2cefbf3b 126 return input;
WD40andTape 9:cd3607ba5643 127 } // End of consume_message()
WD40andTape 9:cd3607ba5643 128
WD40andTape 9:cd3607ba5643 129 void HLComms::make_message(double *dblTime) {
WD40andTape 9:cd3607ba5643 130 memset(send_buffer, 0, sizeof(send_buffer));
WD40andTape 9:cd3607ba5643 131 _snprintf(send_buffer,64,"%f",*dblTime);
WD40andTape 9:cd3607ba5643 132 }
WD40andTape 9:cd3607ba5643 133
WD40andTape 9:cd3607ba5643 134 int HLComms::send_message(void) {
WD40andTape 9:cd3607ba5643 135 char * msg = send_buffer;
WD40andTape 9:cd3607ba5643 136 int error_code = interfaces.clt_sock.send(msg, strlen(msg));
WD40andTape 9:cd3607ba5643 137 return error_code;
WD40andTape 9:cd3607ba5643 138 }