diff --git a/source/application/collision_ctrl.c b/source/application/collision_ctrl.c new file mode 100644 index 0000000..4b878d4 --- /dev/null +++ b/source/application/collision_ctrl.c @@ -0,0 +1,90 @@ +/* + * collision_ctrl.c + * + * Created on: Aug 30, 2016 + * Author: tkl + */ + +#include +#include +#include +#include +#include +#include + +#include "board_devices.h" +#include "queue.h" +#include "list.h" +#include "stack.h" +#include "kernel.h" +#include "driver.h" +#include "shell.h" +#include "drive_ctrl.h" +#include "engine_ctrl.h" + +#pragma pack(push) +#pragma pack(1) +struct collision_ctrl_object { + int distance_mm; +}; +#pragma pack(pop) + +struct collision_ctrl_object collision_ctrl_object; +struct thread_context collision_ctrl_ctx; +stack_t collision_ctrl_stack[256]; + +static void collision_ctrl_loop(void *arg) +{ + drv_open(&pwm5_c2); /* trigger pwm */ + drv_open(&pwm2_c4); /* read pwm */ + while(1) { + int pulse_ns = drv_ioctl(&pwm2_c4, IOCTL_PWM_GET_PULSE_WIDTH_NS, NULL); + collision_ctrl_object.distance_mm = 343 * pulse_ns / 1000 / 2; + if(collision_ctrl_object.distance_mm < 300) { + char print_buffer[50]; + if(engine_ctrl_get_current_speed_value() != 0) { + drive_ctrl_halt(); + sprintf(print_buffer, "collision detected (%dmm)...\r\n", collision_ctrl_object.distance_mm); + shell_write(print_buffer, strlen(print_buffer)); + sprintf(print_buffer, "going back...\r\n"); + shell_write(print_buffer, strlen(print_buffer)); + drive_ctrl_backward(); + sleep_ms(3000); /* todo: randomize */ + drive_ctrl_halt(); + + sprintf(print_buffer, "start turning...\r\n"); + shell_write(print_buffer, strlen(print_buffer)); + drive_ctrl_turn_left(); + sleep_ms(3000); + drive_ctrl_halt(); + + sprintf(print_buffer, "restart forward...\r\n"); + shell_write(print_buffer, strlen(print_buffer)); + drive_ctrl_forward(); + } + else { + sprintf(print_buffer, "collision detected while not driving 8(\r\n"); + shell_write(print_buffer, strlen(print_buffer)); + } + } + sleep_ms(1); + } +} + +int collision_ctrl_init(void) +{ + thread_create( + &collision_ctrl_ctx, + collision_ctrl_stack, + sizeof(collision_ctrl_stack) / sizeof(stack_t), + collision_ctrl_loop, + NULL, + THREAD_PRIO_LOW); + + return 0; +} + +int collision_ctrl_get_distance_mm(void) +{ + return collision_ctrl_object.distance_mm; +} diff --git a/source/application/include/collision_ctrl.h b/source/application/include/collision_ctrl.h new file mode 100644 index 0000000..9275593 --- /dev/null +++ b/source/application/include/collision_ctrl.h @@ -0,0 +1,14 @@ +/* + * collision_ctrl.h + * + * Created on: Aug 30, 2016 + * Author: tkl + */ + +#ifndef SOURCE_APPLICATION_INCLUDE_COLLISION_CTRL_H_ +#define SOURCE_APPLICATION_INCLUDE_COLLISION_CTRL_H_ + +int collision_ctrl_init(void); +int collision_ctrl_get_distance_mm(void); + +#endif /* SOURCE_APPLICATION_INCLUDE_COLLISION_CTRL_H_ */ diff --git a/source/application/include/sensor_data.h b/source/application/include/sensor_data.h new file mode 100644 index 0000000..ed4e245 --- /dev/null +++ b/source/application/include/sensor_data.h @@ -0,0 +1,14 @@ +/* + * sensor_data.h + * + * Created on: Aug 30, 2016 + * Author: tkl + */ + +#ifndef SOURCE_APPLICATION_INCLUDE_SENSOR_DATA_H_ +#define SOURCE_APPLICATION_INCLUDE_SENSOR_DATA_H_ + +int sensor_data_init(void); +int sensor_data_sys_msg(bool state); + +#endif /* SOURCE_APPLICATION_INCLUDE_SENSOR_DATA_H_ */ diff --git a/source/application/main.c b/source/application/main.c index 4544360..cd70b1c 100644 --- a/source/application/main.c +++ b/source/application/main.c @@ -10,12 +10,16 @@ #include "shell.h" #include "shell_commands.h" #include "drive_ctrl.h" +#include "collision_ctrl.h" +#include "sensor_data.h" int main(void) { - drive_ctrl_init(); shell_init(&uart_1); shell_commands_init(); + drive_ctrl_init(); + collision_ctrl_init(); + sensor_data_init(); while(1) { sleep_ms(1000); diff --git a/source/application/sensor_data.c b/source/application/sensor_data.c new file mode 100644 index 0000000..7b72a50 --- /dev/null +++ b/source/application/sensor_data.c @@ -0,0 +1,65 @@ +/* + * sensor_data.c + * + * Created on: Aug 30, 2016 + * Author: tkl + */ + +#include +#include +#include +#include +#include +#include + +#include "driver.h" +#include "queue.h" +#include "list.h" +#include "stack.h" +#include "kernel.h" +#include "shell.h" +#include "collision_ctrl.h" + +struct sensor_data_object { + bool sys_msg_on; +}; + +struct sensor_data_object sensor_data_object = { + .sys_msg_on = false, +}; + +struct thread_context sensor_data_ctx; +stack_t sensor_data_stack[256]; + +static void sensor_data_loop(void *arg) +{ + char print_buffer[50]; + while(1) { + int collision_distance = collision_ctrl_get_distance_mm(); + if(sensor_data_object.sys_msg_on) { + sprintf(print_buffer, "#SYS:%d\r\n", collision_distance); + shell_write(print_buffer, strlen(print_buffer)); + } + sleep_ms(1000); + } +} + +int sensor_data_init(void) +{ + thread_create( + &sensor_data_ctx, + sensor_data_stack, + sizeof(sensor_data_stack) / sizeof(stack_t), + sensor_data_loop, + NULL, + THREAD_PRIO_LOW); + + return 0; +} + +int sensor_data_sys_msg(bool state) +{ + sensor_data_object.sys_msg_on = state; + + return 0; +} diff --git a/source/application/shell_commands.c b/source/application/shell_commands.c index b8100ba..fb54ef4 100644 --- a/source/application/shell_commands.c +++ b/source/application/shell_commands.c @@ -18,6 +18,7 @@ #include "kernel.h" #include "drive_ctrl.h" #include "version.h" +#include "sensor_data.h" static void *drive_turn_right_cb(const char *param); static void *drive_turn_left_cb(const char *param); @@ -27,6 +28,20 @@ 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 void *app_version(const char *param); +static void *sys_msg_on_cb(const char *param); +static void *sys_msg_off_cb(const char *param); + +static struct command cmd_sys_msg_on = { + .command = "sys on", + .description = "Switch system message on.", + .command_callback = sys_msg_on_cb, +}; + +static struct command cmd_sys_msg_off = { + .command = "sys off", + .description = "Switch system message off.", + .command_callback = sys_msg_off_cb, +}; static struct command cmd_app_version = { .command = "version", @@ -86,6 +101,8 @@ int shell_commands_init(void) shell_add_command(&drive_halt); shell_add_command(&drive_turn_left); shell_add_command(&drive_turn_right); + shell_add_command(&cmd_sys_msg_off); + shell_add_command(&cmd_sys_msg_on); return 0; } @@ -146,3 +163,14 @@ static void *app_version(const char *param) return NULL; } + +static void *sys_msg_on_cb(const char *param) +{ + sensor_data_sys_msg(true); + return NULL; +} +static void *sys_msg_off_cb(const char *param) +{ + sensor_data_sys_msg(false); + return NULL; +}