basic drive module and shell interface

source/application/drive_ctrl.c

@@ -0,0 +1,125 @@
+/*
+ * drive_ctrl.c
+ *
+ *  Created on: Aug 10, 2016
+ *      Author: tkl
+ */
+
+#include <stdlib.h>
+
+#include "driver.h"
+#include "board_devices.h"
+#include "engines.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;
+
+	/* open enable pins */
+	open(drive_ctrl.left_forward->enable);
+	write(drive_ctrl.left_forward->enable, "0", 1);
+	open(drive_ctrl.left_backward->enable);
+	write(drive_ctrl.left_backward->enable, "0", 1);
+	open(drive_ctrl.right_forward->enable);
+	write(drive_ctrl.right_forward->enable, "0", 1);
+	open(drive_ctrl.right_backward->enable);
+	write(drive_ctrl.right_backward->enable, "0", 1);
+
+	/* open pwm's */
+	open(drive_ctrl.left_backward->pwm);
+	ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
+	open(drive_ctrl.left_forward->pwm);
+	ioctl(drive_ctrl.left_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
+	open(drive_ctrl.right_backward->pwm);
+	ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
+	open(drive_ctrl.right_forward->pwm);
+	ioctl(drive_ctrl.right_forward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&duty);
+
+	/* enable enable pins */
+	write(drive_ctrl.left_forward->enable, "1", 1);
+	write(drive_ctrl.left_backward->enable, "1", 1);
+	write(drive_ctrl.right_forward->enable, "1", 1);
+	write(drive_ctrl.right_backward->enable, "1", 1);
+
+	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 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 = ioctl(drive_ctrl.left_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&back_speed);
+	ret |= 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 = ioctl(drive_ctrl.right_backward->pwm, IOCTL_PWM_SET_DUTY_CYCLE, (const void *)&back_speed);
+	ret |= 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)
+{
+	return drive_ctrl_object.current_speed;
+}

source/application/include/drive_ctrl.h

@@ -0,0 +1,19 @@
+/*
+ * drive_ctrl.h
+ *
+ *  Created on: Aug 10, 2016
+ *      Author: tkl
+ */
+
+#ifndef SOURCE_APPLICATION_INCLUDE_DRIVE_CTRL_H_
+#define SOURCE_APPLICATION_INCLUDE_DRIVE_CTRL_H_
+
+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);
+
+
+#endif /* SOURCE_APPLICATION_INCLUDE_DRIVE_CTRL_H_ */

source/application/include/engines.h

@@ -0,0 +1,39 @@
+/*
+ * engines.h
+ *
+ *  Created on: Aug 10, 2016
+ *      Author: tkl
+ */
+
+#ifndef SOURCE_APPLICATION_INCLUDE_ENGINES_H_
+#define SOURCE_APPLICATION_INCLUDE_ENGINES_H_
+
+struct engine_ctrl {
+	const struct driver *pwm;
+	const struct driver *enable;
+};
+
+static struct engine_ctrl right_forward = {
+		.pwm = &pwm_1,
+		.enable = &gpio_c1,
+};
+
+static struct engine_ctrl right_backward = {
+		.pwm = &pwm_2,
+		.enable = &gpio_c0,
+};
+
+static struct engine_ctrl left_forward = {
+		.pwm = &pwm_3,
+		.enable = &gpio_c3,
+};
+
+static struct engine_ctrl left_backward = {
+		.pwm = &pwm_4,
+		.enable = &gpio_c2,
+};
+
+
+
+
+#endif /* SOURCE_APPLICATION_INCLUDE_ENGINES_H_ */

source/application/include/shell_commands.h

@@ -0,0 +1,13 @@
+/*
+ * shell_commands.h
+ *
+ *  Created on: Aug 10, 2016
+ *      Author: tkl
+ */
+
+#ifndef SOURCE_APPLICATION_SHELL_COMMANDS_H_
+#define SOURCE_APPLICATION_SHELL_COMMANDS_H_
+
+int shell_commands_init(void);
+
+#endif /* SOURCE_APPLICATION_SHELL_COMMANDS_H_ */

source/application/main.c

@@ -1,55 +1,21 @@
-#include <stdint.h>
 #include <stdbool.h>
-#include <string.h>
+#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
 
-#include "driver.h"
 #include "board_devices.h"
 #include "stack.h"
 #include "queue.h"
 #include "kernel.h"
 #include "driver.h"
+#include "list.h"
+#include "shell.h"
+#include "shell_commands.h"
+#include "drive_ctrl.h"
 
-#define STACK_SIZE  256
-stack_t tc_1_stack[STACK_SIZE];
-struct thread_context tc_1;
-
-void task1(void *arg)
-{
-	char rd = '0';
-	open(&led_4);
-	write(&led_4, &rd, 1);
-	while(1) {
-		sleep_ms(1000);
-		read(&led_4, &rd, 1);
-		if(rd == '0')
-			rd = '1';
-		else
-			rd = '0';
-		write(&led_4, &rd, 1);
-		write(&uart_1, "Driver test\r\n", 13);
-	}
-}
-
-char rx_buf[80];
-void task2(void *arg)
-{
-	while(1) {
-		int i = read(&uart_1, rx_buf, 80);
-		if(i > 0) {
-			write(&uart_1, rx_buf, i);
-		}
-	}
-}
-
-stack_t tc_2_stack[STACK_SIZE];
-struct thread_context tc_2;
 int main(void)
 {
-	open(&uart_1);
+	drive_ctrl_init();
-	thread_create(&tc_1, tc_1_stack, STACK_SIZE, task1, NULL, THREAD_PRIO_LOW);
+	shell_init(&uart_1);
-	thread_create(&tc_2, tc_2_stack, STACK_SIZE, task2, NULL, THREAD_PRIO_LOW);
+	shell_commands_init();
 
 	schedule_start();
 

source/application/shell_commands.c

@@ -0,0 +1,148 @@
+/*
+ * shell_commands.c
+ *
+ *  Created on: Aug 10, 2016
+ *      Author: tkl
+ */
+#include <stddef.h>
+#include <stdlib.h>
+
+#include "list.h"
+#include "driver.h"
+#include "shell.h"
+#include "drive_ctrl.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 struct command drive_forward = {
+		.command = "f",
+		.description = "Start driving forward with preset speed.",
+		.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);
+
+	return 0;
+}
+
+static void *drive_turn_right_cb(const char *param)
+{
+	int speed = drive_ctrl_get_speed_value();
+	drive_ctrl_set_speed_left(speed);
+	drive_ctrl_set_speed_right(0);
+
+	return NULL;
+}
+
+static void *drive_turn_left_cb(const char *param)
+{
+	int speed = drive_ctrl_get_speed_value();
+	drive_ctrl_set_speed_left(0);
+	drive_ctrl_set_speed_right(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);
+
+	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);
+
+	return NULL;
+}