Added motors.

bluez/roomba.py

@@ -7,10 +7,14 @@ try:
 except ImportError:
   import gobject as GObject
 import sys
+import time
 
 from dbus.mainloop.glib import DBusGMainLoop
 
 import bluezutils
+import struct
+import keyboard
+
 
 bus = None
 mainloop = None
@@ -25,7 +29,8 @@ GATT_CHRC_IFACE =    'org.bluez.GattCharacteristic1'
 ROOMBA_SVC_UUID =    '35f28386-3070-4f3b-ba38-27507e991760'
 
 ROOMBA_CHR_CTL_UUID =    '35f28386-3070-4f3b-ba38-27507e991762'
-ROOMBA_CHR_SENSORS_UUID = '35f28386-3070-4f3b-ba38-27507e991764'
+ROOMBA_CHR_SENSORS_READ_UUID = '35f28386-3070-4f3b-ba38-27507e991764'
+ROOMBA_CHR_SENSORS_NOTIFY_UUID = '35f28386-3070-4f3b-ba38-27507e991766'
 
 ROOMBA="F4:A4:72:BA:27:C1"
 
@@ -34,14 +39,106 @@ ROOMBA="F4:A4:72:BA:27:C1"
 roomba_service = None
 roomba_ctrl_chrc = None
 roomba_sensors_chrc = None
+roomba_notify_chrc = None
+bt_write_in_progress = False
+bt_write_buffer = None
+bt_write_idx = 0
+bt_write_len = 0
+cur_speed = struct.unpack('h', struct.pack('h', 0))[0]
+angle = struct.unpack('h', struct.pack('h', 0))[0]
+
+def bt_write(L):
+    global bt_write_buffer
+    global bt_write_in_progress
+    global bt_write_idx
+    global bt_write_len
+
+    while (bt_write_in_progress):
+        time.sleep(0.1)
+    bt_write_in_progress = True
+    bt_write_buffer = L
+    bt_write_len = len(L)
+    bt_write_idx = 0
+    roomba_ctrl_chrc[0].WriteValue(bytes(bt_write_buffer[bt_write_idx]), {},  reply_handler=ctrl_write_cb,
+                                      error_handler=generic_error_cb,
+                                      dbus_interface=GATT_CHRC_IFACE)
+    bt_write_idx = 1
+
+
+
+def ctrl_write_cb():
+    global bt_write_buffer
+    global bt_write_in_progress
+    global bt_write_idx
+    global bt_write_len
+    if bt_write_idx < bt_write_len:
+        print("Writing next char %d / %d " % (bt_write_idx, bt_write_len - 1))
+        roomba_ctrl_chrc[0].WriteValue(bytes(bt_write_buffer[bt_write_idx]), {},  reply_handler=ctrl_write_cb,
+                                          error_handler=generic_error_cb,
+                                          dbus_interface=GATT_CHRC_IFACE)
+        bt_write_idx += 1
+    else:
+        bt_write_in_progress = False
+        print("written.")
+        return
 
+def roomba_drive(s, a):
+    print("driving %d %d" % (s, a))
+    si = struct.pack('!h', int(s))
+    if (a == 0):
+        ai = struct.pack('!H', 0x8000)
+    else:
+        ai = struct.pack('!h', int(a))
+    print("integers: %02x %02x %02x %02x" % (si[0], si[1], ai[0], ai[1]))
+    bt_write([[0x44], [si[0]], [si[1]], [ai[0]], [ai[1]], [0xFF]])
+
+
+def roomba_mode(m):
+    bt_write([[0x4d], [m], [0xFF]])
+
+def key_parse(key):
+    print("keypress")
+    global cur_speed
+    global angle
+    print("spd: %d ang: %d" % (cur_speed, angle))
+    if keyboard.is_pressed('w'):
+        cur_speed += 50
+    if keyboard.is_pressed('s'):
+        cur_speed -= 50
+    if keyboard.is_pressed('a'):
+        angle = 200
+    if keyboard.is_pressed('d'):
+        angle = (-200)
+    if keyboard.is_pressed('0'):
+        roomba_mode(0)
+        return
+    if keyboard.is_pressed('1'):
+        roomba_mode(1)
+        return
+    if keyboard.is_pressed('2'):
+        roomba_mode(2)
+        return
+    if keyboard.is_pressed('3'):
+        roomba_mode(3)
+        return
+    if keyboard.is_pressed('4'):
+        roomba_mode(4)
+        return
+    if keyboard.is_pressed('5'):
+        roomba_mode(5)
+        return
+    if keyboard.is_pressed(' '):
+        cur_speed = 0
+        angle = 0
+    cur_speed = int(cur_speed)
+    angle = int(angle)
+
+    roomba_drive(cur_speed, angle)
 
 def generic_error_cb(error):
     print('D-Bus call failed: ' + str(error))
     mainloop.quit()
 
-
-
 def sensor_contact_val_to_str(val):
     if val == 0 or val == 1:
         return 'not supported'
@@ -57,8 +154,11 @@ def mode_val_cb(value):
     print('Mode value: ' + hex(value[0]))
 
 def sensors_val_cb(value):
