⚗️ (spike): Add autocharge of robot

seal strategy

spikes/CMakeLists.txt

@@ -4,6 +4,7 @@
 
 add_subdirectory(${SPIKES_DIR}/lk_activity_kit)
 add_subdirectory(${SPIKES_DIR}/lk_audio)
+add_subdirectory(${SPIKES_DIR}/lk_auto_charge)
 add_subdirectory(${SPIKES_DIR}/lk_behavior_kit)
 add_subdirectory(${SPIKES_DIR}/lk_ble)
 add_subdirectory(${SPIKES_DIR}/lk_bluetooth)
@@ -50,6 +51,7 @@ add_subdirectory(${SPIKES_DIR}/stl_cxxsupport)
 add_custom_target(spikes_leka)
 add_dependencies(spikes_leka
 	spike_lk_activity_kit
+	spike_lk_auto_charge
 	spike_lk_ble
 	spike_lk_bluetooth
 	spike_lk_cg_animations

spikes/lk_auto_charge/CMakeLists.txt

@@ -0,0 +1,30 @@
+# Leka - LekaOS
+# Copyright 2023 APF France handicap
+# SPDX-License-Identifier: Apache-2.0
+
+add_mbed_executable(spike_lk_auto_charge)
+
+target_include_directories(spike_lk_auto_charge
+	PRIVATE
+	.
+)
+
+target_sources(spike_lk_auto_charge
+	PRIVATE
+	main.cpp
+	SealStrategy.cpp
+)
+
+target_link_libraries(spike_lk_auto_charge
+	EventLoopKit
+	CoreBattery
+	BatteryKit
+	CorePwm
+	CoreMotor
+	CoreIMU
+	CoreI2C
+	IMUKit
+	BLEKit
+)
+
+target_link_custom_leka_targets(spike_lk_auto_charge)

spikes/lk_auto_charge/SealStrategy.cpp

@@ -0,0 +1,108 @@
+// Leka - LekaOS
+// Copyright 2023 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#include "SealStrategy.h"
+
+#include "rtos/ThisThread.h"
+
+#include "MathUtils.h"
+
+using namespace leka;
+using namespace std::chrono;
+
+void SealStrategy::init()
+{
+	_event_loop.registerCallback([this] { run(); });
+}
+
+auto SealStrategy::sealConvertToPwmFrom(float angle) const -> float
+{
+	auto res = utils::math::map(angle, kMinAngleInput, kMaxAngleInput, kMinPwmOutput, kMaxPwmOutput);
+	return res;
+}
+
+void SealStrategy::start()
+{
+	should_stop = false;
+	_event_loop.start();
+}
+
+void SealStrategy::stop()
+{
+	should_stop = true;
+	_event_loop.stop();
+	rtos::ThisThread::sleep_for(100ms);
+	stopMotors();
+}
+
+void SealStrategy::run()
+{
+	if (should_stop || _battery.isCharging()) {
+		stopMotors();
+		return;
+	}
+
+	auto angles				 = _imukit.getEulerAngles();
+	auto is_right_tilted	 = angles.roll > 0;
+	auto should_move_forward = angles.pitch > 0;
+
+	if (abs(angles.roll) > kRollTolerance) {
+		auto speed_offset = sealConvertToPwmFrom(angles.roll > 0 ? angles.roll : -angles.roll);
+		if (is_right_tilted) {
+			if (should_move_forward) {
+				spinRight(kMinPwmOutput, kMinPwmOutput + speed_offset);
+			} else {
+				spinLeft(kMinPwmOutput, kMinPwmOutput + speed_offset);
+			}
+		} else {
+			if (should_move_forward) {
+				spinLeft(kMinPwmOutput + speed_offset, kMinPwmOutput);
+			} else {
+				spinRight(kMinPwmOutput + speed_offset, kMinPwmOutput);
+			}
+		}
+	} else if (abs(angles.pitch) > kPitchTolerance) {
+		auto speed = sealConvertToPwmFrom(angles.pitch > 0 ? angles.pitch : -angles.pitch);
+		if (should_move_forward) {
+			moveForward(speed);
+		} else {
+			moveBackward(speed);
+		}
+	} else {
+		stopMotors();
+	}
+
+	rtos::ThisThread::sleep_for(10ms);
+	_event_loop.start();
+}
+
+void SealStrategy::stopMotors()
+{
+	_motor_right.stop();
+	_motor_left.stop();
+}
+
+void SealStrategy::spinLeft(float left_speed, float right_speed)
+{
+	_motor_left.spin(Rotation::clockwise, left_speed);
+	_motor_right.spin(Rotation::clockwise, right_speed);
+}
+
+void SealStrategy::spinRight(float left_speed, float right_speed)
+{
+	_motor_left.spin(Rotation::counterClockwise, left_speed);
+	_motor_right.spin(Rotation::counterClockwise, right_speed);
+}
+
+void SealStrategy::moveForward(float speed)
+{
+	_motor_left.spin(Rotation::counterClockwise, speed);
+	_motor_right.spin(Rotation::clockwise, speed);
+}
+
+void SealStrategy::moveBackward(float speed)
+{
+	_motor_left.spin(Rotation::clockwise, speed);
+	_motor_right.spin(Rotation::counterClockwise, speed);
+}

spikes/lk_auto_charge/SealStrategy.h

@@ -0,0 +1,62 @@
+// Leka - LekaOS
+// Copyright 2023 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#pragma once
+
+#include "interface/drivers/Battery.h"
+#include "interface/drivers/Motor.h"
+#include "interface/libs/EventLoop.h"
+#include "interface/libs/IMUKit.hpp"
+
+namespace leka {
+
+class SealStrategy
+{
+  public:
+	SealStrategy(interface::EventLoop &event_loop, interface::Battery &battery, interface::Motor &motor_left,
+				 interface::Motor &motor_right, interface::IMUKit &imu_kit)
+		: _event_loop(event_loop),
+		  _battery(battery),
+		  _motor_left(motor_left),
+		  _motor_right(motor_right),
+		  _imukit(imu_kit)
+	{
+	}
+
+	~SealStrategy() = default;
+
+	void init();
+	void start();
+	void stop();
+
+  private:
+	[[nodiscard]] auto sealConvertToPwmFrom(float angle) const -> float;
+
+	void run();
+
+	void stopMotors();
+	void spinRight(float left_speed, float right_speed);
+	void spinLeft(float left_speed, float right_speed);
+	void moveForward(float speed);
+	void moveBackward(float speed);
+
+	interface::EventLoop &_event_loop;
+
+	interface::Battery &_battery;
+	interface::Motor &_motor_left;
+	interface::Motor &_motor_right;
+	interface::IMUKit &_imukit;
+
+	const float kMinAngleInput = 0.F;
+	const float kMaxAngleInput = 30.F;
+	const float kMinPwmOutput  = 0.15F;	  // Min to move robot
+	const float kMaxPwmOutput  = 0.70F;
+
+	const float kRollTolerance	= 3.F;	 // in degrees
+	const float kPitchTolerance = 3.F;	 // in degrees
+
+	bool should_stop = true;
+};
+
+}	// namespace leka

spikes/lk_auto_charge/main.cpp

@@ -0,0 +1,160 @@
+// Leka - LekaOS
+// Copyright 2023 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#include "drivers/DigitalOut.h"
+#include "drivers/InterruptIn.h"
+#include "rtos/ThisThread.h"
+
+#include "BLEKit.h"
+#include "BLEServiceFileExchange.h"
+
+#include "BatteryKit.h"
+#include "CoreBattery.h"
+#include "CoreI2C.h"
+#include "CoreLSM6DSOX.hpp"
+#include "CoreMotor.h"
+#include "CorePwm.h"
+#include "EventLoopKit.h"
+#include "HelloWorld.h"
+#include "IMUKit.hpp"
+#include "LogKit.h"
+#include "MathUtils.h"
+#include "SealStrategy.h"
+
+using namespace std::chrono;
+using namespace leka;
+
+namespace {
+
+auto event_loop = EventLoopKit {};
+
+namespace battery {
+
+	namespace charge {
+
+		auto status_input = mbed::InterruptIn {PinName::BATTERY_CHARGE_STATUS};
+
+	}
+
+	auto cells = CoreBattery {PinName::BATTERY_VOLTAGE, battery::charge::status_input};
+
+}	// namespace battery
+
+namespace motors {
+
+	namespace left {
+
+		namespace internal {
+
+			auto dir_1 = mbed::DigitalOut {MOTOR_LEFT_DIRECTION_1};
+			auto dir_2 = mbed::DigitalOut {MOTOR_LEFT_DIRECTION_2};
+			auto speed = CorePwm {MOTOR_LEFT_PWM};
+
+		}	// namespace internal
+
+		auto motor = CoreMotor {internal::dir_1, internal::dir_2, internal::speed};
+
+	}	// namespace left
+
+	namespace right {
+
+		namespace internal {
+
+			auto dir_1 = mbed::DigitalOut {MOTOR_RIGHT_DIRECTION_1};
+			auto dir_2 = mbed::DigitalOut {MOTOR_RIGHT_DIRECTION_2};
+			auto speed = CorePwm {MOTOR_RIGHT_PWM};
+
+		}	// namespace internal
+
+		auto motor = CoreMotor {internal::dir_1, internal::dir_2, internal::speed};
+
+	}	// namespace right
+
+	void stop()
+	{
+		left::motor.stop();
+		right::motor.stop();
+	}
+
+	void moveForward(float left_speed, float right_speed)
+	{
+		left::motor.spin(Rotation::counterClockwise, left_speed);
+		right::motor.spin(Rotation::clockwise, right_speed);
+	}
+
+	void moveBackward(float speed)
+	{
+		left::motor.spin(Rotation::clockwise, speed);
+		right::motor.spin(Rotation::counterClockwise, speed);
+	}
+
+	void spin(float speed)
+	{
+		left::motor.spin(Rotation::counterClockwise, speed);
+		right::motor.spin(Rotation::clockwise, speed);
+	}
+
+}	// namespace motors
+
+namespace imu {
+
+	namespace internal {
+
+		auto drdy_irq = CoreInterruptIn {PinName::SENSOR_IMU_IRQ};
+		auto i2c	  = CoreI2C(PinName::SENSOR_IMU_TH_I2C_SDA, PinName::SENSOR_IMU_TH_I2C_SCL);
+
+	}	// namespace internal
+
+	CoreLSM6DSOX lsm6dsox(internal::i2c, internal::drdy_irq);
+
+}	// namespace imu
+
+IMUKit imukit(imu::lsm6dsox);
+
+}	// namespace
+
+auto mainboard_led = mbed::DigitalOut {LED1};
+
+auto service_file_exchange = BLEServiceFileExchange {};
+auto services			   = std::to_array<interface::BLEService *>({&service_file_exchange});
+auto blekit				   = BLEKit {};
+auto should_stop		   = true;
+
+auto sealStrategy = SealStrategy {event_loop, battery::cells, motors::left::motor, motors::right::motor, imukit};
+
+auto main() -> int
+{
+	rtos::ThisThread::sleep_for(140ms);
+	logger::init();
+
+	HelloWorld hello;
+	hello.start();
+
+	log_info("Hello world!");
+
+	imu::lsm6dsox.init();
+
+	imukit.stop();
+	imukit.init();
+	imukit.start();
+
+	auto runStrategy = [] { sealStrategy.start(); };
+	battery::cells.onChargeDidStop(runStrategy);
+
+	blekit.setServices(services);
+	service_file_exchange.onSetFileExchangeState([](bool should_start) {
+		if (should_start) {
+			sealStrategy.start();
+		} else {
+			sealStrategy.stop();
+		}
+	});
+	blekit.init();
+
+	sealStrategy.init();
+
+	while (true) {
+		rtos::ThisThread::sleep_for(10min);
+	}
+}