Add controller implementation

implementation/software/controller.c

@@ -0,0 +1,228 @@
+#include "include/controller.h"
+
+
+void print_point(gbVec2 *point)
+{
+    printf("position x: %.4f, y: %.4f\n", point->x, point->y);
+}
+
+int controller_init(controller_t *control)
+{
+    gpio_init(BTN0_PIN,BTN0_MODE);
+    event_t event = {.handler = controller_update};
+    control->event = event;
+    controller_home(control);
+    event_timeout_init(&(control->event_timeout), control->queue, &(control->event));
+    event_post(control->queue,&(control->event));
+    return 0;
+}
+
+
+void controller_update(event_t *event)
+{
+    controller_t *control = container_of(event, controller_t, event);
+    controller_calculate_velocity(control);
+    event_timeout_set(&(control->event_timeout), control->time_step);
+}
+
+
+int controller_setpoint(controller_t *control, gbVec2 *point)
+{
+    control->setpoint = *point;
+    controller_inverse_kinematics(control, point, &control->angle1_setpoint , &control->angle2_setpoint);
+    return 0;
+}
+
+void controller_inverse_kinematics(controller_t *control, gbVec2 *point, float *angle1, float *angle2)
+{
+    float phi = gb_arctan2(point->y, point->x);
+    float length_a2 = gb_square(control->arm_a);
+    float length_b2 = gb_square(control->arm_b);
+    float length_c2 = gb_vec2_mag2(*point);
+    float length_c = gb_sqrt(length_c2);
+    float b = gb_arccos((length_a2 + length_c2 - length_b2) / (2 * control->arm_a * length_c));
+    float c = gb_arccos((length_a2 + length_b2 - length_c2) / (2 * control->arm_a * control->arm_b));
+    *angle1 = b + phi;
+    *angle2 = *angle1 - GB_MATH_PI + c;
+}
+
+void controller_current_position(controller_t *control, gbVec2 *point)
+{
+    float angle_a;
+    stepper_get_angle(control->stepper_a, &angle_a);
+    float angle_b;
+    stepper_get_angle(control->stepper_b, &angle_b);
+    gbVec2 joint1 = {
+        .x = control->arm_a * gb_cos(angle_a),
+        .y = control->arm_a * gb_sin(angle_a)
+    };
+    gbVec2 joint2 = {
+        .x = control->arm_b * gb_cos(angle_b),
+        .y = control->arm_b * gb_sin(angle_b)
+    };
+    gb_vec2_add(point, joint1, joint2);
+}
+
+
+int controller_calculate_velocity(controller_t *control)
+{
+    uint32_t setpoint_time;
+    uint32_t ik_time;
+    uint32_t angle_time;
+    uint32_t velocity_time;
+    uint32_t start = xtimer_now_usec();
+    gbVec2 next_setpoint;
+    int status = controller_sub_setpoint(control, &next_setpoint, 0.005);
+    setpoint_time = xtimer_now_usec();
+    float angle1;
+    float angle2;
+    controller_inverse_kinematics(control, &next_setpoint, &angle1, &angle2);
+    ik_time = xtimer_now_usec();
+    float angle_a;
+    stepper_get_angle(control->stepper_a, &angle_a);
+    float angle_b;
+    stepper_get_angle(control->stepper_b, &angle_b);
+    angle_time = xtimer_now_usec();
+    //printf("angle1: %f, angle2 %f\n", angle1, angle2);
+    //printf("anglea: %f, angleb %f\n", angle_a, angle_b);
+    float omega1 = (angle1-angle_a)*25;
+    float omega2 = (angle2-angle_b)*25;
+    if (status == CONTROLLER_STATISFIED)
+    {
+        omega1 = 0.0;
+        omega2 = 0.0;
+    }
+   // printf("omega1: %f, omega2 %f\n", omega1, omega2);
+    stepper_set_velocity(control->stepper_a, omega1);
+    stepper_set_velocity(control->stepper_b, omega2);
+    velocity_time = xtimer_now_usec();
+    printf("Setpoint: %lu\n", setpoint_time - start);
+    printf("IK-time: %lu\n", ik_time - setpoint_time);
+    printf("Angles: %lu\n", angle_time - ik_time);
+    printf("velocity: %lu\n", velocity_time - angle_time);
+    return 0;
+}
+
+int controller_sub_setpoint(controller_t *control, gbVec2 *setpoint, float distance)
+{
+    // TODO: if the length is shorten than the distance, do not scale it up
+    // TODO: if the length is zero. Do nothing.
+    // get current position.
+    gbVec2 current_position;
+
+    controller_current_position(control, &current_position);
+    // print_point(&current_position);
+
+    // compare current position with setpoint.
+    //
+    while(1){
+        gbVec2 position_error;
+        path_node_t *next_point = path_next_point(control->path);
+        //gb_vec2_sub(&position_error, next_point->point, current_position);
+        float error_magnitude = gb_vec2_mag(position_error);
+        gbVec2 scaled_position_error;
+        //printf("Magnitude: %f Error ", error_magnitude);
+        //print_point(&position_error);
+        if (error_magnitude > distance)
+        {
+            gb_vec2_div(&scaled_position_error, position_error, error_magnitude/distance);
+            gb_vec2_add(setpoint, current_position, scaled_position_error);
+            //printf("Next ");
+            //print_point(setpoint);
+            return CONTROLLER_OK;
+        }
+        else if (next_point->list.next == NULL)
+        {
+            scaled_position_error = current_position;
+            gb_vec2_add(setpoint, current_position, scaled_position_error);
+            //printf("No new point\n");
+            if (error_magnitude < distance/8)
+            {
+                return CONTROLLER_STATISFIED;
+            }
+            return CONTROLLER_NO_PATH;
+        }
+        else 
+        {
+            path_increment(control->path);
+            printf("Increment Path\n");
+        }
+    }
+    /*gbVec2 position_error;
+      gb_vec2_sub(&position_error, control->setpoint, current_position);
+    // scale the length of the error.
+    // to limit the length.
+    control->error = gb_vec2_mag(position_error);
+    gbVec2 scaled_position_error;
+    if (control->error > distance) {
+    gb_vec2_div(&scaled_position_error, position_error, control->error/distance);
+    } else {
+    scaled_position_error = position_error;
+    }
+    gb_vec2_add(setpoint, current_position, scaled_position_error);
+    return 0;*/
+}
+
+int controller_home(controller_t *control){
+    stepper_disable(control->stepper_b);
+    stepper_disable(control->stepper_a);
+    while(gpio_read(BTN0_PIN)!=0)
+    {
+    }
+    xtimer_usleep(1000000);
+    stepper_enable(control->stepper_b);
+    stepper_set_direction(control->stepper_b,STEPPER_FORWARD);
+    while(gpio_read(BTN0_PIN)!=0){
+        stepper_step(control->stepper_b);
+        xtimer_usleep(3000);
+    }
+    stepper_enable(control->stepper_a);
+    stepper_set_direction(control->stepper_a,STEPPER_REVERSE);
+    stepper_set_direction(control->stepper_b,STEPPER_REVERSE);
+    uint32_t homing_b_steps = stepper_get_count(control->stepper_a,1.92);
+    uint32_t homing_a_steps= stepper_get_count(control->stepper_b,0.75);
+    uint32_t homing_combined = homing_b_steps - homing_a_steps;
+    for(uint32_t i = 0; i < homing_combined; i++)
+    {
+        stepper_step(control->stepper_b);
+        xtimer_usleep(3000);
+    }
+    stepper_enable(control->stepper_b);
+    for(uint32_t i = 0; i < homing_a_steps; i++)
+    {
+        stepper_step(control->stepper_b);
+        stepper_step(control->stepper_a);
+        xtimer_usleep(3000);
+    }
+    control->stepper_a->step_count = stepper_get_count(control->stepper_a,GB_MATH_PI/2.0);
+    control->stepper_b->step_count = 0;
+    control->stepper_a->zero_count = 0;
+    control->stepper_b->zero_count = 0;
+    float angle_a;
+    float angle_b;
+    stepper_get_angle(control->stepper_a, &angle_a);
+    stepper_get_angle(control->stepper_b, &angle_b);
+    //printf("angle1: %f, angle2 %f\n", angle1, angle2);
+    printf("Angle A: %f, B: %f\n", angle_a, angle_b);
+    while(gpio_read(BTN0_PIN)!=0)
+    {
+    }
+    xtimer_usleep(500000);
+    //
+    // run stepper_a clockwise
+    // until switch.
+    //
+    // set homing angles
+    // 
+    // set setpoint
+    return 0;
+}
+
+
+int controller_lift_marker(controller_t *control, bool lift){
+    (void)lift;
+    (void)control;
+    return 0;
+}
+    
+

