Spike autocharge

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,33 @@
+# 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
+	HappyToupie.cpp
+	HappyFishy.cpp
+)
+
+target_link_libraries(spike_lk_auto_charge
+	EventLoopKit
+	CoreTimeout
+	CoreBattery
+	BatteryKit
+	CorePwm
+	CoreMotor
+	CoreIMU
+	CoreI2C
+	IMUKit
+	BLEKit
+)
+
+target_link_custom_leka_targets(spike_lk_auto_charge)

spikes/lk_auto_charge/HappyFishy.cpp

@@ -0,0 +1,88 @@
+// Leka - LekaOS
+// Copyright 2023 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#include "HappyFishy.h"
+
+#include "rtos/ThisThread.h"
+
+#include "MathUtils.h"
+
+using namespace leka;
+using namespace std::chrono;
+
+auto HappyFishy::convertToPwmFrom(float angle) const -> float
+{
+	auto res = utils::math::map(angle, kMinAngleInput, kMaxAngleInput, kMinPwmOutput, kMaxPwmOutput);
+	return res;
+}
+
+void HappyFishy::start()
+{
+	auto on_timeout = [this] { stop(); };
+	_timeout.onTimeout(on_timeout);
+	_timeout.start(20s);
+
+	should_stop = false;
+
+	_event_loop.registerCallback([this] { run(); });
+	_event_loop.start();
+}
+
+void HappyFishy::stop()
+{
+	should_stop = true;
+	_event_loop.stop();
+	rtos::ThisThread::sleep_for(100ms);
+	stopMotors();
+	_timeout.stop();
+}
+
+void HappyFishy::run()
+{
+	if (should_stop || _battery.isCharging()) {
+		stop();
+		return;
+	}
+
+	if (move_left) {
+		spinLeft(0.25F, 0.25F);
+		rtos::ThisThread::sleep_for(300ms);
+	} else {
+		spinRight(0.25F, 0.25F);
+		rtos::ThisThread::sleep_for(320ms);
+	}
+	move_left = !move_left;
+
+	_event_loop.start();
+}
+
+void HappyFishy::stopMotors()
+{
+	_motor_right.stop();
+	_motor_left.stop();
+}
+
+void HappyFishy::spinLeft(float left_speed, float right_speed)
+{
+	_motor_left.spin(Rotation::clockwise, left_speed);
+	_motor_right.spin(Rotation::clockwise, right_speed);
+}
+
+void HappyFishy::spinRight(float left_speed, float right_speed)
+{
+	_motor_left.spin(Rotation::counterClockwise, left_speed);
+	_motor_right.spin(Rotation::counterClockwise, right_speed);
+}
+
+void HappyFishy::moveForward(float speed)
+{
+	_motor_left.spin(Rotation::counterClockwise, speed);
+	_motor_right.spin(Rotation::clockwise, speed);
+}
+
+void HappyFishy::moveBackward(float speed)
+{
+	_motor_left.spin(Rotation::clockwise, speed);
+	_motor_right.spin(Rotation::counterClockwise, speed);
+}

spikes/lk_auto_charge/HappyFishy.h

