r2maeh2/engine_control
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Merge branch 'develop' into 'master'

Browse Source 
Develop



See merge request !17
This commit is contained in:
tkl
2016-09-21 14:49:16 +00:00
parent 0a98f81be7 78e4a17e07
commit 51fb102389
20 changed files with 776 additions and 240 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.gitlab-ci.yml
View File

@@ -1,6 +1,6 @@
variables:
SW_MAJOR: "0"
SW_MINOR: "1"
SW_MINOR: "2"
APP: "engine_control"
before_script:

71
source/application/collision_ctrl.c Normal file
View File

@@ -0,0 +1,71 @@
/*
* collision_ctrl.c
*
* Created on: Aug 30, 2016
* Author: tkl
*/
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#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;
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;
}
bool collision_ctrl_is_collision(void)
{
if(collision_ctrl_object.distance_mm < 400)
return true;
return false;
}

219
source/application/drive_ctrl.c
View File

@@ -5,121 +5,146 @@
* Author: tkl
*/
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <stdint.h>
#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 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;
return engine_ctrl_init();
}
/* 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);
int drive_ctrl_turn_right(void)
{
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 0;
}
static unsigned int calc_speed_front(int speed_percent)
int drive_ctrl_turn_left(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_right(i);
engine_ctrl_set_speed_left(-1 * i);
sleep_ms(10);
}
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 = drv_ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&back_speed);
ret |= drv_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 = drv_ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&back_speed);
ret |= drv_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)
int drive_ctrl_boost(void)
{
return drive_ctrl_object.current_speed;
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_retard(void)
{
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_halt(void)
{
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;
}

134
source/application/engine_ctrl.c Normal file
View File

@@ -0,0 +1,134 @@
/*
* engine_ctrl.c
*
* Created on: Aug 30, 2016
* Author: tkl
*/
#include <stdlib.h>
#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;
}

15
source/application/include/collision_ctrl.h Normal file
View File

@@ -0,0 +1,15 @@
/*
* 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);
bool collision_ctrl_is_collision(void);
#endif /* SOURCE_APPLICATION_INCLUDE_COLLISION_CTRL_H_ */

12
source/application/include/drive_ctrl.h
View File

@@ -10,10 +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_speed_value(int speed_percent);
int drive_ctrl_get_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_ */

22
source/application/include/engine_ctrl.h Normal file
View File

@@ -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_ */

23
source/application/include/engines.h
View File

@@ -10,27 +10,22 @@
struct engine_ctrl {
const struct driver *pwm;
const struct driver *enable;
};
static struct engine_ctrl right_forward = {
.pwm = &pwm_2,
.enable = &gpio_c0,
};
static struct engine_ctrl right_backward = {
.pwm = &pwm_1,
.enable = &gpio_c1,
};
static struct engine_ctrl left_forward = {
.pwm = &pwm_4,
.enable = &gpio_c2,
};
static struct engine_ctrl right_backward = {
.pwm = &pwm_3,
};
static struct engine_ctrl right_forward = {
.pwm = &pwm_2,
};
static struct engine_ctrl left_backward = {
.pwm = &pwm_3,
.enable = &gpio_c3,
.pwm = &pwm_1,
};
#endif /* SOURCE_APPLICATION_INCLUDE_ENGINES_H_ */

14
source/application/include/sensor_data.h Normal file
View File

@@ -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_ */

26
source/application/include/system_state.h Normal file
View File

@@ -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_ */

11
source/application/main.c
View File

@@ -10,15 +10,22 @@
#include "shell.h"
#include "shell_commands.h"
#include "drive_ctrl.h"
#include "collision_ctrl.h"
#include "sensor_data.h"
#include "system_state.h"
int main(void)
{
drive_ctrl_init();
shell_init(&uart_1);
shell_commands_init();
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;

65
source/application/sensor_data.c Normal file
View File

@@ -0,0 +1,65 @@
/*
* sensor_data.c
*
* Created on: Aug 30, 2016
* Author: tkl
*/
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#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;
}

168
source/application/shell_commands.c
View File

@@ -6,21 +6,37 @@
*/
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <stdint.h>
#include "list.h"
#include "driver.h"
#include "shell.h"
#include "drive_ctrl.h"
#include "queue.h"
#include "stack.h"
#include "kernel.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);
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",
@@ -29,148 +45,62 @@ 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(&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_app_version);
shell_add_command(&drive_forward);
shell_add_command(&drive_halt);
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)
{
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);
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);
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);
system_state_force(SYS_IDLE);
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);
system_state_force(SYS_DRIVING);
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;
}

222
source/application/system_state.c Normal file
View File

@@ -0,0 +1,222 @@
/*
* system_state.c
*
* Created on: Sep 19, 2016
* Author: tkl
*/
#include <string.h>
#include <stdbool.h>
#include <stdint.h>
#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;
}

3
source/os/release/include/board_devices.h
View File

@@ -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;
@@ -11,6 +12,8 @@ extern const struct driver pwm_4;
extern const struct driver pwm_3;
extern const struct driver pwm_2;
extern const struct driver pwm_1;
extern const struct driver pwm5_c2;
extern const struct driver pwm2_c4;
extern const struct driver uart_1;
#endif /* BOARD_DEVICES_H */

3
source/os/release/include/driver.h
View File

@@ -9,12 +9,15 @@
#define SOURCE_FIRMWARE_KERNEL_DRIVER_INCLUDE_DRIVER_H_
#define IOCTL_PWM_SET_DUTY_CYCLE 0
#define IOCTL_PWM_GET_PERIOD_NS 1
#define IOCTL_PWM_GET_PULSE_WIDTH_NS 2
enum driver_type {
DRIVER_TYPE_ADC,
DRIVER_TYPE_GPIO,
DRIVER_TYPE_I2C,
DRIVER_TYPE_PWM,
DRIVER_TYPE_RNG,
DRIVER_TYPE_RTC,
DRIVER_TYPE_SPI,
DRIVER_TYPE_UART

1
source/os/release/include/kernel.h
View File

@@ -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_ */

1
source/os/release/include/shell.h
View File

@@ -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_ */

BIN
source/os/release/libkosmos-arm-stm32f4-discovery-0.1.4.a → source/os/release/libkosmos-arm-stm32f4-discovery-0.1.10.287.a
View File

Binary file not shown.

2
source/os/release/libkosmos-arm-stm32f4-discovery.a
View File

@@ -1 +1 @@
libkosmos-arm-stm32f4-discovery-0.1.4.a
libkosmos-arm-stm32f4-discovery-0.1.10.287.a