-    roomba_sensors = value[0] | (value[1] << 8) | (value[2] << 16) | (value[3] << 24)
-    print('Sensors Value : ' + str(hex(roomba_sensors)))
+    print(bytes(value))
+    x = struct.unpack("I11B", bytes(value))
+    for y in x:
+        print('Sensors Values : ' + str(y))
+
 
 def roomba_sensors_start_notify_cb():
     print('Roomba sensors: notification enabled')
@@ -74,10 +174,17 @@ def roomba_sensors_changed_cb(iface, changed_props, invalidated_props):
     value = changed_props.get('Value', None)
     if not value:
         return
+    print("sensor values: changed.\n")
+    roomba_sensors_chrc[0].ReadValue({}, reply_handler=sensors_val_cb,
+                                    error_handler=generic_error_cb,
+                                    dbus_interface=GATT_CHRC_IFACE)
+
+
+
+def sensors_start_notify_cb():
+    print("Sensors notifications: enabled.")
+
 
-    print('New Sensor values')
-    roomba_sensors = value[0] | (value[1] << 8) | (value[2] << 16) | (value[3] << 24)
-    print('\tValue : ' + str(hex(roomba_sensors)))
 
 
 def start_client():
@@ -89,6 +196,14 @@ def start_client():
                                     error_handler=generic_error_cb,
                                     dbus_interface=GATT_CHRC_IFACE)
 
+    roomba_sensors_prop_iface = dbus.Interface(roomba_notify_chrc[0], DBUS_PROP_IFACE)
+    roomba_sensors_prop_iface.connect_to_signal("PropertiesChanged", roomba_sensors_changed_cb)
+
+    roomba_notify_chrc[0].StartNotify(reply_handler=sensors_start_notify_cb,
+                                      error_handler=generic_error_cb,
+                                      dbus_interface=GATT_CHRC_IFACE)
+
+    roomba_mode(0x02)
 
 
 def process_chrc(chrc_path):
@@ -101,9 +216,12 @@ def process_chrc(chrc_path):
     if uuid == ROOMBA_CHR_CTL_UUID:
         global roomba_ctrl_chrc
         roomba_ctrl_chrc = (chrc, chrc_props)
-    elif uuid == ROOMBA_CHR_SENSORS_UUID:
+    elif uuid == ROOMBA_CHR_SENSORS_READ_UUID:
         global roomba_sensors_chrc
         roomba_sensors_chrc = (chrc, chrc_props)
+    elif uuid == ROOMBA_CHR_SENSORS_NOTIFY_UUID:
+        global roomba_notify_chrc
+        roomba_notify_chrc = (chrc, chrc_props)
     else:
         print('Unrecognized characteristic: ' + uuid)
 
@@ -122,8 +240,6 @@ def process_roomba_service(service_path, chrc_paths):
 
     print('roomba GATT service found: ' + service_path)
 
-
-
     global roomba_service
     roomba_service = (service, service_props, service_path)
     
@@ -196,7 +312,17 @@ def main():
         sys.exit(1)
 
     start_client()
-
+    keyboard.on_press_key("w", key_parse)
+    keyboard.on_press_key("a", key_parse)
+    keyboard.on_press_key("s", key_parse)
+    keyboard.on_press_key("d", key_parse)
+    keyboard.on_press_key(" ", key_parse)
+    keyboard.on_press_key("0", key_parse)
+    keyboard.on_press_key("1", key_parse)
+    keyboard.on_press_key("2", key_parse)
+    keyboard.on_press_key("3", key_parse)
+    keyboard.on_press_key("4", key_parse)
+    keyboard.on_press_key("5", key_parse)
     mainloop.run()
 
 

roomba/gatt-srv.c

@@ -208,6 +208,7 @@ uint8_t get_mode(void);
 int switch_mode(int newmode);
 static uint8_t cmd_buf[20];
 static int cmd_buf_off = 0;
+static int cmd_buf_exp_len = 0;
 
 void order66(void);
 
@@ -234,6 +235,28 @@ static void parse_bt_char(uint8_t c)
                 printf("Order 66\n");
                 order66();
                 break;
+            case 'D':
+                {
+                int16_t speed, radius;
+                if (len < 5)
+                    return;
+                memcpy(&speed, cmd_buf + 1, 2);
+                memcpy(&radius, cmd_buf + 3, 2);
+                printf("Drive %hd %hd\n", speed, radius);
+                motors_drive(speed, radius);
+                }
+                break;
+            case 'C':
+                {
+                    if (len < 4)
+                        return;
+                    printf("Clean brushes/vac/sidebrush: %d %d %d\n", cmd_buf[1], cmd_buf[2], cmd_buf[3]);
+                    motors_clean_set(cmd_buf[1], cmd_buf[2], cmd_buf[3]);
+                }
+                break;
+            case 'S':
+                motors_stop();
+                break;
             default:
                 printf("Unknown command %02x\n", cmd_buf[0]);
 
@@ -526,9 +549,5 @@ void *gatt_srv(void *arg)
             update_sensors();
         }
     }
