drive.c

Go to the documentation of this file.

#include <config.h>

#include <stdio.h>
#include <sys/time.h>
#include <time.h>
#include <errno.h>
#include <string.h>
#include <math.h>

#include <drivers/drivers.h>
#include <cartcomm/comm.h>
#include <conf.h>

#define DEBUG_LEVEL 2
#include <debug.h>

//#define KEEP_TIME

#define SLEEP_OVERHEAD_US 850


void drive(drivefunc_t func, void *data) {
    int sockfd = -1;
    int result = 0;
    int func_result = 0;
    char msg[MAX_MSG_SIZE];
    cframe_t cf;
    int next_msg = -1;
    int retries = 0;
    struct timespec req, rem;

#ifdef KEEP_TIME
    struct timeval now, then;
    long passed_time;
#endif

    while(func_result == 0) {
#ifdef KEEP_TIME
        gettimeofday(&now, NULL);

        passed_time = now.tv_usec - then.tv_usec;

//        adjustment = (long) ((0.9 * adjustment) + (0.1 * (COMM_PERIOD_US - passed_time)));

//        DEBUG2("Actual loop time = %ld uS", passed_time);
//        DEBUG2("Error of loop time = %ld uS", COMM_PERIOD_US - passed_time);

        gettimeofday(&then, NULL);
#endif

        if(sockfd < 0) {
            req.tv_sec = 0;
            req.tv_nsec = COMM_PERIOD_US * 500;
            nanosleep(&req, &rem);

            DEBUG1("Trying to reconnect to server..");
            sockfd = cartcomm_connect_sync(conf.server, conf.port);
            if(sockfd < 0)
                continue;

            next_msg = -1;
        }

        // This read will block, we have nothing better to be doing
        result = cartcomm_read_msg(sockfd, msg, sizeof(msg));
        if(result < 0) {
            DEBUG1("read_msg failed!");
            retries++;

            if(retries > MAX_RETRIES) {
                cartcomm_disconnect(sockfd);
                sockfd = -1;
                retries = 0;
            }

            continue;
        }
        retries = 0;

        DEBUG3("\n<<< %s", msg);

        result = cartcomm_decode_msg(&cf, msg);
        if(result < 0) {
            DEBUG1("decode_msg failed");
            continue;
        }

        if(next_msg > -1) {
            if(cf.seqno != next_msg) {
                DEBUG1("Missed message(s): %ld", cf.seqno - next_msg);
            }
        }
        next_msg = cf.seqno + 1;

        // XXX Mode switching, need to put something otherwise it's empty
        strcpy(cf.cmd_mode, "auto");

        // Patch up some values before calling the driver function
        cf.cmd_irad -= conf.steering_offset;
        cf.cur_irad -= conf.steering_offset;

        func_result = func(&cf, data);

        // Unpatch up those values (otherwise the cart will complain)
        cf.cmd_irad += conf.steering_offset;
        cf.cur_irad += conf.steering_offset;

        // If we are done, then make sure to tell the cart to stop
        if(func_result != 0) {
            cf.cmd_speed = 0.0;
            cf.cmd_irad = 0.0;
        }

        cartcomm_encode_msg(&cf, msg);

        DEBUG3("\n>>> %s", msg);

        result = cartcomm_send_msg(sockfd, msg);
        if(result < 0) {
            DEBUG1("send_msg failed");
            return;
        }

#ifdef KEEP_TIME
        gettimeofday(&now, NULL);

        passed_time = now.tv_usec - then.tv_usec;
        DEBUG2("Took %ld usec to process", passed_time);

        gettimeofday(&now, NULL);

/*
        rem.tv_sec = 0;
        rem.tv_nsec = (COMM_PERIOD_US - SLEEP_OVERHEAD_US - passed_time) * 1000;

wait_more:
        req.tv_sec = 0;
        req.tv_nsec = rem.tv_nsec;
        result = nanosleep(&req, &rem);
        if(result < 0 && errno == EINTR)
            goto wait_more;
*/

#endif
    }

    cartcomm_disconnect(sockfd);
}

---