123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521 |
- /* 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;
- }
-
|