diff --git a/source/application/collision_ctrl.c b/source/application/collision_ctrl.c new file mode 100644 index 0000000..e3a54a1 --- /dev/null +++ b/source/application/collision_ctrl.c @@ -0,0 +1,91 @@ +/* + * 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 */ + drv_open(&rng); + 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); + } +} + +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/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/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/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/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 bb57655..fb54ef4 100644 --- a/source/application/shell_commands.c +++ b/source/application/shell_commands.c @@ -6,6 +6,7 @@ */ #include #include +#include #include #include @@ -17,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); @@ -26,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", @@ -77,6 +93,7 @@ static struct command drive_turn_right = { 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); @@ -84,147 +101,76 @@ 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_app_version); + 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_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; } static void *app_version(const char *param) { -#if 0 char *greeter = "engine_control version: "; - drv_write(shell_object.shell_device, greeter, strlen(greeter)); - drv_write(shell_object.shell_device, MAJOR_VERSION, strlen(MAJOR_VERSION)); - drv_write(shell_object.shell_device, ".", 1); - drv_write(shell_object.shell_device, MINOR_VERSION, strlen(MINOR_VERSION)); - drv_write(shell_object.shell_device, ".", 1); - drv_write(shell_object.shell_device, BUILD_NUMBER, strlen(BUILD_NUMBER)); -#endif + shell_write(greeter, strlen(greeter)); + shell_write(MAJOR_VERSION, strlen(MAJOR_VERSION)); + shell_write(".", 1); + shell_write(MINOR_VERSION, strlen(MINOR_VERSION)); + shell_write(".", 1); + shell_write(BUILD_NUMBER, strlen(BUILD_NUMBER)); + shell_write("\r\n", 2); + + 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; } diff --git a/source/os/release/include/board_devices.h b/source/os/release/include/board_devices.h index 657b7b3..129e41c 100644 --- a/source/os/release/include/board_devices.h +++ b/source/os/release/include/board_devices.h @@ -3,6 +3,7 @@ #ifndef BOARD_DEVICES_H #define BOARD_DEVICES_H +extern const struct driver rng; extern const struct driver gpio_c0; extern const struct driver gpio_c1; extern const struct driver gpio_c2; diff --git a/source/os/release/include/driver.h b/source/os/release/include/driver.h index 6ce5b5d..8af40b4 100644 --- a/source/os/release/include/driver.h +++ b/source/os/release/include/driver.h @@ -17,6 +17,7 @@ enum driver_type { DRIVER_TYPE_GPIO, DRIVER_TYPE_I2C, DRIVER_TYPE_PWM, + DRIVER_TYPE_RNG, DRIVER_TYPE_RTC, DRIVER_TYPE_SPI, DRIVER_TYPE_UART diff --git a/source/os/release/include/shell.h b/source/os/release/include/shell.h index c242e8d..1b176ee 100644 --- a/source/os/release/include/shell.h +++ b/source/os/release/include/shell.h @@ -19,5 +19,6 @@ struct command { int shell_init(const struct driver *shell_device); int shell_add_command(struct command *command); +int shell_write(const char *buffer, unsigned int len); #endif /* SOURCE_FIRMWARE_KERNEL_INTERFACE_SHELL_H_ */ diff --git a/source/os/release/libkosmos-arm-stm32f4-discovery-0.1.5.250.a b/source/os/release/libkosmos-arm-stm32f4-discovery-0.1.9.276.a similarity index 88% rename from source/os/release/libkosmos-arm-stm32f4-discovery-0.1.5.250.a rename to source/os/release/libkosmos-arm-stm32f4-discovery-0.1.9.276.a index c1d60ea..05fd6e1 100644 Binary files a/source/os/release/libkosmos-arm-stm32f4-discovery-0.1.5.250.a and b/source/os/release/libkosmos-arm-stm32f4-discovery-0.1.9.276.a differ diff --git a/source/os/release/libkosmos-arm-stm32f4-discovery.a b/source/os/release/libkosmos-arm-stm32f4-discovery.a index f542b68..3117043 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.5.250.a \ No newline at end of file +libkosmos-arm-stm32f4-discovery-0.1.9.276.a \ No newline at end of file