From 33a8e8ae2d49f4867599755a3b96a2165018d35b Mon Sep 17 00:00:00 2001 From: Thomas Klaehn Date: Tue, 30 Aug 2016 10:07:47 +0200 Subject: [PATCH] engine/drive ctrl functionality splitted/reordered --- source/application/drive_ctrl.c | 225 +++++++++++------------ source/application/engine_ctrl.c | 134 ++++++++++++++ source/application/include/drive_ctrl.h | 16 +- source/application/include/engine_ctrl.h | 22 +++ source/application/include/engines.h | 5 - source/application/shell_commands.c | 97 +--------- 6 files changed, 281 insertions(+), 218 deletions(-) create mode 100644 source/application/engine_ctrl.c create mode 100644 source/application/include/engine_ctrl.h diff --git a/source/application/drive_ctrl.c b/source/application/drive_ctrl.c index 421f18b..2bf6a34 100644 --- a/source/application/drive_ctrl.c +++ b/source/application/drive_ctrl.c @@ -5,149 +5,146 @@ * Author: tkl */ +#include #include +#include +#include +#include +#include "queue.h" +#include "stack.h" +#include "kernel.h" #include "driver.h" -#include "board_devices.h" -#include "engines.h" +#include "engine_ctrl.h" #include "drive_ctrl.h" -struct drive_ctrl { - struct engine_ctrl *left_forward; - struct engine_ctrl *left_backward; - struct engine_ctrl *right_forward; - struct engine_ctrl *right_backward; -}; - -struct drive_ctrl_object { - unsigned int target_speed; - unsigned int turn_speed; - unsigned int current_speed; -}; - -static struct drive_ctrl drive_ctrl = { - .left_forward = &left_forward, - .left_backward = &left_backward, - .right_forward = &right_forward, - .right_backward = &right_backward, -}; - -static struct drive_ctrl_object drive_ctrl_object = { - .target_speed = 80, /* % */ - .turn_speed = 40, - .current_speed = 0, /* % */ -}; - int drive_ctrl_init(void) { - unsigned int duty = 0; - - /* open enable pins */ - drv_open(drive_ctrl.left_forward->enable); - drv_write(drive_ctrl.left_forward->enable, "0", 1); - drv_open(drive_ctrl.left_backward->enable); - drv_write(drive_ctrl.left_backward->enable, "0", 1); - drv_open(drive_ctrl.right_forward->enable); - drv_write(drive_ctrl.right_forward->enable, "0", 1); - drv_open(drive_ctrl.right_backward->enable); - drv_write(drive_ctrl.right_backward->enable, "0", 1); - - /* open pwm's */ - drv_open(drive_ctrl.left_backward->pwm); - drv_ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty); - drv_open(drive_ctrl.left_forward->pwm); - drv_ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty); - drv_open(drive_ctrl.right_backward->pwm); - drv_ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty); - drv_open(drive_ctrl.right_forward->pwm); - drv_ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty); - - /* enable enable pins */ - drv_write(drive_ctrl.left_forward->enable, "1", 1); - drv_write(drive_ctrl.left_backward->enable, "1", 1); - drv_write(drive_ctrl.right_forward->enable, "1", 1); - drv_write(drive_ctrl.right_backward->enable, "1", 1); - - return 0; + return engine_ctrl_init(); } -static unsigned int calc_speed_front(int speed_percent) +int drive_ctrl_turn_right(void) { - unsigned int speed_front = 0; - if(speed_percent > 0) { - if(speed_percent > 100) - speed_front = 100; - else - speed_front = (unsigned int) speed_percent; + drive_ctrl_halt(); + int turn_speed = engine_ctrl_get_turn_speed_value(); + for(int i = 0; i < turn_speed; i++) { + engine_ctrl_set_speed_left(i); + engine_ctrl_set_speed_right(-1 * i); + sleep_ms(10); } - return speed_front; + + return 0; } -static unsigned int calc_speed_back(int speed_percent) +int drive_ctrl_turn_left(void) { - unsigned int speed_back = 0; - if(speed_percent < 0) { - if(speed_percent < -100) - speed_back = 100; - else - speed_back = (unsigned int) abs(speed_percent); + drive_ctrl_halt(); + int turn_speed = engine_ctrl_get_turn_speed_value(); + for(int i = 0; i < turn_speed; i++) { + engine_ctrl_set_speed_right(i); + engine_ctrl_set_speed_left(-1 * i); + sleep_ms(10); } - return speed_back; -} - -int drive_ctrl_set_speed_left(int speed_percent) -{ - unsigned int front_speed = calc_speed_front(speed_percent); - unsigned int back_speed = calc_speed_back(speed_percent); - int ret = drv_ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&front_speed); - ret |= drv_ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&back_speed); - - return ret; -} - -int drive_ctrl_set_speed_right(int speed_percent) -{ - unsigned int front_speed = calc_speed_front(speed_percent); - unsigned int back_speed = calc_speed_back(speed_percent); - int ret = drv_ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&front_speed); - ret |= drv_ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&back_speed); - - return ret; -} - -int drive_ctrl_set_target_speed_value(int speed_percent) -{ - drive_ctrl_object.target_speed = speed_percent; return 0; } -int drive_ctrl_get_target_speed_value(void) +int drive_ctrl_boost(void) { - return drive_ctrl_object.target_speed; -} - -int drive_ctrl_set_current_speed_value(int speed_percent) -{ - drive_ctrl_object.current_speed = speed_percent; + int speed = engine_ctrl_get_target_speed_value(); + speed += 10; + engine_ctrl_set_target_speed_value(speed); + engine_ctrl_set_speed_left(speed); + engine_ctrl_set_speed_right(speed); return 0; } -int drive_ctrl_get_current_speed_value(void) +int drive_ctrl_retard(void) { - return drive_ctrl_object.current_speed; -} - -int drive_ctrl_set_turn_speed_value(int speed_percent) -{ - drive_ctrl_object.turn_speed = speed_percent; + int speed = engine_ctrl_get_target_speed_value(); + speed -= 10; + engine_ctrl_set_target_speed_value(speed); + engine_ctrl_set_speed_left(speed); + engine_ctrl_set_speed_right(speed); return 0; } -int drive_ctrl_get_turn_speed_value(void) +int drive_ctrl_halt(void) { - return drive_ctrl_object.turn_speed; + int current_speed = engine_ctrl_get_current_speed_value(); + if(current_speed > 0) { + for(int i = current_speed; i > 0; i--) { + engine_ctrl_set_current_speed_value(i); + engine_ctrl_set_speed_left(i); + engine_ctrl_set_speed_right(i); + sleep_ms(10); + } + } + else { + for(int i = current_speed; i < 0; i++) { + engine_ctrl_set_current_speed_value(i); + engine_ctrl_set_speed_left(i); + engine_ctrl_set_speed_right(i); + sleep_ms(10); + } + } + + return 0; +} + +int drive_ctrl_forward(void) +{ + int target_speed = engine_ctrl_get_target_speed_value(); + int current_speed = engine_ctrl_get_current_speed_value(); + if(target_speed < 0) + target_speed = abs(target_speed); + engine_ctrl_set_target_speed_value(target_speed); + if(current_speed < target_speed) { + for(int i = current_speed; i < target_speed; i++) { + engine_ctrl_set_current_speed_value(i); + engine_ctrl_set_speed_left(i); + engine_ctrl_set_speed_right(i); + sleep_ms(10); + } + } + else { + for(int i = current_speed; i > target_speed; i--) { + engine_ctrl_set_current_speed_value(i); + engine_ctrl_set_speed_left(i); + engine_ctrl_set_speed_right(i); + sleep_ms(10); + } + } + + return 0; +} + +int drive_ctrl_backward(void) +{ + int target_speed = engine_ctrl_get_target_speed_value(); + int current_speed = engine_ctrl_get_current_speed_value(); + if(target_speed > 0) + target_speed *= -1; + engine_ctrl_set_target_speed_value(target_speed); + if(current_speed > target_speed) { + for(int i = current_speed; i > target_speed; i--) { + engine_ctrl_set_current_speed_value(i); + engine_ctrl_set_speed_left(i); + engine_ctrl_set_speed_right(i); + sleep_ms(10); + } + } + else { + for(int i = current_speed; i < target_speed; i++) { + engine_ctrl_set_current_speed_value(i); + engine_ctrl_set_speed_left(i); + engine_ctrl_set_speed_right(i); + sleep_ms(10); + } + } + + return 0; } diff --git a/source/application/engine_ctrl.c b/source/application/engine_ctrl.c new file mode 100644 index 0000000..bb2f3c7 --- /dev/null +++ b/source/application/engine_ctrl.c @@ -0,0 +1,134 @@ +/* + * engine_ctrl.c + * + * Created on: Aug 30, 2016 + * Author: tkl + */ +#include + +#include "board_devices.h" +#include "driver.h" +#include "engines.h" + +struct drive_ctrl { + struct engine_ctrl *left_forward; + struct engine_ctrl *left_backward; + struct engine_ctrl *right_forward; + struct engine_ctrl *right_backward; +}; + +struct drive_ctrl_object { + unsigned int target_speed; + unsigned int turn_speed; + unsigned int current_speed; +}; + +static struct drive_ctrl drive_ctrl = { + .left_forward = &left_forward, + .left_backward = &left_backward, + .right_forward = &right_forward, + .right_backward = &right_backward, +}; + +static struct drive_ctrl_object drive_ctrl_object = { + .target_speed = 100,/* % */ + .turn_speed = 40, /* % */ + .current_speed = 0, /* % */ +}; + +int engine_ctrl_init(void) +{ + unsigned int duty = 0; + + /* open pwm's */ + drv_open(drive_ctrl.left_backward->pwm); + drv_ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty); + drv_open(drive_ctrl.left_forward->pwm); + drv_ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty); + drv_open(drive_ctrl.right_backward->pwm); + drv_ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty); + drv_open(drive_ctrl.right_forward->pwm); + drv_ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty); + + return 0; +} + +static unsigned int calc_speed_front(int speed_percent) +{ + unsigned int speed_front = 0; + if(speed_percent > 0) { + if(speed_percent > 100) + speed_front = 100; + else + speed_front = (unsigned int) speed_percent; + } + return speed_front; +} + +static unsigned int calc_speed_back(int speed_percent) +{ + unsigned int speed_back = 0; + if(speed_percent < 0) { + if(speed_percent < -100) + speed_back = 100; + else + speed_back = (unsigned int) abs(speed_percent); + } + return speed_back; +} + +int engine_ctrl_set_speed_left(int speed_percent) +{ + unsigned int front_speed = calc_speed_front(speed_percent); + unsigned int back_speed = calc_speed_back(speed_percent); + int ret = drv_ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&front_speed); + ret |= drv_ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&back_speed); + + return ret; +} + +int engine_ctrl_set_speed_right(int speed_percent) +{ + unsigned int front_speed = calc_speed_front(speed_percent); + unsigned int back_speed = calc_speed_back(speed_percent); + int ret = drv_ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&front_speed); + ret |= drv_ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&back_speed); + + return ret; +} + +int engine_ctrl_set_target_speed_value(int speed_percent) +{ + drive_ctrl_object.target_speed = speed_percent; + + return 0; +} + +int engine_ctrl_get_target_speed_value(void) +{ + return drive_ctrl_object.target_speed; +} + +int engine_ctrl_set_current_speed_value(int speed_percent) +{ + drive_ctrl_object.current_speed = speed_percent; + + return 0; +} + +int engine_ctrl_get_current_speed_value(void) +{ + return drive_ctrl_object.current_speed; +} + +int drive_ctrl_set_turn_speed_value(int speed_percent) +{ + drive_ctrl_object.turn_speed = speed_percent; + + return 0; +} + +int engine_ctrl_get_turn_speed_value(void) +{ + return drive_ctrl_object.turn_speed; +} diff --git a/source/application/include/drive_ctrl.h b/source/application/include/drive_ctrl.h index 2672ecc..8edab17 100644 --- a/source/application/include/drive_ctrl.h +++ b/source/application/include/drive_ctrl.h @@ -10,14 +10,12 @@ int drive_ctrl_init(void); -int drive_ctrl_set_speed_left(int speed_percent); -int drive_ctrl_set_speed_right(int speed_percent); -int drive_ctrl_set_target_speed_value(int speed_percent); -int drive_ctrl_get_target_speed_value(void); -int drive_ctrl_set_current_speed_value(int speed_percent); -int drive_ctrl_get_current_speed_value(void); -int drive_ctrl_set_turn_speed_value(int speed_percent); -int drive_ctrl_get_turn_speed_value(void); - +int drive_ctrl_turn_right(void); +int drive_ctrl_turn_left(void); +int drive_ctrl_boost(void); +int drive_ctrl_retard(void); +int drive_ctrl_halt(void); +int drive_ctrl_forward(void); +int drive_ctrl_backward(void); #endif /* SOURCE_APPLICATION_INCLUDE_DRIVE_CTRL_H_ */ diff --git a/source/application/include/engine_ctrl.h b/source/application/include/engine_ctrl.h new file mode 100644 index 0000000..245451f --- /dev/null +++ b/source/application/include/engine_ctrl.h @@ -0,0 +1,22 @@ +/* + * engine_ctrl.h + * + * Created on: Aug 30, 2016 + * Author: tkl + */ + +#ifndef SOURCE_APPLICATION_INCLUDE_ENGINE_CTRL_H_ +#define SOURCE_APPLICATION_INCLUDE_ENGINE_CTRL_H_ + +int engine_ctrl_init(void); + +int engine_ctrl_set_speed_left(int speed_percent); +int engine_ctrl_set_speed_right(int speed_percent); +int engine_ctrl_set_target_speed_value(int speed_percent); +int engine_ctrl_get_target_speed_value(void); +int engine_ctrl_set_current_speed_value(int speed_percent); +int engine_ctrl_get_current_speed_value(void); +int engine_ctrl_set_turn_speed_value(int speed_percent); +int engine_ctrl_get_turn_speed_value(void); + +#endif /* SOURCE_APPLICATION_INCLUDE_ENGINE_CTRL_H_ */ diff --git a/source/application/include/engines.h b/source/application/include/engines.h index 6a955ed..cc6d7ed 100644 --- a/source/application/include/engines.h +++ b/source/application/include/engines.h @@ -10,27 +10,22 @@ struct engine_ctrl { const struct driver *pwm; - const struct driver *enable; }; static struct engine_ctrl left_forward = { .pwm = &pwm_4, - .enable = &gpio_c2, }; static struct engine_ctrl right_backward = { .pwm = &pwm_3, - .enable = &gpio_c3, }; static struct engine_ctrl right_forward = { .pwm = &pwm_2, - .enable = &gpio_c0, }; static struct engine_ctrl left_backward = { .pwm = &pwm_1, - .enable = &gpio_c1, }; #endif /* SOURCE_APPLICATION_INCLUDE_ENGINES_H_ */ diff --git a/source/application/shell_commands.c b/source/application/shell_commands.c index 1d02284..b8100ba 100644 --- a/source/application/shell_commands.c +++ b/source/application/shell_commands.c @@ -92,126 +92,43 @@ int shell_commands_init(void) static void *drive_turn_right_cb(const char *param) { - drive_halt_cb(NULL); - int turn_speed = drive_ctrl_get_turn_speed_value(); - for(int i = 0; i < turn_speed; i++) { - drive_ctrl_set_speed_left(i); - drive_ctrl_set_speed_right(-1 * i); - sleep_ms(10); - } - + drive_ctrl_turn_right(); return NULL; } static void *drive_turn_left_cb(const char *param) { - drive_halt_cb(NULL); - int turn_speed = drive_ctrl_get_turn_speed_value(); - for(int i = 0; i < turn_speed; i++) { - drive_ctrl_set_speed_right(i); - drive_ctrl_set_speed_left(-1 * i); - sleep_ms(10); - } - + drive_ctrl_turn_left(); return NULL; } static void *drive_boost_cb(const char *param) { - int speed = drive_ctrl_get_target_speed_value(); - speed += 10; - drive_ctrl_set_target_speed_value(speed); - drive_ctrl_set_speed_left(speed); - drive_ctrl_set_speed_right(speed); - + drive_ctrl_boost(); return NULL; } static void *drive_retard_cb(const char *param) { - int speed = drive_ctrl_get_target_speed_value(); - speed -= 10; - drive_ctrl_set_target_speed_value(speed); - drive_ctrl_set_speed_left(speed); - drive_ctrl_set_speed_right(speed); - + drive_ctrl_retard(); return NULL; } static void *drive_halt_cb(const char *param) { - int current_speed = drive_ctrl_get_current_speed_value(); - if(current_speed > 0) { - for(int i = current_speed; i > 0; i--) { - drive_ctrl_set_current_speed_value(i); - drive_ctrl_set_speed_left(i); - drive_ctrl_set_speed_right(i); - sleep_ms(10); - } - } - else { - for(int i = current_speed; i < 0; i++) { - drive_ctrl_set_current_speed_value(i); - drive_ctrl_set_speed_left(i); - drive_ctrl_set_speed_right(i); - sleep_ms(10); - } - } - + drive_ctrl_halt(); return NULL; } static void *drive_forward_cb(const char *param) { - int target_speed = drive_ctrl_get_target_speed_value(); - int current_speed = drive_ctrl_get_current_speed_value(); - if(target_speed < 0) - target_speed = abs(target_speed); - drive_ctrl_set_target_speed_value(target_speed); - if(current_speed < target_speed) { - for(int i = current_speed; i < target_speed; i++) { - drive_ctrl_set_current_speed_value(i); - drive_ctrl_set_speed_left(i); - drive_ctrl_set_speed_right(i); - sleep_ms(10); - } - } - else { - for(int i = current_speed; i > target_speed; i--) { - drive_ctrl_set_current_speed_value(i); - drive_ctrl_set_speed_left(i); - drive_ctrl_set_speed_right(i); - sleep_ms(10); - } - } - + drive_ctrl_forward(); return NULL; } static void *drive_backward_cb(const char *param) { - int target_speed = drive_ctrl_get_target_speed_value(); - int current_speed = drive_ctrl_get_current_speed_value(); - if(target_speed > 0) - target_speed *= -1; - drive_ctrl_set_target_speed_value(target_speed); - if(current_speed > target_speed) { - for(int i = current_speed; i > target_speed; i--) { - drive_ctrl_set_current_speed_value(i); - drive_ctrl_set_speed_left(i); - drive_ctrl_set_speed_right(i); - sleep_ms(10); - } - } - else { - for(int i = current_speed; i < target_speed; i++) { - drive_ctrl_set_current_speed_value(i); - drive_ctrl_set_speed_left(i); - drive_ctrl_set_speed_right(i); - sleep_ms(10); - } - } - + drive_ctrl_backward(); return NULL; }