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

engine/drive ctrl functionality splitted/reordered

Browse Source
This commit is contained in:
Thomas Klaehn 2016-08-30 10:07:47 +02:00
parent 30e55ff1cf
commit 33a8e8ae2d
6 changed files with 281 additions and 218 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

225
source/application/drive_ctrl.c
View File

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

134
source/application/engine_ctrl.c Normal file
View File

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

16
source/application/include/drive_ctrl.h
View File

@ -10,14 +10,12 @@
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_target_speed_value(int speed_percent);
int drive_ctrl_get_target_speed_value(void);
int drive_ctrl_set_current_speed_value(int speed_percent);
int drive_ctrl_get_current_speed_value(void);
int drive_ctrl_set_turn_speed_value(int speed_percent);
int drive_ctrl_get_turn_speed_value(void);
int drive_ctrl_turn_right(void);
int drive_ctrl_turn_left(void);
int drive_ctrl_boost(void);
int drive_ctrl_retard(void);
int drive_ctrl_halt(void);
int drive_ctrl_forward(void);
int drive_ctrl_backward(void);
#endif /* SOURCE_APPLICATION_INCLUDE_DRIVE_CTRL_H_ */

22
source/application/include/engine_ctrl.h Normal file
View File

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

5
source/application/include/engines.h
View File

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

97
source/application/shell_commands.c
View File

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