@@ -0,0 +1,60 @@
+// 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/drivers/Timeout.h"
+#include "interface/libs/EventLoop.h"
+
+namespace leka {
+
+class HappyFishy
+{
+  public:
+	HappyFishy(interface::EventLoop &event_loop, interface::Timeout &timeout, interface::Battery &battery,
+			   interface::Motor &motor_left, interface::Motor &motor_right)
+		: _event_loop(event_loop),
+		  _timeout(timeout),
+		  _battery(battery),
+		  _motor_left(motor_left),
+		  _motor_right(motor_right)
+	{
+	}
+
+	~HappyFishy() = default;
+
+	void start();
+	void stop();
+
+  private:
+	[[nodiscard]] auto convertToPwmFrom(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::Timeout &_timeout;
+
+	interface::Battery &_battery;
+	interface::Motor &_motor_left;
+	interface::Motor &_motor_right;
+
+	const float kMinAngleInput = 0.F;
+	const float kMaxAngleInput = 30.F;
+	const float kMinPwmOutput  = 0.15F;	  // Min to move robot
+	const float kMaxPwmOutput  = 0.70F;
+
+	bool move_left = true;
+
+	bool should_stop = true;
+};
+
+}	// namespace leka

spikes/lk_auto_charge/HappyToupie.cpp

@@ -0,0 +1,82 @@
+// Leka - LekaOS
+// Copyright 2023 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#include "HappyToupie.h"
+
+#include "rtos/ThisThread.h"
+
+#include "MathUtils.h"
+
+using namespace leka;
+using namespace std::chrono;
+
+auto HappyToupie::convertToPwmFrom(float angle) const -> float
+{
+	auto res = utils::math::map(angle, kMinAngleInput, kMaxAngleInput, kMinPwmOutput, kMaxPwmOutput);
+	return res;
+}
+
+void HappyToupie::start()
+{
+	auto on_timeout = [this] { stop(); };
+	_timeout.onTimeout(on_timeout);
+	_timeout.start(20s);
+
+	should_stop = false;
+
+	_event_loop.registerCallback([this] { run(); });
+	_event_loop.start();
+}
+
+void HappyToupie::stop()
+{
+	should_stop = true;
+	_event_loop.stop();
+	rtos::ThisThread::sleep_for(100ms);
+	stopMotors();
+	_timeout.stop();
+}
+
+void HappyToupie::run()
+{
+	if (should_stop || _battery.isCharging()) {
+		stop();
+		return;
+	}
+
+	spinLeft(1.F, 1.F);
+
+	rtos::ThisThread::sleep_for(10ms);
+	_event_loop.start();
+}
+
+void HappyToupie::stopMotors()
+{
+	_motor_right.stop();
+	_motor_left.stop();
+}
+
+void HappyToupie::spinLeft(float left_speed, float right_speed)
+{
+	_motor_left.spin(Rotation::clockwise, left_speed);
+	_motor_right.spin(Rotation::clockwise, right_speed);
+}
+
+void HappyToupie::spinRight(float left_speed, float right_speed)
+{
+	_motor_left.spin(Rotation::counterClockwise, left_speed);
+	_motor_right.spin(Rotation::counterClockwise, right_speed);
+}
+
+void HappyToupie::moveForward(float speed)
+{
+	_motor_left.spin(Rotation::counterClockwise, speed);
+	_motor_right.spin(Rotation::clockwise, speed);
+}
+
+void HappyToupie::moveBackward(float speed)
+{
+	_motor_left.spin(Rotation::clockwise, speed);
+	_motor_right.spin(Rotation::counterClockwise, speed);
+}

spikes/lk_auto_charge/HappyToupie.h

@@ -0,0 +1,58 @@
+// 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/drivers/Timeout.h"
+#include "interface/libs/EventLoop.h"
+
+namespace leka {
+
+class HappyToupie
+{
+  public:
+	HappyToupie(interface::EventLoop &event_loop, interface::Timeout &timeout, interface::Battery &battery,
+				interface::Motor &motor_left, interface::Motor &motor_right)
+		: _event_loop(event_loop),
+		  _timeout(timeout),
+		  _battery(battery),
+		  _motor_left(motor_left),
+		  _motor_right(motor_right)
+	{
+	}
+
+	~HappyToupie() = default;
+
+	void start();
+	void stop();
+
+  private:
+	[[nodiscard]] auto convertToPwmFrom(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::Timeout &_timeout;
+
+	interface::Battery &_battery;
+	interface::Motor &_motor_left;
+	interface::Motor &_motor_right;
+
+	const float kMinAngleInput = 0.F;
+	const float kMaxAngleInput = 30.F;
+	const float kMinPwmOutput  = 0.15F;	  // Min to move robot
+	const float kMaxPwmOutput  = 0.70F;
+
+	bool should_stop = true;
+};
+
+}	// namespace leka