diff --git a/source/application/drive_ctrl.c b/source/application/drive_ctrl.c index 7acc498..421f18b 100644 --- a/source/application/drive_ctrl.c +++ b/source/application/drive_ctrl.c @@ -21,6 +21,8 @@ struct drive_ctrl { }; struct drive_ctrl_object { + unsigned int target_speed; + unsigned int turn_speed; unsigned int current_speed; }; @@ -32,7 +34,9 @@ static struct drive_ctrl drive_ctrl = { }; static struct drive_ctrl_object drive_ctrl_object = { - .current_speed = 50, /* % */ + .target_speed = 80, /* % */ + .turn_speed = 40, + .current_speed = 0, /* % */ }; int drive_ctrl_init(void) @@ -112,14 +116,38 @@ int drive_ctrl_set_speed_right(int speed_percent) return ret; } -int drive_ctrl_set_speed_value(int speed_percent) +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) +{ + return drive_ctrl_object.target_speed; +} + +int drive_ctrl_set_current_speed_value(int speed_percent) { drive_ctrl_object.current_speed = speed_percent; return 0; } -int drive_ctrl_get_speed_value(void) +int drive_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 drive_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 f499a5b..2672ecc 100644 --- a/source/application/include/drive_ctrl.h +++ b/source/application/include/drive_ctrl.h @@ -12,8 +12,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_speed_value(int speed_percent); -int drive_ctrl_get_speed_value(void); +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); #endif /* SOURCE_APPLICATION_INCLUDE_DRIVE_CTRL_H_ */ diff --git a/source/application/shell_commands.c b/source/application/shell_commands.c index 0cb89f8..bb57655 100644 --- a/source/application/shell_commands.c +++ b/source/application/shell_commands.c @@ -6,10 +6,15 @@ */ #include #include +#include +#include #include "list.h" #include "driver.h" #include "shell.h" +#include "queue.h" +#include "stack.h" +#include "kernel.h" #include "drive_ctrl.h" #include "version.h" @@ -86,31 +91,35 @@ int shell_commands_init(void) static void *drive_turn_right_cb(const char *param) { - int speed = drive_ctrl_get_speed_value(); - drive_ctrl_set_speed_left(abs(speed)); - if(speed > 0) - speed *= -1; - drive_ctrl_set_speed_right(speed); + 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); + } return NULL; } static void *drive_turn_left_cb(const char *param) { - int speed = drive_ctrl_get_speed_value(); - drive_ctrl_set_speed_right(abs(speed)); - if(speed > 0) - speed *= -1; - drive_ctrl_set_speed_left(speed); + 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); + } return NULL; } static void *drive_boost_cb(const char *param) { - int speed = drive_ctrl_get_speed_value(); + int speed = drive_ctrl_get_target_speed_value(); speed += 10; - drive_ctrl_set_speed_value(speed); + drive_ctrl_set_target_speed_value(speed); drive_ctrl_set_speed_left(speed); drive_ctrl_set_speed_right(speed); @@ -119,9 +128,9 @@ static void *drive_boost_cb(const char *param) static void *drive_retard_cb(const char *param) { - int speed = drive_ctrl_get_speed_value(); + int speed = drive_ctrl_get_target_speed_value(); speed -= 10; - drive_ctrl_set_speed_value(speed); + drive_ctrl_set_target_speed_value(speed); drive_ctrl_set_speed_left(speed); drive_ctrl_set_speed_right(speed); @@ -130,32 +139,77 @@ static void *drive_retard_cb(const char *param) static void *drive_halt_cb(const char *param) { - drive_ctrl_set_speed_left(0); - drive_ctrl_set_speed_right(0); + 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); + } + } 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); + 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); + } + } 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); + 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); + } + } return NULL; }