-
-    /* run an event loop for handling the heart rate update events */
-    //event_loop(&_eq);
-
     return 0;
 }

roomba/main.c

@@ -80,7 +80,7 @@ const uint32_t part_len[] = { 5000000, 5000000, 3200000, 2500000, 4200000, 42000
 // 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_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)
@@ -188,7 +188,7 @@ static void roomba_full_to_safe(void)
     mutex_unlock(&roomba_mutex);
 }
 
-static void roomba_cmd(uint8_t cmd, uint8_t *payload, uint8_t len)
+void roomba_cmd(uint8_t cmd, uint8_t *payload, uint8_t len)
 {
     uart_t dev = UART_DEV(0);
     mutex_lock(&roomba_mutex);
@@ -285,6 +285,45 @@ static int cmd_info(int argc, char **argv)
 }
 
 
+int cmd_drive(int argc, char **argv)
+{
+    int i;
+    uint16_t speed, radius;
+    if (argc != 3) {
+        printf("usage: %s <speed radius>\n", argv[0]);
+        return 1;
+    }
+    sscanf(argv[1], "%hd", &speed);
+    sscanf(argv[2], "%hd", &radius);
+    printf("speed: %hd, radius: %hd\n", speed, radius);
+    motors_drive(speed, radius);
+    return 0;
+}
+
+int cmd_stop(int argc, char **argv)
+{
+    (void)argc;
+    (void)argv;
+    motors_stop();
+    return 0;
+}
+
+int cmd_cw(int argc, char **argv)
+{
+    (void)argc;
+    (void)argv;
+    motors_spin(1);
+    return 0;
+}
+
+int cmd_ccw(int argc, char **argv)
+{
+    (void)argc;
+    (void)argv;
+    motors_spin(0);
+    return 0;
+}
+
 int cmd_mode(int argc, char **argv)
 {
     int i;
@@ -459,6 +498,8 @@ void *sensors_loop(void *arg)
             memcpy(&roomba_sensors_data, &tmp, sizeof(struct roomba_sensors_data));
             print_sensors();
             msg.type = MSG_SENSORS_AVAILABLE;
+            if (tmp.drop_caster || tmp.drop_left || tmp.drop_right || tmp.bump_left || tmp.bump_right || tmp.wall)
+                motors_stop();
             msg_send(&msg, gatt_srv_pid);
         }
     }
@@ -486,6 +527,11 @@ static const shell_command_t shell_commands[] = {
     { "mode", "Switch mode", cmd_mode },
     { "play", "play song", cmd_play },
     { "info", "device info", cmd_info },
+    { "d", "drive speed radius", cmd_drive },
+    { "cw", "spin clockwise", cmd_cw },
+    { "ccw", "spin counter-clockwise", cmd_cw },
+    { "ccw", "spin counter-clockwise", cmd_ccw },
+    { "s", "stop", cmd_stop },
     { NULL, NULL, NULL }
 };
 

roomba/motors.c

@@ -0,0 +1,48 @@
+#include <stdio.h>
+#include <stdint.h>
+
+#include "assert.h"
+#include "periph/gpio.h"
+#include "timex.h"
+#include "roomba.h"
+#include "board.h"
+
+#define RADIUS_STRAIGHT 0x8000
+
+
+void motors_drive(int16_t speed, int16_t radius)
+{
+    uint8_t data[4];
+    if ((speed > 500) || (speed < (0 - 500)) || (radius > 2000) || (radius < (0 - 2000)))
+        motors_stop();
+    data[0] = ((uint16_t)speed) & 0xFF;
+    data[1] = (((uint16_t)speed) & 0xFF00) >> 8;
+    if ((radius == 0) && (speed != 0))
+        radius = RADIUS_STRAIGHT;
+    data[2] = ((uint16_t)radius) & 0xFF;
+    data[3] = (((uint16_t)radius) & 0xFF00) >> 8;
+    roomba_cmd(ROOMBA_DRIVE, data, 4);
+}
+
+void motors_stop(void)
+{
+    motors_drive(0, 0);
+    motors_clean_set(0,0,0);
+}
+
+void motors_spin(int cw)
+{
+    if(cw)
+        motors_drive(10, -1);
+    else
+        motors_drive(10, 1);
+}
+
+
+void motors_clean_set(int brush, int vac, int side)
+{
+    uint8_t data;
+    data = ((!!brush) << 2) | ((!!vac) << 1) | (!!side);
+    roomba_cmd(ROOMBA_MOTORS, &data, 1);
+}
+

roomba/roomba.h

@@ -74,8 +74,18 @@ struct __attribute__((packed)) roomba_sensors_data {
     uint8_t ovc;
 };
 
+void roomba_cmd(uint8_t cmd, uint8_t *payload, uint8_t len);
+
+
+/* Sensors */
 struct roomba_sensors_data *roomba_sensors(void);
 
+/* Motors */
+void motors_clean_set(int brush, int vac, int side);
+void motors_spin(int cw);
+void motors_stop(void);
+void motors_drive(int16_t speed, int16_t radius);
+
 
 #endif