diff --git a/source/application/collision_ctrl.c b/source/application/collision_ctrl.c index e3a54a1..4948175 100644 --- a/source/application/collision_ctrl.c +++ b/source/application/collision_ctrl.c @@ -41,33 +41,6 @@ static void collision_ctrl_loop(void *arg) 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(500); - drive_ctrl_halt(); - - sprintf(print_buffer, "start turning...\r\n"); - shell_write(print_buffer, strlen(print_buffer)); - drive_ctrl_turn_left(); - sleep_ms(drv_read(&rng, NULL, 0) % 3500 + 500); // rand between 500 and 4000 - 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); } } @@ -89,3 +62,10 @@ int collision_ctrl_get_distance_mm(void) { return collision_ctrl_object.distance_mm; } + +bool collision_ctrl_is_collision(void) +{ + if(collision_ctrl_object.distance_mm < 400) + return true; + return false; +} diff --git a/source/application/include/collision_ctrl.h b/source/application/include/collision_ctrl.h index 9275593..f05f268 100644 --- a/source/application/include/collision_ctrl.h +++ b/source/application/include/collision_ctrl.h @@ -10,5 +10,6 @@ int collision_ctrl_init(void); int collision_ctrl_get_distance_mm(void); +bool collision_ctrl_is_collision(void); #endif /* SOURCE_APPLICATION_INCLUDE_COLLISION_CTRL_H_ */ diff --git a/source/application/include/system_state.h b/source/application/include/system_state.h new file mode 100644 index 0000000..379f624 --- /dev/null +++ b/source/application/include/system_state.h @@ -0,0 +1,26 @@ +/* + * system_state.h + * + * Created on: Sep 19, 2016 + * Author: tkl + */ + +#ifndef SOURCE_APPLICATION_INCLUDE_SYSTEM_STATE_H_ +#define SOURCE_APPLICATION_INCLUDE_SYSTEM_STATE_H_ + +enum system_state { + SYS_INIT = 0, + SYS_IDLE, + SYS_DRIVING, + SYS_COLLISION_DETECTED, + SYS_COLLISION_DRIVE_BACK, + SYS_COLLISION_TURN, + SYS_MAX +}; + +int system_state_init(void); +int system_state_poll(void); +int system_state_force(enum system_state state); + + +#endif /* SOURCE_APPLICATION_INCLUDE_SYSTEM_STATE_H_ */ diff --git a/source/application/main.c b/source/application/main.c index cd70b1c..5204f2a 100644 --- a/source/application/main.c +++ b/source/application/main.c @@ -12,6 +12,7 @@ #include "drive_ctrl.h" #include "collision_ctrl.h" #include "sensor_data.h" +#include "system_state.h" int main(void) { @@ -20,9 +21,11 @@ int main(void) drive_ctrl_init(); collision_ctrl_init(); sensor_data_init(); + system_state_init(); while(1) { - sleep_ms(1000); + system_state_poll(); + sleep_ms(0); } return 0; diff --git a/source/application/shell_commands.c b/source/application/shell_commands.c index fb54ef4..c584da2 100644 --- a/source/application/shell_commands.c +++ b/source/application/shell_commands.c @@ -16,17 +16,12 @@ #include "queue.h" #include "stack.h" #include "kernel.h" -#include "drive_ctrl.h" #include "version.h" #include "sensor_data.h" +#include "system_state.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 void *app_version(const char *param); static void *sys_msg_on_cb(const char *param); static void *sys_msg_off_cb(const char *param); @@ -50,102 +45,37 @@ static struct command cmd_app_version = { }; static struct command drive_forward = { - .command = "f", - .description = "Start driving forward with preset speed.", + .command = "d", + .description = "Start driving .", .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(&cmd_app_version); 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); shell_add_command(&cmd_sys_msg_off); shell_add_command(&cmd_sys_msg_on); return 0; } -static void *drive_turn_right_cb(const char *param) -{ - drive_ctrl_turn_right(); - return NULL; -} - -static void *drive_turn_left_cb(const char *param) -{ - drive_ctrl_turn_left(); - return NULL; -} - -static void *drive_boost_cb(const char *param) -{ - drive_ctrl_boost(); - return NULL; -} - -static void *drive_retard_cb(const char *param) -{ - drive_ctrl_retard(); - return NULL; -} - static void *drive_halt_cb(const char *param) { - drive_ctrl_halt(); + system_state_force(SYS_IDLE); return NULL; } static void *drive_forward_cb(const char *param) { - drive_ctrl_forward(); - return NULL; -} - -static void *drive_backward_cb(const char *param) -{ - drive_ctrl_backward(); + system_state_force(SYS_DRIVING); return NULL; } diff --git a/source/application/system_state.c b/source/application/system_state.c new file mode 100644 index 0000000..c26e733 --- /dev/null +++ b/source/application/system_state.c @@ -0,0 +1,222 @@ +/* + * system_state.c + * + * Created on: Sep 19, 2016 + * Author: tkl + */ + +#include +#include +#include + +#include "collision_ctrl.h" +#include "drive_ctrl.h" + +#include "driver.h" +#include "stack.h" +#include "queue.h" +#include "list.h" +#include "shell.h" +#include "kernel.h" + +#include "board_devices.h" + +#include "system_state.h" + +#pragma pack(push) +#pragma pack(1) +struct collision_timer { + bool running; + unsigned long tick; +}; +#pragma pack(pop) + +struct system_state_object { + enum system_state system_state; + enum system_state last_state; + enum system_state saved_state; + enum system_state force_state; + struct collision_timer collision_timer; +}; + +static struct system_state_object system_state_object = { + .system_state = SYS_INIT, + .last_state = SYS_MAX, + .saved_state = SYS_MAX, + .force_state = SYS_MAX, + .collision_timer.running = false, + .collision_timer.tick = 0, +}; + +int system_state_init(void) +{ + return 0; +} + +static bool is_transition(void) +{ + return (system_state_object.system_state != system_state_object.last_state) ? true : false; +} + +static enum system_state handle_state_init(void) +{ + enum system_state next_state = SYS_INIT; + if(is_transition()) { + system_state_object.last_state = system_state_object.system_state; + char print_buffer[] = "#SYS_ST: INIT\r\n"; + shell_write(print_buffer, strlen(print_buffer)); + drive_ctrl_halt(); + } + // TODO: check various sensor states e.g. battery, position state etc... + next_state = SYS_IDLE; + return next_state; +} + +static enum system_state handle_state_idle(void) +{ + enum system_state next_state = SYS_IDLE; + if(is_transition()) { + system_state_object.last_state = system_state_object.system_state; + char print_buffer[] = "#SYS_ST: IDLE\r\n"; + shell_write(print_buffer, strlen(print_buffer)); + drive_ctrl_halt(); + } + if(system_state_object.force_state == SYS_DRIVING) { + next_state = SYS_DRIVING; + system_state_object.force_state = SYS_MAX; + } + return next_state; +} + +static enum system_state handle_state_driving(void) +{ + enum system_state next_state = SYS_DRIVING; + if(is_transition()) { + system_state_object.last_state = system_state_object.system_state; + char print_buffer[] = "#SYS_ST: DRIVING\r\n"; + shell_write(print_buffer, strlen(print_buffer)); + drive_ctrl_forward(); + } + if(collision_ctrl_is_collision()) { + next_state = SYS_COLLISION_DETECTED; + system_state_object.saved_state = system_state_object.system_state; + } + if(system_state_object.force_state == SYS_IDLE) { + next_state = SYS_IDLE; + system_state_object.force_state = SYS_MAX; + } + + return next_state; +} + +static enum system_state handle_state_collision_detected(void) +{ + enum system_state next_state = SYS_COLLISION_DETECTED; + if(is_transition()) { + system_state_object.last_state = system_state_object.system_state; + drive_ctrl_halt(); + char print_buffer[] = "#SYS_ST: COLLISION_DETECTED\r\n"; + shell_write(print_buffer, strlen(print_buffer)); + } + next_state = SYS_COLLISION_DRIVE_BACK; + if(system_state_object.force_state == SYS_IDLE) { + next_state = SYS_IDLE; + system_state_object.force_state = SYS_MAX; + } + return next_state; +} + +static enum system_state handle_state_collision_drive_back(void) +{ + enum system_state next_state = SYS_COLLISION_DRIVE_BACK; + if(is_transition()) { + system_state_object.last_state = system_state_object.system_state; + char print_buffer[] = "#SYS_ST: COLLISION_DRIVE_BACK\r\n"; + shell_write(print_buffer, strlen(print_buffer)); + system_state_object.collision_timer.tick = sys_tick_get_ms() + 500; + system_state_object.collision_timer.running = true; + } + if(system_state_object.collision_timer.running && + (system_state_object.collision_timer.tick < sys_tick_get_ms())) { + drive_ctrl_halt(); + next_state = SYS_COLLISION_TURN; + system_state_object.collision_timer.running = false; + } + if(system_state_object.force_state == SYS_IDLE) { + next_state = SYS_IDLE; + system_state_object.force_state = SYS_MAX; + system_state_object.collision_timer.running = false; + } + return next_state; +} + +static enum system_state handle_state_collision_turn(void) +{ + enum system_state next_state = SYS_COLLISION_TURN; + if(is_transition()) { + system_state_object.last_state = system_state_object.system_state; + char print_buffer[] = "#SYS_ST: COLLISION_TURN\r\n"; + shell_write(print_buffer, strlen(print_buffer)); + drv_open(&rng); + // random number between 500 and 4000 + unsigned int sleep = (unsigned int)drv_read(&rng, NULL, 0) % 3500 + 500; + drv_close(&rng); + system_state_object.collision_timer.tick = sys_tick_get_ms() + sleep; + system_state_object.collision_timer.running = true; + // randomize turn direction + if(sleep % 2) + drive_ctrl_turn_left(); + else + drive_ctrl_turn_right(); + } + if(system_state_object.collision_timer.running && + (system_state_object.collision_timer.tick < sys_tick_get_ms())) { + drive_ctrl_halt(); + next_state = system_state_object.saved_state; + system_state_object.collision_timer.running = false; + } + if(system_state_object.force_state == SYS_IDLE) { + next_state = SYS_IDLE; + system_state_object.force_state = SYS_MAX; + system_state_object.collision_timer.running = false; + } + return next_state; +} + +int system_state_poll(void) +{ + enum system_state next_state = SYS_MAX; + switch(system_state_object.system_state) { + case SYS_INIT: + next_state = handle_state_init(); + break; + case SYS_IDLE: + next_state = handle_state_idle(); + break; + case SYS_DRIVING: + next_state = handle_state_driving(); + break; + case SYS_COLLISION_DETECTED: + next_state = handle_state_collision_detected(); + break; + case SYS_COLLISION_DRIVE_BACK: + next_state = handle_state_collision_drive_back(); + break; + case SYS_COLLISION_TURN: + next_state = handle_state_collision_turn(); + break; + case SYS_MAX: + break; + } + if(next_state < SYS_MAX) { + system_state_object.system_state = next_state; + return 0; + } + return -1; +} + +int system_state_force(enum system_state state) +{ + system_state_object.force_state = state; + return 0; +} diff --git a/source/os/release/include/kernel.h b/source/os/release/include/kernel.h index e69f607..ec33802 100644 --- a/source/os/release/include/kernel.h +++ b/source/os/release/include/kernel.h @@ -60,5 +60,6 @@ struct thread_context *thread_create( void thread_exit(void); void sleep_ms(unsigned int ms); +unsigned long sys_tick_get_ms(void); #endif /* SOURCE_FIRMWARE_KERNEL_INTERFACE_KERNEL_H_ */ diff --git a/source/os/release/libkosmos-arm-stm32f4-discovery-0.1.9.276.a b/source/os/release/libkosmos-arm-stm32f4-discovery-0.1.10.287.a similarity index 92% rename from source/os/release/libkosmos-arm-stm32f4-discovery-0.1.9.276.a rename to source/os/release/libkosmos-arm-stm32f4-discovery-0.1.10.287.a index 05fd6e1..5fc8b54 100644 Binary files a/source/os/release/libkosmos-arm-stm32f4-discovery-0.1.9.276.a and b/source/os/release/libkosmos-arm-stm32f4-discovery-0.1.10.287.a differ diff --git a/source/os/release/libkosmos-arm-stm32f4-discovery.a b/source/os/release/libkosmos-arm-stm32f4-discovery.a index 3117043..489e625 120000 --- a/source/os/release/libkosmos-arm-stm32f4-discovery.a +++ b/source/os/release/libkosmos-arm-stm32f4-discovery.a @@ -1 +1 @@ -libkosmos-arm-stm32f4-discovery-0.1.9.276.a \ No newline at end of file +libkosmos-arm-stm32f4-discovery-0.1.10.287.a \ No newline at end of file