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

basic drive module and shell interface

Browse Source
This commit is contained in:
tkl
2016-08-11 14:06:45 +02:00
parent 95defd4a87
commit 8270eca957
6 changed files with 352 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

125
source/application/drive_ctrl.c Normal file
View File

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

19
source/application/include/drive_ctrl.h Normal file
View File

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

39
source/application/include/engines.h Normal file
View File

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

13
source/application/include/shell_commands.h Normal file
View File

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

50
source/application/main.c
View File

@@ -1,55 +1,21 @@
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.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);
thread_create(&tc_1, tc_1_stack, STACK_SIZE, task1, NULL, THREAD_PRIO_LOW);
thread_create(&tc_2, tc_2_stack, STACK_SIZE, task2, NULL, THREAD_PRIO_LOW);
drive_ctrl_init();
shell_init(&uart_1);
shell_commands_init();
schedule_start();

148
source/application/shell_commands.c Normal file
View File

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