state machine handling added

source/application/collision_ctrl.c

@@ -41,6 +41,7 @@ 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 0
 		if(collision_ctrl_object.distance_mm < 300) {
 			char print_buffer[50];
 			if(engine_ctrl_get_current_speed_value() != 0) {
@@ -68,6 +69,7 @@ static void collision_ctrl_loop(void *arg)
 				shell_write(print_buffer, strlen(print_buffer));
 			}
 		}
+#endif
 		sleep_ms(1);
 	}
 }
@@ -89,3 +91,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;
+}

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

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

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;

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;
 }
 

source/application/system_state.c

@@ -0,0 +1,221 @@
+/*
+ * 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
+		int sleep = drv_read(&rng, NULL, 0) % 3500 + 500;
+		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;
+}

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

source/os/release/libkosmos-arm-stm32f4-discovery.a

@@ -1 +0,0 @@
-libkosmos-arm-stm32f4-discovery-0.1.9.276.a