91 lines
2.1 KiB
C
91 lines
2.1 KiB
C
|
/*
|
||
|
|
* collision_ctrl.c
|
||
|
|
*
|
||
|
|
* Created on: Aug 30, 2016
|
||
|
|
* Author: tkl
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include <stddef.h>
|
||
|
|
#include <stdlib.h>
|
||
|
|
#include <string.h>
|
||
|
|
#include <stdbool.h>
|
||
|
|
#include <stdint.h>
|
||
|
|
#include <stdio.h>
|
||
|
|
|
||
|
|
#include "board_devices.h"
|
||
|
|
#include "queue.h"
|
||
|
|
#include "list.h"
|
||
|
|
#include "stack.h"
|
||
|
|
#include "kernel.h"
|
||
|
|
#include "driver.h"
|
||
|
|
#include "shell.h"
|
||
|
|
#include "drive_ctrl.h"
|
||
|
|
#include "engine_ctrl.h"
|
||
|
|
|
||
|
|
#pragma pack(push)
|
||
|
|
#pragma pack(1)
|
||
|
|
struct collision_ctrl_object {
|
||
|
|
int distance_mm;
|
||
|
|
};
|
||
|
|
#pragma pack(pop)
|
||
|
|
|
||
|
|
struct collision_ctrl_object collision_ctrl_object;
|
||
|
|
struct thread_context collision_ctrl_ctx;
|
||
|
|
stack_t collision_ctrl_stack[256];
|
||
|
|
|
||
|
|
static void collision_ctrl_loop(void *arg)
|
||
|
|
{
|
||
|
|
drv_open(&pwm5_c2); /* trigger pwm */
|
||
|
|
drv_open(&pwm2_c4); /* read pwm */
|
||
|
|
while(1) {
|
||
|
|
int pulse_ns = drv_ioctl(&pwm2_c4, IOCTL_PWM_GET_PULSE_WIDTH_NS, NULL);
|
||
|
|
collision_ctrl_object.distance_mm = 343 * pulse_ns / 1000 / 2;
|
||
|
|
if(collision_ctrl_object.distance_mm < 300) {
|
||
|
|
char print_buffer[50];
|
||
|
|
if(engine_ctrl_get_current_speed_value() != 0) {
|
||
|
|
drive_ctrl_halt();
|
||
|
|
sprintf(print_buffer, "collision detected (%dmm)...\r\n", collision_ctrl_object.distance_mm);
|
||
|
|
shell_write(print_buffer, strlen(print_buffer));
|
||
|
|
sprintf(print_buffer, "going back...\r\n");
|
||
|
|
shell_write(print_buffer, strlen(print_buffer));
|
||
|
|
drive_ctrl_backward();
|
||
|
|
sleep_ms(3000); /* todo: randomize */
|
||
|
|
drive_ctrl_halt();
|
||
|
|
|
||
|
|
sprintf(print_buffer, "start turning...\r\n");
|
||
|
|
shell_write(print_buffer, strlen(print_buffer));
|
||
|
|
drive_ctrl_turn_left();
|
||
|
|
sleep_ms(3000);
|
||
|
|
drive_ctrl_halt();
|
||
|
|
|
||
|
|
sprintf(print_buffer, "restart forward...\r\n");
|
||
|
|
shell_write(print_buffer, strlen(print_buffer));
|
||
|
|
drive_ctrl_forward();
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
sprintf(print_buffer, "collision detected while not driving 8(\r\n");
|
||
|
|
shell_write(print_buffer, strlen(print_buffer));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
sleep_ms(1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
int collision_ctrl_init(void)
|
||
|
|
{
|
||
|
|
thread_create(
|
||
|
|
&collision_ctrl_ctx,
|
||
|
|
collision_ctrl_stack,
|
||
|
|
sizeof(collision_ctrl_stack) / sizeof(stack_t),
|
||
|
|
collision_ctrl_loop,
|
||
|
|
NULL,
|
||
|
|
THREAD_PRIO_LOW);
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
int collision_ctrl_get_distance_mm(void)
|
||
|
|
{
|
||
|
|
return collision_ctrl_object.distance_mm;
|
||
|
|
}
|