implementation/software/include/controller.h

@@ -0,0 +1,63 @@
+#ifndef CONTROLLER_H
+#define CONTROLLER_H
+
+#include "event.h"
+#include "gb_math.h"
+#include "stepper.h"
+#include "path.h"
+
+
+#define VEL_DIV 1024
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+enum {
+    CONTROLLER_OK = 0,
+    CONTROLLER_ERR = -1,
+    CONTROLLER_NO_PATH = -2,
+    CONTROLLER_STATISFIED = -3,
+};
+
+typedef struct {
+    event_t event;
+    event_timeout_t event_timeout;
+    event_queue_t *queue;
+    stepper_t *stepper_a;
+    stepper_t *stepper_b;
+    float arm_a;
+    float arm_b;
+    float angle1_setpoint;
+    float angle2_setpoint;
+    uint32_t time_step;
+    gbVec2 setpoint;
+    path_t *path;
+    float error;
+} controller_t;
+
+int controller_init(controller_t *control);
+
+void controller_update(event_t *event);
+
+int controller_setpoint(controller_t *control, gbVec2 *point);
+
+void controller_inverse_kinematics(controller_t *control, gbVec2 *point, float *angle1, float *angle2);
+
+void controller_current_position(controller_t *control, gbVec2 *point);
+
+int controller_calculate_velocity(controller_t *control);
+
+int controller_sub_setpoint(controller_t *control, gbVec2 *setpoint, float distance);
+
+int controller_home(controller_t *control);
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif /* CONTROLLER_H */