135 lines
3.2 KiB
C
135 lines
3.2 KiB
C
/*
|
|
* 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;
|
|
}
|