mahd/implementation/software/main.c

#include <stdlib.h>
#include <stdio.h>
#define GB_MATH_IMPLEMENTATION
#include "include/gb_math.h"

#include "include/stepper.h"
#include "include/path.h"
#include "include/stepper_params.h"
#include "include/sensor.h"
#include "include/controller.h"
#include "xtimer.h"
#include "thread.h"
#include "servo.h"
#include "thread_flags.h"

#define STACKSIZE               THREAD_STACKSIZE_DEFAULT
#define PRIO                    (THREAD_PRIORITY_MAIN - 1)


static char stack[STACKSIZE];
//static event_queue_t queue;

void test_velocity(stepper_t *stepper, int steps){
    //float angle;
    stepper_set_direction(stepper, STEPPER_FORWARD);
    for ( int i = 0; i < steps; i++)
    {
        stepper_step(stepper);
        xtimer_usleep(400);
        //stepper_get_angle(stepper, &angle);
        //printf("Angle: %f | Steps: %i\n",angle,i);
    }
    printf("reverse\n");
    stepper_set_direction(stepper, STEPPER_REVERSE);
    for ( int i = 0; i < steps; i++)
    {
        stepper_step(stepper);
        xtimer_usleep(400);
        //stepper_get_angle(stepper, &angle);
        //printf("Angle: %f | Steps: %i\n",angle,i);
    }
}

typedef struct{
    event_t event;
    thread_t *thread;
    thread_flags_t flags;
} test_t;

static void *claiming_thread(void *arg)
{
    event_queue_t *dqs = arg;

    event_queue_claim(dqs);

    event_loop(dqs);

    return NULL;
}

/*int main(void)
{
    event_queue_t event_queue = EVENT_QUEUE_INIT_DETACHED;
    event_queue_init_detached(&event_queue);
    thread_create(stack, sizeof(stack), PRIO, 0, claiming_thread, &event_queue, "ct");
    event_t event1 = {.handler = set1};
    event_t event2 = {.handler = set1};
    thread_t *main_thread = thread_get_active();
    test_t test1 = {
        .event = event1,
        .thread = main_thread,
        .flags = 0x1
    };
    test_t test2 = {
        .event = event2,
        .thread = main_thread,
        .flags = 0x2
    };
    event_timeout_t timeout1;
    event_timeout_t timeout2;
    event_timeout_init(&timeout1, &event_queue, (event_t*)&test1);
    event_timeout_init(&timeout2, &event_queue, (event_t*)&test2);
    event_timeout_set(&timeout1, 1000000);
    event_timeout_set(&timeout2, 3000000);
    printf("Timeouts are set\n");
    thread_flags_wait_all(0x3);
    printf("Flags are set\n");
}*/


/*
int main(void)
{
    event_queue_init(&queue);
    stepper_t stepper_a;
    stepper_a.queue = &queue;
    stepper_init(&stepper_a, &stepper_params[0]);
    test_velocity(&stepper_a,800);
    //stepper_set_velocity(&stepper_a, 20);
    //event_loop(&queue);
    stepper_disable(&stepper_a);
    return 0;
}
*/

/*int main(void)
{

    stepper_t stepper_a;
    stepper_t stepper_b;
    controller_t controller_scara;
    controller_scara.stepper_a = &stepper_a;
    controller_scara.stepper_b = &stepper_b;
    controller_scara.queue = &queue;
    controller_scara.time_step = 10000;
    controller_scara.arm_a = 0.070;
    controller_scara.arm_b = 0.055;
    gbVec2 setpoint = {
        .x = 0.015,
        .y = 0.085,
    };
    float anglea;
    float angleb;
    controller_inverse_kinematics(&controller_scara, &setpoint, &anglea, &angleb);
    printf("Angle A: %f, B: %f\n", anglea,angleb);
}
*/

int main(void)
{
    //printf("Frequentie: %lu Hz\n");
    servo_t servo;
    int servo_ret = servo_init(&servo, PWM_DEV(0), 2, 0, 1000);
    printf("Servo_set: %i\n", servo_ret);
    servo_set(&servo,350);
    xtimer_usleep(1000000);
    servo_set(&servo,750);
    xtimer_usleep(1000000);
    return 0;

    
    event_queue_t queue = EVENT_QUEUE_INIT_DETACHED;
    event_queue_init_detached(&queue);
    thread_create(stack, sizeof(stack), PRIO, 0, claiming_thread, &queue, "ct");

    thread_t* main_thread = thread_get_active();


    path_t path;
    path_init(&path);
    //point_t point = {.x = 0, .y = 0};
    //path_add_vec(&path, &point, PATH_DRAW);
    //point_t point1= {.x = 72, .y = 90};
    //path_add_vec(&path, &point1, PATH_DRAW);
    path_add_char(&path, 'S');
    path_add_char(&path, 'Q');
    path_add_char(&path, 'X');
    point_t point1= {.x = 72, .y = 89};
    path_add_vec(&path, &point1, PATH_MOVE);
    point_t point2= {.x = 72, .y = 89};
    path_add_vec(&path, &point2, PATH_MOVE);
    //path_add_char(&path, '!');
    path_reverse(&path);
    path_reset(&path);
    
    // Set stepper A
    stepper_t stepper_a;
    stepper_a.queue = &queue;
    stepper_a.thread = main_thread;
    stepper_a.flags = 0x1;
    stepper_a.id = 'A';
    stepper_init(&stepper_a, &stepper_params[0]);

    // Set stepper B
    stepper_t stepper_b;
    stepper_b.queue = &queue;
    stepper_b.thread = main_thread;
    stepper_b.flags = 0x2;
    stepper_b.id = 'B';
    stepper_params_t stepper_params_b;
    memcpy(&stepper_params_b, &stepper_params[0], sizeof(stepper_params_t));
    stepper_params_b.dir_pin = GPIO_PIN(1,1);
    stepper_params_b.step_pin = GPIO_PIN(1,2);
    stepper_params_b.enable_pin = GPIO_PIN(1,12);
    stepper_init(&stepper_b, &stepper_params_b);

    controller_t controller_scara;
    controller_scara.stepper_a = &stepper_a;
    controller_scara.stepper_b = &stepper_b;
    controller_scara.time_step = 4000;
    controller_scara.path = &path;
    controller_init(&controller_scara);
    for (int i = 0; i < 50; i++){
    controller_loop(&controller_scara);
    }
    printf("einde\n");



    //event_loop(&queue);
    return 0;
}


int test_registers(stepper_t *dev) {
    xtimer_usleep(10000);
    stepper_read(dev, 0x0);
    xtimer_usleep(10000);
    stepper_read(dev, 0x6C);
    xtimer_usleep(10000);
    stepper_read(dev, 0x70);
    xtimer_usleep(10000);
    return STEPPER_OK;
}