collision detection system

source/application/collision_ctrl.c

@@ -0,0 +1,90 @@
+/*
+ * 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 */
+	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(3000);	/* todo: randomize */
+				drive_ctrl_halt();
+
+				sprintf(print_buffer, "start turning...\r\n");
+				shell_write(print_buffer, strlen(print_buffer));
+				drive_ctrl_turn_left();
+				sleep_ms(3000);
+				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;
+}

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

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

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

source/application/sensor_data.c

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

source/application/shell_commands.c

@@ -18,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);
@@ -27,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",
@@ -86,6 +101,8 @@ 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_sys_msg_off);
+	shell_add_command(&cmd_sys_msg_on);
 
 	return 0;
 }
@@ -146,3 +163,14 @@ static void *app_version(const char *param)
 
 	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;
+}