From 8270eca957affc61f214cf3041e5fc117192b88e Mon Sep 17 00:00:00 2001 From: tkl Date: Thu, 11 Aug 2016 14:06:45 +0200 Subject: [PATCH] basic drive module and shell interface --- source/application/drive_ctrl.c | 125 +++++++++++++++++ source/application/include/drive_ctrl.h | 19 +++ source/application/include/engines.h | 39 ++++++ source/application/include/shell_commands.h | 13 ++ source/application/main.c | 50 ++----- source/application/shell_commands.c | 148 ++++++++++++++++++++ 6 files changed, 352 insertions(+), 42 deletions(-) create mode 100644 source/application/drive_ctrl.c create mode 100644 source/application/include/drive_ctrl.h create mode 100644 source/application/include/engines.h create mode 100644 source/application/include/shell_commands.h create mode 100644 source/application/shell_commands.c diff --git a/source/application/drive_ctrl.c b/source/application/drive_ctrl.c new file mode 100644 index 0000000..7c951dc --- /dev/null +++ b/source/application/drive_ctrl.c @@ -0,0 +1,125 @@ +/* + * drive_ctrl.c + * + * Created on: Aug 10, 2016 + * Author: tkl + */ + +#include + +#include "driver.h" +#include "board_devices.h" +#include "engines.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 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 = { + .current_speed = 50, /* % */ +}; + +int drive_ctrl_init(void) +{ + unsigned int duty = 0; + + /* open enable pins */ + open(drive_ctrl.left_forward->enable); + write(drive_ctrl.left_forward->enable, "0", 1); + open(drive_ctrl.left_backward->enable); + write(drive_ctrl.left_backward->enable, "0", 1); + open(drive_ctrl.right_forward->enable); + write(drive_ctrl.right_forward->enable, "0", 1); + open(drive_ctrl.right_backward->enable); + write(drive_ctrl.right_backward->enable, "0", 1); + + /* open pwm's */ + open(drive_ctrl.left_backward->pwm); + ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty); + open(drive_ctrl.left_forward->pwm); + ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty); + open(drive_ctrl.right_backward->pwm); + ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty); + open(drive_ctrl.right_forward->pwm); + ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty); + + /* enable enable pins */ + write(drive_ctrl.left_forward->enable, "1", 1); + write(drive_ctrl.left_backward->enable, "1", 1); + write(drive_ctrl.right_forward->enable, "1", 1); + write(drive_ctrl.right_backward->enable, "1", 1); + + 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 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 = ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&back_speed); + ret |= ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&front_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 = ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&back_speed); + ret |= ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&front_speed); + + return ret; +} + +int drive_ctrl_set_speed_value(int speed_percent) +{ + drive_ctrl_object.current_speed = speed_percent; + + return 0; +} + +int drive_ctrl_get_speed_value(void) +{ + return drive_ctrl_object.current_speed; +} diff --git a/source/application/include/drive_ctrl.h b/source/application/include/drive_ctrl.h new file mode 100644 index 0000000..f499a5b --- /dev/null +++ b/source/application/include/drive_ctrl.h @@ -0,0 +1,19 @@ +/* + * drive_ctrl.h + * + * Created on: Aug 10, 2016 + * Author: tkl + */ + +#ifndef SOURCE_APPLICATION_INCLUDE_DRIVE_CTRL_H_ +#define SOURCE_APPLICATION_INCLUDE_DRIVE_CTRL_H_ + +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_speed_value(int speed_percent); +int drive_ctrl_get_speed_value(void); + + +#endif /* SOURCE_APPLICATION_INCLUDE_DRIVE_CTRL_H_ */ diff --git a/source/application/include/engines.h b/source/application/include/engines.h new file mode 100644 index 0000000..5f9dd20 --- /dev/null +++ b/source/application/include/engines.h @@ -0,0 +1,39 @@ +/* + * engines.h + * + * Created on: Aug 10, 2016 + * Author: tkl + */ + +#ifndef SOURCE_APPLICATION_INCLUDE_ENGINES_H_ +#define SOURCE_APPLICATION_INCLUDE_ENGINES_H_ + +struct engine_ctrl { + const struct driver *pwm; + const struct driver *enable; +}; + +static struct engine_ctrl right_forward = { + .pwm = &pwm_1, + .enable = &gpio_c1, +}; + +static struct engine_ctrl right_backward = { + .pwm = &pwm_2, + .enable = &gpio_c0, +}; + +static struct engine_ctrl left_forward = { + .pwm = &pwm_3, + .enable = &gpio_c3, +}; + +static struct engine_ctrl left_backward = { + .pwm = &pwm_4, + .enable = &gpio_c2, +}; + + + + +#endif /* SOURCE_APPLICATION_INCLUDE_ENGINES_H_ */ diff --git a/source/application/include/shell_commands.h b/source/application/include/shell_commands.h new file mode 100644 index 0000000..4ba9527 --- /dev/null +++ b/source/application/include/shell_commands.h @@ -0,0 +1,13 @@ +/* + * shell_commands.h + * + * Created on: Aug 10, 2016 + * Author: tkl + */ + +#ifndef SOURCE_APPLICATION_SHELL_COMMANDS_H_ +#define SOURCE_APPLICATION_SHELL_COMMANDS_H_ + +int shell_commands_init(void); + +#endif /* SOURCE_APPLICATION_SHELL_COMMANDS_H_ */ diff --git a/source/application/main.c b/source/application/main.c index 7b3fe36..fc7f263 100644 --- a/source/application/main.c +++ b/source/application/main.c @@ -1,55 +1,21 @@ -#include #include -#include -#include -#include +#include -#include "driver.h" #include "board_devices.h" #include "stack.h" #include "queue.h" #include "kernel.h" #include "driver.h" +#include "list.h" +#include "shell.h" +#include "shell_commands.h" +#include "drive_ctrl.h" -#define STACK_SIZE 256 -stack_t tc_1_stack[STACK_SIZE]; -struct thread_context tc_1; - -void task1(void *arg) -{ - char rd = '0'; - open(&led_4); - write(&led_4, &rd, 1); - while(1) { - sleep_ms(1000); - read(&led_4, &rd, 1); - if(rd == '0') - rd = '1'; - else - rd = '0'; - write(&led_4, &rd, 1); - write(&uart_1, "Driver test\r\n", 13); - } -} - -char rx_buf[80]; -void task2(void *arg) -{ - while(1) { - int i = read(&uart_1, rx_buf, 80); - if(i > 0) { - write(&uart_1, rx_buf, i); - } - } -} - -stack_t tc_2_stack[STACK_SIZE]; -struct thread_context tc_2; int main(void) { - open(&uart_1); - thread_create(&tc_1, tc_1_stack, STACK_SIZE, task1, NULL, THREAD_PRIO_LOW); - thread_create(&tc_2, tc_2_stack, STACK_SIZE, task2, NULL, THREAD_PRIO_LOW); + drive_ctrl_init(); + shell_init(&uart_1); + shell_commands_init(); schedule_start(); diff --git a/source/application/shell_commands.c b/source/application/shell_commands.c new file mode 100644 index 0000000..9e671e8 --- /dev/null +++ b/source/application/shell_commands.c @@ -0,0 +1,148 @@ +/* + * shell_commands.c + * + * Created on: Aug 10, 2016 + * Author: tkl + */ +#include +#include + +#include "list.h" +#include "driver.h" +#include "shell.h" +#include "drive_ctrl.h" + +static void *drive_turn_right_cb(const char *param); +static void *drive_turn_left_cb(const char *param); +static void *drive_boost_cb(const char *param); +static void *drive_retard_cb(const char *param); +static void *drive_halt_cb(const char *param); +static void *drive_forward_cb(const char *param); +static void *drive_backward_cb(const char *param); + +static struct command drive_forward = { + .command = "f", + .description = "Start driving forward with preset speed.", + .command_callback = drive_forward_cb, +}; + +static struct command drive_backward = { + .command = "b", + .description = "Start driving backward with preset speed.", + .command_callback = drive_backward_cb, +}; + +static struct command drive_halt = { + .command = "h", + .description = "Stop driving.", + .command_callback = drive_halt_cb, +}; + +static struct command drive_boost = { + .command = "+", + .description = "Boost speed.", + .command_callback = drive_boost_cb, +}; + +static struct command drive_retard = { + .command = "-", + .description = "Retard speed", + .command_callback = drive_retard_cb, +}; + +static struct command drive_turn_left = { + .command = "l", + .description = "Turn left.", + .command_callback = drive_turn_left_cb, +}; + +static struct command drive_turn_right = { + .command = "r", + .description = "Turn right", + .command_callback = drive_turn_right_cb, +}; + +int shell_commands_init(void) +{ + shell_add_command(&drive_forward); + shell_add_command(&drive_backward); + shell_add_command(&drive_boost); + shell_add_command(&drive_retard); + shell_add_command(&drive_halt); + shell_add_command(&drive_turn_left); + shell_add_command(&drive_turn_right); + + return 0; +} + +static void *drive_turn_right_cb(const char *param) +{ + int speed = drive_ctrl_get_speed_value(); + drive_ctrl_set_speed_left(speed); + drive_ctrl_set_speed_right(0); + + return NULL; +} + +static void *drive_turn_left_cb(const char *param) +{ + int speed = drive_ctrl_get_speed_value(); + drive_ctrl_set_speed_left(0); + drive_ctrl_set_speed_right(speed); + + return NULL; +} + +static void *drive_boost_cb(const char *param) +{ + int speed = drive_ctrl_get_speed_value(); + speed += 10; + drive_ctrl_set_speed_value(speed); + drive_ctrl_set_speed_left(speed); + drive_ctrl_set_speed_right(speed); + + return NULL; +} + +static void *drive_retard_cb(const char *param) +{ + int speed = drive_ctrl_get_speed_value(); + speed -= 10; + drive_ctrl_set_speed_value(speed); + drive_ctrl_set_speed_left(speed); + drive_ctrl_set_speed_right(speed); + + return NULL; +} + +static void *drive_halt_cb(const char *param) +{ + drive_ctrl_set_speed_left(0); + drive_ctrl_set_speed_right(0); + + return NULL; +} + +static void *drive_forward_cb(const char *param) +{ + int speed = drive_ctrl_get_speed_value(); + if(speed < 0) + speed = abs(speed); + drive_ctrl_set_speed_value(speed); + drive_ctrl_set_speed_left(speed); + drive_ctrl_set_speed_right(speed); + + return NULL; +} + +static void *drive_backward_cb(const char *param) +{ + int speed = drive_ctrl_get_speed_value(); + if(speed > 0) + speed *= -1; + drive_ctrl_set_speed_value(speed); + drive_ctrl_set_speed_left(speed); + drive_ctrl_set_speed_right(speed); + + return NULL; +}