Skip to content

Commit

Permalink
feat: some roomba drive efforts
Browse files Browse the repository at this point in the history
  • Loading branch information
@davidecavestro
davidecavestro committed Sep 11, 2022
1 parent 410d864 commit 92659a9
Show file tree
Hide file tree
Showing 2 changed files with 153 additions and 40 deletions.
168 changes: 135 additions & 33 deletions ESPHomeRoombaComponent.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,42 +4,52 @@

class RoombaComponent : public UARTDevice, public CustomAPIDevice, public PollingComponent {
public:
//Sensor *distanceSensor;
// Sensor *distanceSensor;
Sensor *voltageSensor;
Sensor *currentSensor;
Sensor *batteryChargeSensor;
Sensor *batteryCapacitySensor;
Sensor *batteryPercentSensor;
Sensor *batteryTemperatureSensor;
TextSensor *chargingSensor;
TextSensor *activitySensor;
Sensor *driveSpeedSensor;
TextSensor *oiModeSensor;

static RoombaComponent* instance(uint8_t brcPin, UARTComponent *parent, uint32_t updateInterval) {
static RoombaComponent* INSTANCE = new RoombaComponent(brcPin, parent, updateInterval);
static RoombaComponent* instance(uint8_t brcPin, UARTComponent *parent, uint32_t updateInterval, bool lazy650Enabled) {
static RoombaComponent* INSTANCE = new RoombaComponent(brcPin, parent, updateInterval, lazy650Enabled);
return INSTANCE;
}

void setup() override {
// High-impedence on the BRC_PIN
// see https://github.com/johnboiles/esp-roomba-mqtt/commit/fa9af14376f740f366a9ecf4cb59dec2419deeb0#diff-34d21af3c614ea3cee120df276c9c4ae95053830d7f1d3deaf009a4625409ad2R140
pinMode(this->brcPin, INPUT);
if (this->lazy650Enabled) {
// High-impedence on the BRC_PIN
// see https://github.com/johnboiles/esp-roomba-mqtt/commit/fa9af14376f740f366a9ecf4cb59dec2419deeb0#diff-34d21af3c614ea3cee120df276c9c4ae95053830d7f1d3deaf009a4625409ad2R140
pinMode(this->brcPin, INPUT);
} else {
pinMode(this->brcPin, OUTPUT);
digitalWrite(this->brcPin, HIGH);
}

register_service(&RoombaComponent::on_command, "command", {"command"});
}

void update() override {
long now = millis();
// Wakeup the roomba at fixed intervals
if (now - lastWakeupTime > 50000) {
ESP_LOGD("custom", "Time to wakeup");
lastWakeupTime = now;
if (!wasCleaning) {
if (wasDocked) {
wake_on_dock();
if (this->lazy650Enabled) {
long now = millis();
// Wakeup the roomba at fixed intervals
if (now - lastWakeupTime > 50000) {
ESP_LOGD("custom", "Time to wakeup");
lastWakeupTime = now;
if (!wasCleaning) {
if (wasDocked) {
wake_on_dock();
} else {
brc_wakeup();
}
} else {
brc_wakeup();
}
} else {
brc_wakeup();
}
}

Expand All @@ -48,6 +58,7 @@ class RoombaComponent : public UARTDevice, public CustomAPIDevice, public Pollin
int16_t current;
uint16_t batteryCharge;
uint16_t batteryCapacity;
int16_t batteryTemperature;

flush();

Expand All @@ -56,9 +67,13 @@ class RoombaComponent : public UARTDevice, public CustomAPIDevice, public Pollin
SensorVoltage,
SensorCurrent,
SensorBatteryCharge,
SensorBatteryCapacity
SensorBatteryCapacity,
SensorBatteryTemperature,
SensorDistance,
SensorOIMode
};
uint8_t values[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};

uint8_t values[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

bool success = getSensorsList(sensors, sizeof(sensors), values, sizeof(values));
if (!success) {
Expand All @@ -71,8 +86,10 @@ class RoombaComponent : public UARTDevice, public CustomAPIDevice, public Pollin
current = values[3] * 256 + values[4];
batteryCharge = values[5] * 256 + values[6];
batteryCapacity = values[7] * 256 + values[8];

std::string activity = this->get_activity(charging, current);
batteryTemperature = values[9];
std::string oiMode = get_oimode(values[10]);

std::string activity = get_activity(charging, current);
wasCleaning = activity == "Cleaning";
wasDocked = activity == "Docked";

Expand Down Expand Up @@ -101,6 +118,10 @@ class RoombaComponent : public UARTDevice, public CustomAPIDevice, public Pollin
this->batteryPercentSensor->publish_state(battery_level);
}

if (this->batteryTemperatureSensor->state != batteryTemperature) {
this->batteryTemperatureSensor->publish_state(batteryTemperature);
}

if (this->chargingState != charging) {
this->chargingState = charging;
this->chargingSensor->publish_state(ToString(charging));
Expand All @@ -109,6 +130,14 @@ class RoombaComponent : public UARTDevice, public CustomAPIDevice, public Pollin
if (activity.compare(this->activitySensor->state) != 0) {
this->activitySensor->publish_state(activity);
}

if (this->driveSpeedSensor->state != this->speed) {
this->driveSpeedSensor->publish_state(this->speed);
}

if (oiMode.compare(this->oiModeSensor->state) != 0) {
this->oiModeSensor->publish_state(oiMode);
}
}

private:
Expand All @@ -117,17 +146,25 @@ class RoombaComponent : public UARTDevice, public CustomAPIDevice, public Pollin
int lastWakeupTime = 0;
bool wasCleaning = false;
bool wasDocked = false;

RoombaComponent(uint8_t brcPin, UARTComponent *parent, uint32_t updateInterval) : UARTDevice(parent), PollingComponent(updateInterval) {
int16_t speed = 0;

bool lazy650Enabled = false;

RoombaComponent(uint8_t brcPin, UARTComponent *parent, uint32_t updateInterval, bool lazy650Enabled) : UARTDevice(parent), PollingComponent(updateInterval) {
this->brcPin = brcPin;
this->lazy650Enabled = lazy650Enabled;
this->voltageSensor = new Sensor();
this->currentSensor = new Sensor();
this->batteryChargeSensor = new Sensor();
this->batteryCapacitySensor = new Sensor();
this->batteryPercentSensor = new Sensor();
this->batteryTemperatureSensor = new Sensor();

this->chargingSensor = new TextSensor();
this->activitySensor = new TextSensor();

this->driveSpeedSensor = new Sensor();
this->oiModeSensor = new TextSensor();
}

typedef enum {
Expand Down Expand Up @@ -186,13 +223,20 @@ class RoombaComponent : public UARTDevice, public CustomAPIDevice, public Pollin
} ChargeState;

void brc_wakeup() {
ESP_LOGD("custom", "brc_wakeup");
pinMode(this->brcPin, OUTPUT);
digitalWrite(this->brcPin, LOW);
delay(200);
pinMode(this->brcPin, OUTPUT);
delay(200);
start_oi(); // Start
if (this->lazy650Enabled) {
ESP_LOGD("custom", "brc_wakeup");
pinMode(this->brcPin, OUTPUT);
digitalWrite(this->brcPin, LOW);
delay(200);
pinMode(this->brcPin, OUTPUT);
delay(200);
start_oi(); // Start
} else {
digitalWrite(this->brcPin, LOW);
delay(1000);
digitalWrite(this->brcPin, HIGH);
delay(100);
}
}

void on_command(std::string command) {
Expand All @@ -209,16 +253,39 @@ class RoombaComponent : public UARTDevice, public CustomAPIDevice, public Pollin
spot();
}
else if (command == "wakeup") {
this->brc_wakeup();
brc_wakeup();
}
else if (command == "wake_om_dock") {
this->wake_on_dock();
wake_on_dock();
}
else if (command == "sleep") {
sleep();
} else {
ESP_LOGD("custom", "unknown command " + command);
}
else if (command == "go_max") {
ESP_LOGI("roomba", "go max");
alter_speed(1000);
} else if (command == "go_faster") {
ESP_LOGI("roomba", "go faster");
alter_speed(100);
} else if (command == "go_slower") {
ESP_LOGI("roomba", "slow it down");
alter_speed(-100);
} else if (command == "go_right") {
ESP_LOGI("roomba", "easy right");
drive(speed, -250);
} else if (command == "go_left") {
ESP_LOGI("roomba", "easy left");
drive(this->speed, 250);
} else if (command == "halt") {
ESP_LOGI("roomba", "HALT");
this->speed = 0;
drive(0,0);
} else if (command == "drive") {
ESP_LOGI("roomba", "DRIVE mode");
this->speed = 0;
safeMode();
}
ESP_LOGI("roomba", "ACK %s", command.c_str());
}

void start_oi() {
Expand All @@ -241,6 +308,41 @@ class RoombaComponent : public UARTDevice, public CustomAPIDevice, public Pollin
write(143); // Dock
}

void alter_speed(int16_t step) {
int16_t speed = this->speed + step;

if (speed > 500)
speed = 500;
else if (speed < -500)
speed = -500;

this->speed = speed;
this->driveSpeedSensor->publish_state(speed);
this->drive(this->speed, 0);
}

void drive(int16_t velocity, int16_t radius) {
write(137);
write((velocity & 0xff00) >> 8);
write(velocity & 0xff);
write((radius & 0xff00) >> 8);
write(radius & 0xff);
}

void safeMode() {
write(131);
}

std::string get_oimode(uint8_t mode) {
switch(mode) {
case 0: return "off";
case 1: return "passive";
case 2: return "safe";
case 3: return "full";
default: return "unknown";
}
}

void cover() {
write(135);
}
Expand Down
25 changes: 18 additions & 7 deletions example/roomba.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,13 +7,13 @@ esphome:

substitutions:
name: "roomba"
# BRC pin, uart_bus, polling interval in milliseconds
init: 'RoombaComponent::instance(D5, id(uart_bus), 10000);'
# BRC pin, uart_bus, polling interval in milliseconds, enable hack for lazy 650 wakeup
init: 'RoombaComponent::instance(D5, id(uart_bus), 10000, true);'

uart:
id: uart_bus
tx_pin: D1
rx_pin: D2
tx_pin: TX
rx_pin: RX
baud_rate: 115200

# Enable logging
Expand Down Expand Up @@ -52,7 +52,7 @@ sensor:
- platform: custom
lambda: |-
auto r = ${init}
return {r->voltageSensor, r->currentSensor, r->batteryChargeSensor, r->batteryCapacitySensor, r->batteryPercentSensor};
return {r->voltageSensor, r->currentSensor, r->batteryChargeSensor, r->batteryCapacitySensor, r->batteryPercentSensor, r->batteryTemperatureSensor, r->driveSpeedSensor};
sensors:
- name: "${name} voltage"
unit_of_measurement: "V"
Expand All @@ -69,12 +69,23 @@ sensor:
- name: "${name} battery"
unit_of_measurement: "%"
accuracy_decimals: 0
- name: "${name} temp"
unit_of_measurement: "°C"
accuracy_decimals: 0
- name: "${name} drive speed"
unit_of_measurement: "mm/s"
accuracy_decimals: 0

text_sensor:
- platform: custom
lambda: |-
auto r = ${init}
return {r-> chargingSensor, r->activitySensor};
return {r->chargingSensor, r->activitySensor, r->oiModeSensor};
text_sensors:
- name: "${name} charging state"
- name: "${name} activity"
- name: "${name} activity"
- name: "${name} OI mode"

switch:
- platform: safe_mode
name: "Vacuum Restart (Safe Mode)"

0 comments on commit 92659a9

Please sign in to comment.