rosetta.c

Go to the documentation of this file.

#include <config.h>

#include <string.h>
#include <math.h>

#include <cartcomm/cframe.h>
#include <drivers/drivers.h>
#include <recorder.h>

#define DEG_TO_RAD (3.14159 / 180.0)
#define CART_WHEEL_BASE 1.667

typedef struct {
    int counter;
} state_t;

void *driver_rosetta_alloc(void) {
    state_t *state;

    state = malloc(sizeof(state_t));
    if(state == NULL)
        return NULL;

    state->counter = 0;

    return state;
}

void driver_rosetta_free(void *data) {
    free(data);
}

int driver_rosetta(cframe_t *cf, void *data) {
    state_t *state = (state_t *) data;

    recorder_add(cf);

    if(strcmp(cf->cur_mode, "auto") != 0)
        return 0;

    state->counter = (state->counter + 1) % 100;

    if(state->counter < 30) {
        cf->cmd_irad = sin(35.0 * DEG_TO_RAD) / CART_WHEEL_BASE;
        cf->cmd_speed = 2.5;
    } else if(state->counter < 70) {
        cf->cmd_irad = 0;
        cf->cmd_speed = 4.5;
    } else {
        cf->cmd_irad = -sin(35.0 * DEG_TO_RAD) / CART_WHEEL_BASE;
        cf->cmd_speed = 1.5;
    }

    return 0;
}

---