126 lines
3.4 KiB
C
126 lines
3.4 KiB
C
/*
|
|
* 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 */
|
|
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;
|
|
}
|
|
|
|
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 = 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_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;
|
|
}
|