engine/drive ctrl functionality splitted/reordered

source/application/drive_ctrl.c

@@ -5,149 +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 "engine_ctrl.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 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;
+	return engine_ctrl_init();
+}
 
-	/* open enable pins */
+int drive_ctrl_turn_right(void)
-	drv_open(drive_ctrl.left_forward->enable);
+{
-	drv_write(drive_ctrl.left_forward->enable, "0", 1);
+	drive_ctrl_halt();
-	drv_open(drive_ctrl.left_backward->enable);
+	int turn_speed = engine_ctrl_get_turn_speed_value();
-	drv_write(drive_ctrl.left_backward->enable, "0", 1);
+	for(int i = 0; i < turn_speed; i++) {
-	drv_open(drive_ctrl.right_forward->enable);
+		engine_ctrl_set_speed_left(i);
-	drv_write(drive_ctrl.right_forward->enable, "0", 1);
+		engine_ctrl_set_speed_right(-1 * i);
-	drv_open(drive_ctrl.right_backward->enable);
+		sleep_ms(10);
-	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;
 }
 
-static unsigned int calc_speed_front(int speed_percent)
+int drive_ctrl_turn_left(void)
 {
-	unsigned int speed_front = 0;
+	drive_ctrl_halt();
-	if(speed_percent > 0) {
+	int turn_speed = engine_ctrl_get_turn_speed_value();
-		if(speed_percent > 100)
+	for(int i = 0; i < turn_speed; i++) {
-			speed_front = 100;
+		engine_ctrl_set_speed_right(i);
-		else
+		engine_ctrl_set_speed_left(-1 * i);
-			speed_front = (unsigned int) speed_percent;
+		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.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 speed = engine_ctrl_get_target_speed_value();
-}
+	speed += 10;
-
+	engine_ctrl_set_target_speed_value(speed);
-int drive_ctrl_set_current_speed_value(int speed_percent)
+	engine_ctrl_set_speed_left(speed);
-{
+	engine_ctrl_set_speed_right(speed);
-	drive_ctrl_object.current_speed = speed_percent;
 
 	return 0;
 }
 
-int drive_ctrl_get_current_speed_value(void)
+int drive_ctrl_retard(void)
 {
-	return drive_ctrl_object.current_speed;
+	int speed = engine_ctrl_get_target_speed_value();
-}
+	speed -= 10;
-
+	engine_ctrl_set_target_speed_value(speed);
-int drive_ctrl_set_turn_speed_value(int speed_percent)
+	engine_ctrl_set_speed_left(speed);
-{
+	engine_ctrl_set_speed_right(speed);
-	drive_ctrl_object.turn_speed = speed_percent;
 
 	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;
 }

source/application/engine_ctrl.c

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

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_turn_right(void);
-int drive_ctrl_set_speed_right(int speed_percent);
+int drive_ctrl_turn_left(void);
-int drive_ctrl_set_target_speed_value(int speed_percent);
+int drive_ctrl_boost(void);
-int drive_ctrl_get_target_speed_value(void);
+int drive_ctrl_retard(void);
-int drive_ctrl_set_current_speed_value(int speed_percent);
+int drive_ctrl_halt(void);
-int drive_ctrl_get_current_speed_value(void);
+int drive_ctrl_forward(void);
-int drive_ctrl_set_turn_speed_value(int speed_percent);
+int drive_ctrl_backward(void);
-int drive_ctrl_get_turn_speed_value(void);
-
 
 #endif /* SOURCE_APPLICATION_INCLUDE_DRIVE_CTRL_H_ */

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

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

source/application/shell_commands.c

@@ -92,126 +92,43 @@ int shell_commands_init(void)
 
 static void *drive_turn_right_cb(const char *param)
 {
-	drive_halt_cb(NULL);
+	drive_ctrl_turn_right();
-	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);
-	}
-
 	return NULL;
 }
 
 static void *drive_turn_left_cb(const char *param)
 {
-	drive_halt_cb(NULL);
+	drive_ctrl_turn_left();
-	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);
-	}
-
 	return NULL;
 }
 
 static void *drive_boost_cb(const char *param)
 {
-	int speed = drive_ctrl_get_target_speed_value();
+	drive_ctrl_boost();
-	speed += 10;
-	drive_ctrl_set_target_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_target_speed_value();
+	drive_ctrl_retard();
-	speed -= 10;
-	drive_ctrl_set_target_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)
 {
-	int current_speed = drive_ctrl_get_current_speed_value();
+	drive_ctrl_halt();
-	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);
-		}
-	}
-
 	return NULL;
 }
 
 static void *drive_forward_cb(const char *param)
 {
-	int target_speed = drive_ctrl_get_target_speed_value();
+	drive_ctrl_forward();
-	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);
-		}
-	}
-
 	return NULL;
 }
 
 static void *drive_backward_cb(const char *param)
 {
-	int target_speed = drive_ctrl_get_target_speed_value();
+	drive_ctrl_backward();
-	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);
-		}
-	}
-
 	return NULL;
 }