You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
138 lines
4.2 KiB
138 lines
4.2 KiB
/*
|
|
* robotNav.c
|
|
*
|
|
* Author: Erich Styger
|
|
* License: PDX-License-Identifier: BSD-3-Clause
|
|
*/
|
|
|
|
#include "robotNav.h"
|
|
#include "gpio.h"
|
|
#include "udp.h"
|
|
#include <stdio.h>
|
|
#include <unistd.h> /* interface to sleep */
|
|
|
|
static void BlinkLed(Gpio_Pins_e led) {
|
|
Gpio_WritePin(led, true); /* on */
|
|
usleep(100*1000);
|
|
Gpio_WritePin(led, false); /* off */
|
|
}
|
|
|
|
int ROBONAV_test(void) {
|
|
bool buttonPressed;
|
|
|
|
printf("Use NAV switch on HAT, <CENTER> for > 1 sec to exit.\n");
|
|
for(;;) {
|
|
buttonPressed = false;
|
|
if (Gpio_ReadPin(Gpio_Pins_BTN_LEFT_BCM)==0) {
|
|
printf("left\n");
|
|
printf("green\n"); BlinkLed(Gpio_Pins_LED_GREEN_PIN_BCM);
|
|
}
|
|
if (Gpio_ReadPin(Gpio_Pins_BTN_RIGHT_BCM)==0) {
|
|
printf("right\n");
|
|
printf("red\n"); BlinkLed(Gpio_Pins_LED_RED_PIN_BCM);
|
|
}
|
|
if (Gpio_ReadPin(Gpio_Pins_BTN_UP_BCM)==0) {
|
|
printf("up\n");
|
|
printf("blue\n"); BlinkLed(Gpio_Pins_LED_BLUE_PIN_BCM);
|
|
}
|
|
if (Gpio_ReadPin(Gpio_Pins_BTN_DOWN_BCM)==0) {
|
|
printf("down\n");
|
|
printf("yellow\n"); BlinkLed(Gpio_Pins_LED_YELLOW_PIN_BCM);
|
|
}
|
|
if (Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0) {
|
|
printf("center\n");
|
|
|
|
int timeMs = 0;
|
|
while(Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0 && timeMs <= 1000) {
|
|
usleep(100*1000);
|
|
timeMs += 100;
|
|
}
|
|
if (Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0) { /* still pressed? */
|
|
printf("exit program\n");
|
|
break; /* break for loop and return */
|
|
}
|
|
}
|
|
usleep(100*1000);
|
|
} /* for */
|
|
return 0; /* ok */
|
|
}
|
|
|
|
int ROBONAV_navigate(const char *server, int port) {
|
|
unsigned int buttons; /* mask of buttons pressed */
|
|
#define BTN_LEFT_MASK (1<<0)
|
|
#define BTN_RIGHT_MASK (1<<1)
|
|
#define BTN_UP_MASK (1<<2)
|
|
#define BTN_DOWN_MASK (1<<3)
|
|
#define BTN_CENTER_MASK (1<<4)
|
|
#define ALL_BUTTONS_MASK (BTN_LEFT_MASK|BTN_RIGHT_MASK|BTN_UP_MASK|BTN_DOWN_MASK|BTN_CENTER_MASK)
|
|
|
|
printf("Use NAV switch on HAT to navigate robot, <CENTER> for > 1 sec to exit.\n");
|
|
udp_send(server, port, "test");
|
|
for(;;) {
|
|
buttons = 0;
|
|
if (Gpio_ReadPin(Gpio_Pins_BTN_LEFT_BCM)==0) {
|
|
buttons |= BTN_LEFT_MASK;
|
|
}
|
|
if (Gpio_ReadPin(Gpio_Pins_BTN_RIGHT_BCM)==0) {
|
|
buttons |= BTN_RIGHT_MASK;
|
|
}
|
|
if (Gpio_ReadPin(Gpio_Pins_BTN_UP_BCM)==0) {
|
|
buttons |= BTN_UP_MASK;
|
|
}
|
|
if (Gpio_ReadPin(Gpio_Pins_BTN_DOWN_BCM)==0) {
|
|
buttons |= BTN_DOWN_MASK;
|
|
}
|
|
if (Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0) {
|
|
buttons |= BTN_CENTER_MASK;
|
|
}
|
|
if ((buttons&ALL_BUTTONS_MASK)==(BTN_LEFT_MASK|BTN_UP_MASK)) {
|
|
printf("left-up\n");
|
|
udp_send(server, port, "nav_left-up");
|
|
} else if ((buttons&ALL_BUTTONS_MASK)==BTN_LEFT_MASK) {
|
|
printf("left\n");
|
|
udp_send(server, port, "nav_left");
|
|
} else if ((buttons&ALL_BUTTONS_MASK)==(BTN_RIGHT_MASK|BTN_UP_MASK)) {
|
|
printf("right-up\n");
|
|
udp_send(server, port, "nav_right-up");
|
|
} else if ((buttons&ALL_BUTTONS_MASK)==BTN_RIGHT_MASK) {
|
|
printf("right\n");
|
|
udp_send(server, port, "nav_right");
|
|
} else if ((buttons&ALL_BUTTONS_MASK)==BTN_UP_MASK) {
|
|
printf("up\n");
|
|
udp_send(server, port, "nav_up");
|
|
} else if ((buttons&ALL_BUTTONS_MASK)==(BTN_LEFT_MASK|BTN_DOWN_MASK)) {
|
|
printf("left-down\n");
|
|
udp_send(server, port, "nav_down");
|
|
} else if ((buttons&ALL_BUTTONS_MASK)==(BTN_RIGHT_MASK|BTN_DOWN_MASK)) {
|
|
printf("left-down\n");
|
|
udp_send(server, port, "nav_down");
|
|
} else if ((buttons&ALL_BUTTONS_MASK)==BTN_DOWN_MASK) {
|
|
printf("down\n");
|
|
udp_send(server, port, "nav_down");
|
|
} else if ((buttons&ALL_BUTTONS_MASK)==BTN_CENTER_MASK) {
|
|
printf("center\n");
|
|
udp_send(server, port, "nav_center");
|
|
|
|
int timeMs = 0;
|
|
while(Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0 && timeMs <= 1000) {
|
|
usleep(100*1000);
|
|
timeMs += 100;
|
|
}
|
|
if (Gpio_ReadPin(Gpio_Pins_BTN_CENTER_BCM)==0) { /* still pressed? */
|
|
printf("exit program\n");
|
|
udp_send(server, port, "exit");
|
|
break; /* break for loop and return */
|
|
}
|
|
}
|
|
usleep(100*1000);
|
|
} /* for */
|
|
return 0;
|
|
}
|
|
|
|
int ROBONAV_Init(void) {
|
|
return 0; /* ok */
|
|
}
|
|
|
|
int ROBONAV_Deinit(void) {
|
|
return 0; /* ok */
|
|
}
|
|
|