static bool Running_State_Code(void * pObject, oosmos_sRegion * pRegion, const oosmos_sEvent * pEvent)
{
server * pServer = (server *) pObject;
switch (pEvent->Code) {
case oosmos_INSTATE:
oosmos_SyncBegin(pRegion);
while (true) {
printf("Waiting for incoming data...\n");
oosmos_SyncWaitCond(pRegion, sockReceive(pServer->m_pSock, pServer->m_Buffer, sizeof(pServer->m_Buffer), &pServer->m_BytesReceived));
printf("Server side, String: '%s', BytesReceived: %u\n", pServer->m_Buffer, (unsigned) pServer->m_BytesReceived);
oosmos_SyncWaitCond(pRegion, sockSend(pServer->m_pSock, pServer->m_Buffer, pServer->m_BytesReceived));
}
oosmos_SyncEnd(pRegion);
return true;
}
return false;
}
void openSockets(int servers, int max, char * logon, char * revision, char * extra) {
struct sockaddr_in serv_addr;
struct hostent *server;
int i;
if (!noserver) {
if (servers == 0) return; // It's a slave so exit immediately.
server = gethostbyname(HOSTNAME);
if (server == NULL) {
snprintf(buffer, sizeof(buffer), "FATAL %s %d Cannot resolve localhost", progname, controllernum);
logmsg(FATAL, buffer);
return; // To satisfy static code checker
}
bzero((char *) &serv_addr, sizeof(serv_addr));
serv_addr.sin_family = AF_INET;
bcopy((char *)server->h_addr,
(char *)&serv_addr.sin_addr.s_addr,
server->h_length);
serv_addr.sin_port = htons(PORTNO);
for(i = 0; i < servers; i++) {
sockfd[i] = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd[i] < 0) {
snprintf(buffer, sizeof(buffer), "FATAL %s %d Creating socket", progname, controllernum);
logmsg(FATAL, buffer);
}
if (connect(sockfd[i],(struct sockaddr *) &serv_addr,sizeof(serv_addr)) < 0) {
sockfd[i] = 0;
snprintf(buffer, sizeof(buffer), "FATAL %s %d Connecting to socket %d", progname, controllernum, i);
logmsg(FATAL, buffer);
}
// Logon to server
snprintf(buffer, sizeof(buffer), "logon %s %s %d %d %s", logon, getVersion(revision), getpid(), controllernum + i, extra);
sockSend(sockfd[i], buffer);
}
} else sockfd[0] = 1; // noserver - use stdout
}
void logmsg(int severity, char *msg) {
// Write error message to logfile and socket if possible and abort program for FATAL
// Truncate total message including timestamp and 'event ' to 206 bytes.
// Globals used: sockfd logfp
// Due to the risk of looping when you call this routine due to a problem writing on the socket,
// set sockfd to 0 first.
char buffer[206]; // This MUST be local!
time_t now;
if (strlen(msg) > 174) msg[174] = '\0';
now = time(NULL);
strcpy(buffer, ctime(&now));
buffer[24] = ' '; // replace newline with a space
strcat(buffer, msg);
strcat(buffer, "\n");
DEBUG printf("LOGMSG ");
if (logfp && severity > INFO) { // don't even log INFO to the file
fputs(buffer, logfp);
DEBUG printf("File ");
fflush(logfp);
}
if (sockfd[0] > 0) {
strcpy(buffer, "event ");
strcat(buffer, msg);
DEBUG printf("Socket ");
sockSend(sockfd[0], buffer);
}
DEBUG puts(buffer);
if (severity > ERROR) { // If severity is FATAL terminate program
if (logfp) fclose(logfp);
DEBUG printf("LOGMSG Closing log file\n");
exit(severity);
}
}
///<summary> Completely initializes a new POLY connection that has been recieved. Uses synchronous reads and sends. </summary>
///<param name='connection'> Connection socket for the incoming connection. </param>
///<param name='connection_info'> The connection info object associated with this connection. </param>
///<param name='out_connectionPointer'> (OUT) The connection object for the newly initialized connection. </param>
///<returns> Error code on failure, connection realm on success. <returns>
int initializeIncomingConnection(void *connection, void** out_connectionPointer)
{
// send greeting
if (sendGreeting(connection) == POLY_REALM_FAILED) return POLY_REALM_FAILED;
uint8_t buffer[8]; // create a re-usable buffer for operations
// attempt to recieve encryption setting
if (2 != sockRecv(connection, buffer, 2)) return POLY_REALM_FAILED;
// handle encryption negotiation
if (getShortFromBuffer(buffer) != 0) return POLY_REALM_FAILED; // TODO: encryption not yet implemented
// attempt to recieve realm code
if (2 != sockRecv(connection, buffer, 2)) return POLY_REALM_FAILED;
POLYM_CONNECTION_INFO *connection_info;
// connection realm
switch (getShortFromBuffer(buffer))
{
case POLY_REALM_PEER:
{
uint16_t connectionID = allocateNewPeer(&connection_info);
// initalize the peer connection
initializeNewPeerInfo(connection_info, connection, connectionID);
//construct the return buffer
buffer[0] = 0;
buffer[1] = 0; // set the first two bytes (result code) to 0 to indicate success
if (2 != sockSend(connection, buffer, 2)) //send the success code and peer ID
{
freePeer(connectionID);
return POLY_REALM_FAILED;
}
return POLY_REALM_PEER;
}
case POLY_REALM_SERVICE:
{
// this connection is a service
//attempt to recieve service string size
if (2 != sockRecv(connection, buffer, 2)) return POLY_REALM_FAILED;
// check validity of service string size, initialize string
int serviceStringSize = getShortFromBuffer(buffer);
if (serviceStringSize == 0) return POLY_REALM_FAILED;
// initialize new service connection
uint16_t connectionID = allocateNewService(&connection_info);
connection_info->realm_info.service.serviceString = allocateServiceString(serviceStringSize);
//get the service string
if (serviceStringSize != sockRecv(connection, connection_info->realm_info.service.serviceString->buf, serviceStringSize))
{
freeService(connectionID);
return POLY_REALM_FAILED;
}
// TODO: require user approval
// construct the last send buffer to send
buffer[0] = 0;
buffer[1] = 0; // set the first two bytes (result code) to 0 to indicate success
if (2 != sockSend(connection, buffer, 2))
{
// if for some reason this fails, we need to remove the connection from the service list
freeService(connectionID);
return POLY_REALM_SERVICE; // do not return fail so that the connection isn't closed twice
}
connection_info->connectionID = connectionID;
connection_info->realm = POLY_REALM_SERVICE;
return POLY_REALM_SERVICE;
}
case POLY_REALM_CLIENT:
{
uint16_t connectionID = allocateNewClient(&connection_info);
// initalize the client connection
connection_info->connectionID = connectionID;
connection_info->realm = POLY_REALM_CLIENT;
//construct the return buffer
buffer[0] = 0;
buffer[1] = 0; // set the first two bytes (result code) to 0 to indicate success
if (2 != sockSend(connection, buffer, 2)) //send the success code and peer ID
{
freeClient(connectionID);
return POLY_REALM_FAILED;
}
return POLY_REALM_CLIENT;
default:
return POLY_REALM_FAILED;
}
}
}
///<summary> Send the supplied polymorphic error code over the supplied connection as a synchronous action. </summary>
void sendErrorCode(uint16_t errorCode, void *connection)
{
uint8_t buffer[2];
insertShortIntoBuffer(buffer, errorCode);
sockSend(connection, buffer, 2);
}
///<summary> Handles the network negotiation of an outgoing connection. Uses synchronous reads and sends. </summary>
///<param name='ipAddress'> The IP address string to connect to. </param>
///<param name='l4Port'> The transport layer port to connection to. </param>
///<param name='protocol'> The protocol the connection will use. </param>
///<param name='out_connectionPointer'> (OUT) The connection object for the newly initialized connection. </param>
///<returns> Error code on failure, result code from remote server on success. <returns>
uint16_t initializeOutgoingConnection(char *ipAddress, uint16_t l4Port, uint8_t protocol, POLYM_CONNECTION_INFO **out_connectionInfo)
{
POLYM_CONNECTION_INFO *connection_info;
uint16_t connectionID = allocateNewPeer(&connection_info);
char l4PortString[6];
_itoa(l4Port, l4PortString, 10);
void *connection = openNewConnection(ipAddress, l4PortString, &connection_info, protocol);
if (connection == NULL)
return POLY_COMMAND_CONNECT_ERROR_CONNECTION_FAIL;
uint8_t buffer[500];
if (6 != sockRecv(connection, buffer, 6)) // recieve "POLY v" from greeting
{
sendErrorCode(POLY_COMMAND_CONNECT_ERROR_CONNECTION_FAIL, connection);
removeConnection(connection_info);
return POLY_COMMAND_CONNECT_ERROR_CONNECTION_FAIL;
}
int index = 6;
for (; index <= POLYM_GREETING_MAX_LENGTH; ++index) // recieve the rest of the greeting
{
if (1 != sockRecv(connection, &buffer[index], 1))
{
sendErrorCode(POLY_COMMAND_CONNECT_ERROR_CONNECTION_FAIL, connection);
removeConnection(connection_info);
return POLY_COMMAND_CONNECT_ERROR_CONNECTION_FAIL;
}
if (buffer[index] == '\n')
{
break;
}
}
if (index > POLYM_GREETING_MAX_LENGTH) // greeting is too long, close the connection
closeConnectionSocket(connection);
insertShortIntoBuffer(buffer, POLY_ENC_NONE); // encryption mode
insertShortIntoBuffer(&buffer[2], POLY_REALM_PEER); // realm code
sockSend(connection, buffer, 4); // send the connection request
if (2 != sockRecv(connection, buffer, 2))
{
sendErrorCode(POLY_COMMAND_CONNECT_ERROR_CONNECTION_FAIL, connection);
removeConnection(connection_info);
return POLY_COMMAND_CONNECT_ERROR_CONNECTION_FAIL;
}
initializeNewPeerInfo(connection_info, connection, connectionID);
// TODO: send service update request
*out_connectionInfo = connection_info;
return getShortFromBuffer(buffer); //return result code recieved from remote server
}
///<summary> Sends the polymorphic greeting over the supplied connection as a synchronous action. </summary>
///<returns> Error code on failure, 0 for success </returns>
int sendGreeting(void *connection)
{
if (sizeof(POLYM_GREETING) != sockSend(connection, POLYM_GREETING, sizeof(POLYM_GREETING)))
return POLY_REALM_FAILED;
return 0;
}
int main(int argc, char *argv[])
// arg1: controller number
{
int nolog = 0;
struct sockaddr_in serv_addr;
struct hostent *server;
char buffer[256];
int run = 1; // set to 0 to stop main loop
fd_set readfd;
int numfds;
int interval = 60;
int tmout = 90; // seconds to wait in select()
int saveInterval = 600; // Save to NVRAM every 10 mins
int logerror = 0;
int option;
int delay = 1000; // usec to sleep (but it rounds up to 20mSec anyway
time_t update = 0, now;
pthread_t tid;
time_t nextSave;
// Command line arguments
opterr = 0;
while ((option = getopt(argc, argv, "di:slVZ1:2:")) != -1) {
switch (option) {
case 's': noserver = 1; break;
case 'l': nolog = 1; break;
case '?': usage(); exit(1);
case 'i': interval = atoi(optarg); break;
case 'd': debug++; break;
case '1': rate1 = rate2 = atof(optarg); break;
case '2': rate2 = atof(optarg); break;
case 'V': printf("Version %s %s\n", getversion(), id); exit(0);
case 'Z': decode("(b+#Gjv~z`mcx-@ndd`rxbwcl9Vox=,/\x10\x17\x0e\x11\x14\x15\x11\x0b\x1a"
"\x19\x1a\x13\x0cx@NEEZ\\F\\ER\\\x19YTLDWQ'a-1d()#!/#(-9' >q\"!;=?51-??r"); exit(0);
}
}
DEBUG printf("Debug %d. optind %d argc %d\n", debug, optind, argc);
if (optind < argc) controllernum = atoi(argv[optind]); // get optional controller number: parameter 1
sprintf(buffer, LOGFILE, controllernum);
if (!nolog) if ((logfp = fopen(buffer, "a")) == NULL) logerror = errno;
// There is no point in logging the failure to open the logfile
// to the logfile, and the socket is not yet open.
sprintf(buffer, "STARTED %s as %d interval %d %s", argv[0], controllernum, interval, nolog ? "nolog" : "");
logmsg(INFO, buffer);
// Set up socket
if (!noserver) {
sockfd = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd < 0)
logmsg(FATAL, "FATAL " PROGNAME " Creating socket");
server = gethostbyname("localhost");
if (server == NULL) {
logmsg(FATAL, "FATAL " PROGNAME " Cannot resolve localhost");
}
bzero((char *) &serv_addr, sizeof(serv_addr));
serv_addr.sin_family = AF_INET;
bcopy((char *)server->h_addr,
(char *)&serv_addr.sin_addr.s_addr,
server->h_length);
serv_addr.sin_port = htons(PORTNO);
if (connect(sockfd,(struct sockaddr *) &serv_addr,sizeof(serv_addr)) < 0) {
sockfd = 0;
logmsg(ERROR, "ERROR " PROGNAME " Connecting to socket");
}
if (flock(fileno(logfp), LOCK_EX | LOCK_NB) == -1) {
logmsg(FATAL, "FATAL " PROGNAME " is already running, cannot start another one");
}
// Logon to server as meter
sprintf(buffer, "logon " progname " %s %d %d", getversion(), getpid(), controllernum);
sockSend(buffer);
}
else sockfd = 1; // noserver: use stdout
// If we failed to open the logfile and were NOT called with nolog, warn server
// Obviously don't use logmsg!
if (logfp == NULL && nolog == 0) {
sprintf(buffer, "event WARN " PROGNAME " %d could not open logfile %s: %s", controllernum, LOGFILE, strerror(logerror));
sockSend(buffer);
}
// Set up hardware
int mem = open("/dev/mem", O_RDWR);
unsigned char * start = mmap(0, getpagesize(), PROT_READ|PROT_WRITE, MAP_SHARED, mem, 0x80840000);
pbdata = (unsigned int*)(start + 0x04);
pbddr = (unsigned int *)(start + 0x14);
// All inputs
*pbddr = DIO4 | DIO5; // DIO 4/5 outputs.
*pbdata = 0xff; // set to all 1's
numfds = sockfd + 1; // nfds parameter to select. One more than highest descriptor
// Main Loop
count1 = count2 = 0;
readCMOS();
DEBUG fprintf(stderr, "Got values from NVRAM: %d %d (%f %f)\n",
count1, count2, (count1 / rate1), (count2 / rate2));
// Start count thread
if (pthread_create(&tid, NULL, count_pulses, NULL) < 0)
perror("count_pulse");
DEBUG fprintf(stderr, "Thread started as %d (0x%x)\n", tid, tid);
FD_ZERO(&readfd);
update = timeMod(interval);
nextSave = timeMod(saveInterval);
while(run) {
struct timeval tv1;
now = time(NULL);
if (now > nextSave) {
DEBUG fprintf(stderr, "Saving to NVRAM ");
writeCMOS();
nextSave = timeMod(saveInterval);
DEBUG {
struct tm * t;
t = localtime(&nextSave);
strftime(buffer, sizeof(buffer), "%F %T", t);
fprintf(stderr, "Next save at %s\n", buffer);
}
}
if (now > update) { // message time. Will always send out 0 0 at startup.
int fd;
sprintf(buffer, "meter 2 %.1f %.1f", (count1 / rate1), (count2 / rate2));
sockSend(buffer);
update = timeMod(interval);
// Write to /tmp/pulse
fd = open("/tmp/pulse", O_RDWR | O_CREAT | O_TRUNC);
if (fd < 0) perror("/tmp/pulse");
if (fd > 0) {
strcat(buffer, "\n");
write(fd, buffer, strlen(buffer));
close(fd);
}
}
FD_SET(sockfd, &readfd);
tv1.tv_sec = 0;
tv1.tv_usec = tmout;
if (select(sockfd + 1, &readfd, NULL, NULL, &tv1) && sockfd > 1) // anything from server?
run = processSocket(); // the server may request a shutdown by setting run to 0
usleep(delay);
}
int main(int argc, char **argv)
{
//Set communication with robot
std::string msgReceived;
int robotSocket;
if ((robotSocket = socket(PF_INET, SOCK_STREAM, 0)) == -1)
{
printf("Problem creating robot socket. Error number: %d",errno);
exit(-1);
}
struct sockaddr_in remoteSocket;
remoteSocket.sin_family = AF_INET;
int port = ROBOT_PORT;
remoteSocket.sin_port = htons(port);
inet_pton(AF_INET, ROBOT_IP , &remoteSocket.sin_addr.s_addr);
if(connect(robotSocket, (sockaddr*)&remoteSocket,sizeof(remoteSocket)) == -1)
{
printf("Impossible to connect to the ABB robot. Error number: %d",errno);
exit(-1);
}
logging = false;
// Create a dedicated thread for logger
pthread_t loggerThread;
pthread_attr_t attrL;
pthread_attr_init(&attrL);
pthread_attr_setdetachstate(&attrL, PTHREAD_CREATE_JOINABLE);
if (pthread_create(&loggerThread, &attrL, loggerMain, NULL) != 0)
{
printf("Unable to create logger thread. Error number: %d.",errno);
exit(-1);
}
logging = true;
//Set workObject
sockSend(robotSocket, oad::setWorkObject(500.0, 0.0, 500.0, 0.0, 0.0, 1.0, 0.0));
sockReceive(robotSocket, msgReceived);
if(!oad::checkReply(msgReceived))
{
printf("Problem setting the WorkObject.\n");
exit(-1);
}
//Set tool
sockSend(robotSocket, oad::setTool(0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0));
sockReceive(robotSocket, msgReceived);
if(!oad::checkReply(msgReceived))
{
printf("Problem setting the Tool.\n");
exit(-1);
}
//Set speed
sockSend(robotSocket, oad::setSpeed(100,30));
sockReceive(robotSocket, msgReceived);
if(!oad::checkReply(msgReceived))
{
printf("Problem setting the Speed of the robot.\n");
exit(-1);
}
//Set zone
sockSend(robotSocket, oad::setPredefinedZone(1));
sockReceive(robotSocket, msgReceived);
if(!oad::checkReply(msgReceived))
{
printf("Problem setting the Zone of the robot.\n");
exit(-1);
}
//Motion
sockSend(robotSocket, oad::setCartesian(100.0, 100.0, 0.0, 1.0, 0.0, 0.0, 0.0));
sockReceive(robotSocket, msgReceived);
sockSend(robotSocket, oad::setCartesian(100.0, -100.0, 0.0, 1.0, 0.0, 0.0, 0.0));
sockReceive(robotSocket, msgReceived);
sockSend(robotSocket, oad::setCartesian(-100.0, -100.0, 0.0, 1.0, 0.0, 0.0, 0.0));
sockReceive(robotSocket, msgReceived);
sockSend(robotSocket, oad::setCartesian(-100.0, 100.0, 0.0, 1.0, 0.0, 0.0, 0.0));
sockReceive(robotSocket, msgReceived);
sockSend(robotSocket, oad::setCartesian(100.0, 100.0, 0.0, 1.0, 0.0, 0.0, 0.0));
sockReceive(robotSocket, msgReceived);
//Close communication with ABB controller
sockSend(robotSocket, oad::closeConnection());
sleep(1);
close(robotSocket);
//Stop logger thread
void *statusL;
logging = false;
pthread_join(loggerThread, &statusL);
pthread_attr_destroy(&attrL);
return 0;
}
