151 lines
3.1 KiB
C
151 lines
3.1 KiB
C
/*
|
|
* drive_ctrl.c
|
|
*
|
|
* Created on: Aug 10, 2016
|
|
* 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 "engine_ctrl.h"
|
|
|
|
#include "drive_ctrl.h"
|
|
|
|
int drive_ctrl_init(void)
|
|
{
|
|
return engine_ctrl_init();
|
|
}
|
|
|
|
int drive_ctrl_turn_right(void)
|
|
{
|
|
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 0;
|
|
}
|
|
|
|
int drive_ctrl_turn_left(void)
|
|
{
|
|
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 0;
|
|
}
|
|
|
|
int drive_ctrl_boost(void)
|
|
{
|
|
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_retard(void)
|
|
{
|
|
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_halt(void)
|
|
{
|
|
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;
|
|
}
|