danielinux/gadget-roomba-nano33-ble
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity
gadget-roomba-nano33-ble/roomba/main.c
Daniele Lacamera 897e6f3cc7 Roomba modes + sensors + ble notification service
2020-12-25 00:52:58 +01:00

521 lines
15 KiB
C
Raw Blame History

/* main.c
*
*
* this is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* this is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA
*/
#include <stdio.h>
#include "board.h"
#include "net/ipv6/addr.h"
#include "net/gnrc.h"
#include "net/gnrc/netif.h"
#include "shell.h"
#include "periph/flashpage.h"
#include "thread.h"
#include "periph/uart.h"
#include "periph/gpio.h"
#include "xtimer.h"
#include "ringbuffer.h"
#include "roomba.h"
#include "song.h"
#include "mutex.h"
#include "msg.h"
#ifndef SHELL_BUFSIZE
#define SHELL_BUFSIZE (128U)
#endif
#define DISPATCHER_PRIO (THREAD_PRIORITY_MAIN - 1)
#define BT_PRIO (THREAD_PRIORITY_MAIN - 1)
#define MSGQ_SIZE (4)
#define GPIO_WAKEUP GPIO_PIN(1,11)
#define SENSOR_BUFFER_SIZE (sizeof(struct sensors_code1) + sizeof(struct sensors_code2) + \
sizeof(struct sensors_code3))
#define MSG_SENSORS_OFF (0x4000)
#define MSG_SENSORS_ON (0x4100)
#define MSG_SENSORS_AVAILABLE (0x4200)
#define MSG_SENSORS_TIMEOUT (0x4300)
const char roomba_modstring[6][6] = {
"OFF", "DRIVE", "FULL", "CLEAN", "SPOT", "DOCK"
};
extern const uint16_t song0[], song1[], song2[], song3[], song4[], song5[];
// Types
typedef struct {
char rx_mem[SENSOR_BUFFER_SIZE];
ringbuffer_t rx_buf;
} uart_ctx_t;
// Globals
//
static mutex_t roomba_mutex = MUTEX_INIT;
static uart_ctx_t ctx[UART_NUMOF];
static kernel_pid_t order66_pid, sensors_pid, gatt_srv_pid;
static char order66_stack[THREAD_STACKSIZE_MAIN];
static char sensors_stack[THREAD_STACKSIZE_MAIN];
static char gatt_srv_stack[THREAD_STACKSIZE_MAIN];
static msg_t sensors_msgq[MSGQ_SIZE];
static msg_t gatt_srv_msgq[MSGQ_SIZE];
static xtimer_t sensors_msg_timer;
const uint16_t *songs[] = { song0, song1, song2, song3, song4, song5 };
const uint32_t part_len[] = { 5000000, 5000000, 3200000, 2500000, 4200000, 4200000 };
// Prototypes
void *order66(void *arg);
void *sensors_loop(void *arg);
static void roomba_cmd(uint8_t cmd, uint8_t *payload, uint8_t len);
void roomba_led(uint8_t status, uint8_t spot, uint8_t clean, uint8_t max, uint8_t power_c, uint8_t power_i);
int song_len(const uint16_t *s)
{
int l = (s[0] & 0xFF00) >> 8;
return l;
}
uint32_t load_song(int n)
{
uint8_t cmd = ROOMBA_SONG;
int l = song_len(songs[n]);
int i;
mutex_lock(&roomba_mutex);
uart_t dev = UART_DEV(0);
uart_write(dev, &cmd, 1);
uart_write(dev, (const uint8_t *)songs[n], 2 + (l << 1));
mutex_unlock(&roomba_mutex);
xtimer_usleep(100000);
return part_len[n];
}
// Order 66
void *order66(void *arg)
{
uint8_t x = 0;
uint32_t st;
st = load_song(0);
roomba_cmd(ROOMBA_PLAY, &x, 1);
xtimer_usleep(st);
st = load_song(1);
xtimer_usleep(20000);
roomba_cmd(ROOMBA_PLAY, &x, 1);
xtimer_usleep(st);
st = load_song(2);
xtimer_usleep(20000);
roomba_cmd(ROOMBA_PLAY, &x, 1);
xtimer_usleep(st);
st = load_song(3);
xtimer_usleep(20000);
roomba_cmd(ROOMBA_PLAY, &x, 1);
xtimer_usleep(st);
st = load_song(4);
xtimer_usleep(20000);
roomba_cmd(ROOMBA_PLAY, &x, 1);
xtimer_usleep(st);
st = load_song(2);
xtimer_usleep(20000);
roomba_cmd(ROOMBA_PLAY, &x, 1);
xtimer_usleep(st);
st = load_song(3);
xtimer_usleep(20000);
roomba_cmd(ROOMBA_PLAY, &x, 1);
xtimer_usleep(st);
st = load_song(5);
xtimer_usleep(20000);
roomba_cmd(ROOMBA_PLAY, &x, 1);
xtimer_usleep(st);
return NULL;
}
static int roomba_mode = ROOMBA_MODE_OFF;
static void roomba_stop(void)
{
uint8_t cmd = ROOMBA_POWER;
uart_t dev = UART_DEV(0);
mutex_lock(&roomba_mutex);
uart_write(dev, &cmd, 1);
mutex_unlock(&roomba_mutex);
xtimer_usleep(200000);
}
static void roomba_start(void)
{
gpio_clear(GPIO_WAKEUP);
xtimer_usleep(200000);
gpio_set(GPIO_WAKEUP);
uint8_t cmd = ROOMBA_START;
uart_t dev = UART_DEV(0);
mutex_lock(&roomba_mutex);
uart_write(dev, &cmd, 1);
xtimer_usleep(20000);
mutex_unlock(&roomba_mutex);
}
static void roomba_safe_to_full(void)
{
uint8_t cmd = ROOMBA_FULL;
uart_t dev = UART_DEV(0);
mutex_lock(&roomba_mutex);
uart_write(dev, &cmd, 1);
xtimer_usleep(20000);
mutex_unlock(&roomba_mutex);
}
static void roomba_full_to_safe(void)
{
uint8_t cmd = ROOMBA_SAFE;
uart_t dev = UART_DEV(0);
mutex_lock(&roomba_mutex);
uart_write(dev, &cmd, 1);
xtimer_usleep(20000);
mutex_unlock(&roomba_mutex);
}
static void roomba_cmd(uint8_t cmd, uint8_t *payload, uint8_t len)
{
uart_t dev = UART_DEV(0);
mutex_lock(&roomba_mutex);
uart_write(dev, &cmd, 1);
if(len && payload)
uart_write(dev, payload, len);
mutex_unlock(&roomba_mutex);
}
int switch_mode(int newmode)
{
msg_t msg;
int oldmode = roomba_mode;
if (roomba_mode != newmode) {
if (roomba_mode == ROOMBA_MODE_OFF) {
roomba_start();
roomba_mode = ROOMBA_MODE_DRIVE;
roomba_led(0x03, 0, 0, 0, 0x80, 0x80);
}
switch(newmode) {
case ROOMBA_MODE_OFF:
roomba_stop();
roomba_led(0x00, 0, 0, 0, 0xF0, 0x20);
break;
case ROOMBA_MODE_DRIVE:
if (roomba_mode == newmode)
break;
else if (roomba_mode == ROOMBA_MODE_FULL)
roomba_full_to_safe();
else {
roomba_stop();
roomba_start();
}
roomba_led(0x03, 0, 0, 0, 0x80, 0x80);
break;
case ROOMBA_MODE_FULL:
if (roomba_mode == ROOMBA_MODE_DRIVE)
roomba_safe_to_full();
else {
roomba_stop();
roomba_start();
roomba_safe_to_full();
}
roomba_led(0x01, 0, 0, 0, 0xF0, 0xFF);
break;
case ROOMBA_MODE_CLEAN:
roomba_cmd(ROOMBA_CLEAN, NULL, 0);
break;
case ROOMBA_MODE_SPOT:
roomba_cmd(ROOMBA_SPOT, NULL, 0);
break;
case ROOMBA_MODE_DOCK:
roomba_cmd(ROOMBA_DOCK, NULL, 0);
break;
case 66:
order66_pid = thread_create(order66_stack, sizeof(order66_stack),
DISPATCHER_PRIO, 0, order66, NULL, "ORDER66");
newmode = roomba_mode;
break;
}
printf("Mode: %s\n", roomba_modstring[newmode]);
roomba_mode = newmode;
if (roomba_mode == ROOMBA_MODE_FULL) {
msg.type = MSG_SENSORS_ON;
msg_send(&msg, sensors_pid);
}
if (oldmode == ROOMBA_MODE_FULL) {
msg.type = MSG_SENSORS_OFF;
msg_send(&msg, sensors_pid);
}
}
return 0;
}
uint8_t get_mode(void)
{
return roomba_mode;
}
static int cmd_info(int argc, char **argv)
{
(void)argc;
(void)argv;
puts("iRobot ROOMBA 500 powered by RIOT-OS");
printf("You are running RIOT on %s.\n", RIOT_BOARD);
printf("This board features a %s MCU.\n", RIOT_MCU);
puts("\nUART INFO:");
printf("Available devices: %i\n", UART_NUMOF);
if (STDIO_UART_DEV != UART_UNDEF) {
printf("UART used for STDIO (the shell): UART_DEV(%i)\n\n", STDIO_UART_DEV);
}
return 0;
}
int cmd_mode(int argc, char **argv)
{
int i;
if (argc < 2) {
printf("usage: %s <data (string)>\n", argv[0]);
return 1;
}
for (i = 0; i < 6; i++) {
if (strcmp(argv[1], roomba_modstring[i]) == 0) {
switch_mode(i);
return 0;
}
}
if (strcmp(argv[1], "ORDER66") == 0) {
order66_pid = thread_create(order66_stack, sizeof(order66_stack),
DISPATCHER_PRIO, 0, order66, NULL, "ORDER66");
return 0;
}
printf("Mode '%s' unknown\r\n", argv[1]);
return 1;
}
static int cmd_play(int argc, char **argv)
{
uint8_t x = 0, z = 0;
if ((argc != 2) || (strlen(argv[1]) != 1))
{
printf("usage: %s <song number>\n", argv[0]);
return 1;
}
x = argv[1][0] - '0';
load_song(x);
roomba_cmd(ROOMBA_PLAY, &z, 1);
return 0;
}
#define ALLSENSORS_CODE (0)
#define ALLSENSORS_SIZE (26)
#define QUICKSENSORS_CODE (1)
#define QUICKSENSORS_SIZE (10)
static volatile uint8_t sensors_expect_rx = 0;
static volatile uint8_t sensors_data_rx = 0;
static uint8_t raw_sensors_data[ALLSENSORS_SIZE];
static void invalidate_sensors(void)
{
msg_t msg;
memset(&raw_sensors_data, 0, sizeof(struct roomba_sensors_data));
msg.type = MSG_SENSORS_AVAILABLE;
msg_send(&msg, gatt_srv_pid);
}
static int read_sensors(uint8_t mode)
{
uint8_t cmd[2] = {ROOMBA_SENSORS, 0};
int len = -1;
msg_t msg;
uart_t dev = UART_DEV(0);
msg_t msg_timeout;
msg_timeout.type = MSG_SENSORS_TIMEOUT;
if (mode == ALLSENSORS_CODE)
sensors_expect_rx = ALLSENSORS_SIZE;
else
sensors_expect_rx = QUICKSENSORS_SIZE;
cmd[1] = mode;
mutex_lock(&roomba_mutex);
sensors_data_rx = 0;
uart_write(dev, cmd, 1);
uart_write(dev, cmd + 1, 1);
xtimer_set_msg(&sensors_msg_timer, 500000, &msg_timeout, sensors_pid);
if (msg_receive(&msg) >= 0) {
if (msg.type == MSG_SENSORS_AVAILABLE) {
len = sensors_expect_rx;
} else if (msg.type == MSG_SENSORS_TIMEOUT) {
printf("Timeout while reading sensors\n");
invalidate_sensors();
} else if (msg.type == MSG_SENSORS_OFF) {
printf("Paused sensor task.\n");
invalidate_sensors();
} else {
printf("Invalid message type %04x\n", msg.type);
invalidate_sensors();
}
sensors_expect_rx = 0;
}
mutex_unlock(&roomba_mutex);
return len;
}
// Rx callback
static void rx_cb(void *arg, uint8_t data)
{
uart_t dev = (uart_t)arg;
msg_t msg;
//printf("RX: %02x\n", data);
if (sensors_expect_rx > sensors_data_rx)
raw_sensors_data[sensors_data_rx++] = data;
if (sensors_expect_rx == sensors_data_rx)
{
msg.type = MSG_SENSORS_AVAILABLE;
msg_send(&msg, sensors_pid);
}
}
static struct roomba_sensors_data roomba_sensors_data = { };
void print_sensors(void)
{
struct roomba_sensors_data *r = &roomba_sensors_data;
printf("=== Sensors ===\n");
printf("Valid : %s\n", r->valid?"yes":"no");
if (r->valid) {
printf("Drops : C:%d L:%d R:%d\n", r->drop_caster, r->drop_left, r->drop_right);
printf("Bumps : L: %d R: %d\n", r->bump_left, r->bump_right);
printf("Cliffs: FL:%d FR:%d \n", r->cliff_frontleft, r->cliff_frontright);
printf(" L:%d R:%d \n", r->cliff_left, r->cliff_right);
printf("OVC: %02x\n", r->ovc);
}
printf("==============\n\n");
}
void *sensors_loop(void *arg)
{
int ret = 0;
msg_t msg;
struct roomba_sensors_data tmp;
while (1) {
if (get_mode() != ROOMBA_MODE_FULL) {
while (1) {
if ((msg_receive(&msg) >= 0) && (msg.type == MSG_SENSORS_ON)) {
printf("Sensors thread: resume.\n");
break;
}
}
}
xtimer_usleep(250000);
ret = read_sensors(ALLSENSORS_CODE);
if (ret < 0) {
xtimer_usleep(1000000);
} else {
/* Byte 0: Wheeldrops + Bumps */
tmp.drop_caster = !!(raw_sensors_data[0] & (1 << SENSE_DROP_CASTER));
tmp.drop_left = !!(raw_sensors_data[0] & (1 << SENSE_DROP_LEFT));
tmp.drop_right = !!(raw_sensors_data[0] & (1 << SENSE_DROP_RIGHT));
tmp.bump_left = !!(raw_sensors_data[0] & (1 << SENSE_BUMP_LEFT));
tmp.bump_right = !!(raw_sensors_data[0] & (1 << SENSE_BUMP_RIGHT));
/* Byte 1: Wall */
tmp.wall = raw_sensors_data[1];
/* Byte 2: Cliff L */
tmp.cliff_left = raw_sensors_data[2];
/* Byte 3: Cliff FL */
tmp.cliff_frontleft = raw_sensors_data[3];
/* Byte 4: Cliff FR */
tmp.cliff_frontright = raw_sensors_data[4];
/* Byte 5: Cliff R */
tmp.cliff_right = raw_sensors_data[5];
/* Byte 7: Overcurrents */
tmp.ovc = raw_sensors_data[7];
/* 'valid' flag */
tmp.valid = 1;
}
if (memcmp(&roomba_sensors_data, &tmp, sizeof(struct roomba_sensors_data)) != 0) {
msg_t msg;
memcpy(&roomba_sensors_data, &tmp, sizeof(struct roomba_sensors_data));
print_sensors();
msg.type = MSG_SENSORS_AVAILABLE;
msg_send(&msg, gatt_srv_pid);
}
}
}
struct roomba_sensors_data *roomba_sensors(void)
{
return &roomba_sensors_data;
}
void roomba_led(uint8_t status, uint8_t spot, uint8_t clean, uint8_t max, uint8_t power_c, uint8_t power_i)
{
uint8_t led_bytes[3];
led_bytes[0] = ((status & 0x03) << 4) |
((spot & 0x01) << 3) |
((clean & 0x01) << 2) |
((max &0x01) << 1);
led_bytes[1] = power_c;
led_bytes[2] = power_i;
roomba_cmd(ROOMBA_LEDS, led_bytes, 3);
}
void *gatt_srv(void*);
static const shell_command_t shell_commands[] = {
{ "mode", "Switch mode", cmd_mode },
{ "play", "play song", cmd_play },
{ "info", "device info", cmd_info },
{ NULL, NULL, NULL }
};
int main(void)
{
/* initialize UART */
char line_buf[SHELL_BUFSIZE];
uart_init(UART_DEV(0), 115200, rx_cb, (void *)0);
/* initialize ringbuffers */
for (unsigned i = 0; i < UART_NUMOF; i++) {
ringbuffer_init(&(ctx[i].rx_buf), ctx[i].rx_mem, SENSOR_BUFFER_SIZE);
}
/* initialize GPIO WAKEUP */
gpio_init(GPIO_WAKEUP, GPIO_OUT);
gpio_set(GPIO_WAKEUP);
/* Gatt Server */
gatt_srv_pid = thread_create(gatt_srv_stack, sizeof(gatt_srv_stack),
DISPATCHER_PRIO - 1, 0, gatt_srv, NULL, "BLE_gatt");
/* Create Sensors thread */
msg_init_queue(sensors_msgq, MSGQ_SIZE);
sensors_pid = thread_create(sensors_stack, sizeof(sensors_stack),
DISPATCHER_PRIO, 0, sensors_loop, NULL, "sensors");
/* run the shell */
shell_run(shell_commands, line_buf, SHELL_BUFSIZE);
return 0;
}
Reference in a new issue View git blame Copy permalink