From 1e259464c54ad3d4b96c99372a02218678300ad3 Mon Sep 17 00:00:00 2001 From: Wouter Horlings Date: Tue, 1 Sep 2020 12:18:09 +0200 Subject: [PATCH] Add controller implementation --- implementation/software/controller.c | 228 +++++++++++++++++++ implementation/software/include/controller.h | 63 +++++ 2 files changed, 291 insertions(+) create mode 100644 implementation/software/controller.c create mode 100644 implementation/software/include/controller.h diff --git a/implementation/software/controller.c b/implementation/software/controller.c new file mode 100644 index 0000000..b2e0ae5 --- /dev/null +++ b/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, ¤t_position); + // print_point(¤t_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; +} + + diff --git a/implementation/software/include/controller.h b/implementation/software/include/controller.h new file mode 100644 index 0000000..4320220 --- /dev/null +++ b/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 */