nul

spikes/lk_auto_charge/CMakeLists.txt

@@ -13,6 +13,7 @@ target_sources(spike_lk_auto_charge
 	PRIVATE
 	main.cpp
 	SealStrategy.cpp
+	SigmoidStrategy.cpp
 	HappyToupie.cpp
 	HappyFishy.cpp
 )

spikes/lk_auto_charge/SigmoidStrategy.cpp

@@ -0,0 +1,151 @@
+// Leka - LekaOS
+// Copyright 2023 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#include "SigmoidStrategy.h"
+
+#include "rtos/ThisThread.h"
+
+#include "LogKit.h"
+#include "MathUtils.h"
+
+using namespace leka;
+using namespace std::chrono;
+
+void SigmoidStrategy::init()
+{
+	_event_loop.registerCallback([this] { run(); });
+}
+
+auto SigmoidStrategy::sealConvertToPwmFrom(float angle) const -> float
+{
+	// auto res = utils::math::map(angle, kMinAngleInput, kMaxAngleInput, kMinPwmOutput, kMaxPwmOutput);
+	return 0.F;
+}
+
+void SigmoidStrategy::start()
+{
+	auto angles = _imukit.getEulerAngles();
+
+	mid_point		 = angles.roll > 0 ? angles.roll / 2 : -angles.roll / 2;
+	right_movement	 = angles.roll > 0;
+	forward_movement = angles.pitch > 0;
+
+	should_stop = false;
+	_event_loop.start();
+}
+
+void SigmoidStrategy::stop()
+{
+	should_stop = true;
+	_event_loop.stop();
+	rtos::ThisThread::sleep_for(100ms);
+	stopMotors();
+}
+
+void SigmoidStrategy::run()
+{
+	if (should_stop || _battery.isCharging()) {
+		stopMotors();
+		return;
+	}
+
+	auto angles = _imukit.getEulerAngles();
+	log_info("run");
+
+	auto abs_angle_roll = angles.roll > 0 ? angles.roll : -angles.roll;
+	if (abs_angle_roll > kRollTolerance) {
+		if (abs_angle_roll > mid_point) {
+			log_info("premier virage");
+			if (right_movement) {
+				log_info("sur la droite");
+				if (forward_movement) {
+					log_info("vers l'avant");
+					spinLeft(kMinPwmOutput, 0.1f);
+				} else {
+					log_info("vers l'arrière");
+					spinRight(kMinPwmOutput, 0.1f);
+				}
+			} else {
+				log_info("sur la gauche");
+				if (forward_movement) {
+					log_info("vers l'avant");
+					spinRight(0.1f, kMinPwmOutput);
+				} else {
+					log_info("vers l'arrière");
+					spinLeft(0.1f, kMinPwmOutput);
+				}
+			}
+		} else {
+			log_info("second virage");
+			if (right_movement) {
+				log_info("sur la droite");
+				if (forward_movement) {
+					log_info("vers l'avant");
+					// spinLeft(kMinPwmOutput, 0.1f);
+					spinRight(0.1f, kMinPwmOutput);
+				} else {
+					log_info("vers l'arrière");
+					// spinRight(kMinPwmOutput, 0.1f);
+					spinLeft(0.1f, kMinPwmOutput);
+				}
+			} else {
+				log_info("sur la gauche");
+				if (forward_movement) {
+					log_info("vers l'avant");
+					// spinRight(0.1f, kMinPwmOutput);
+					spinLeft(kMinPwmOutput, 0.1f);
+				} else {
+					log_info("vers l'arrière");
+					// spinLeft(0.1f, kMinPwmOutput);
+					spinRight(kMinPwmOutput, 0.1f);
+				}
+			}
+		}
+	} else if (abs(angles.pitch) > kPitchTolerance) {
+		auto should_move_forward = angles.pitch > 0;
+		auto speed				 = sealConvertToPwmFrom(angles.pitch > 0 ? angles.pitch : -angles.pitch);
+		if (should_move_forward) {
+			moveForward(speed);
+		} else {
+			moveBackward(speed);
+		}
+	} else {
+		stopMotors();
+	}
+
+	log_info("==================");
+
+	rtos::ThisThread::sleep_for(500ms);
+	_event_loop.start();
+}
+
+void SigmoidStrategy::stopMotors()
+{
+	_motor_right.stop();
+	_motor_left.stop();
+}
+
+void SigmoidStrategy::spinLeft(float left_speed, float right_speed)
+{
+	_motor_left.spin(Rotation::clockwise, left_speed);
+	_motor_right.spin(Rotation::clockwise, right_speed);
+}
+
+void SigmoidStrategy::spinRight(float left_speed, float right_speed)
+{
+	_motor_left.spin(Rotation::counterClockwise, left_speed);
+	_motor_right.spin(Rotation::counterClockwise, right_speed);
+}
+
+void SigmoidStrategy::moveForward(float speed)
+{
+	_motor_left.spin(Rotation::counterClockwise, speed);
+	_motor_right.spin(Rotation::clockwise, speed);
+}
+
+void SigmoidStrategy::moveBackward(float speed)
+{
+	_motor_left.spin(Rotation::clockwise, speed);
+	_motor_right.spin(Rotation::counterClockwise, speed);
+}

spikes/lk_auto_charge/SigmoidStrategy.h

@@ -0,0 +1,66 @@
+// 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 SigmoidStrategy
+{
+  public:
+	SigmoidStrategy(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)
+	{
+	}
+
+	~SigmoidStrategy() = 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.4F;	 // Min to move robot
+	const float kMaxPwmOutput  = 0.70F;
+
+	const float kRollTolerance	= 3.F;	 // in degrees
+	const float kPitchTolerance = 3.F;	 // in degrees
+
+	float mid_point		  = 0.F;
+	bool right_movement	  = false;
+	bool forward_movement = false;
+
+	bool should_stop = true;
+};
+
+}	// namespace leka

spikes/lk_auto_charge/main.cpp

@@ -23,6 +23,7 @@
 #include "LogKit.h"
 #include "MathUtils.h"
 #include "SealStrategy.h"
+#include "SigmoidStrategy.h"
 
 using namespace std::chrono;
 using namespace leka;
@@ -123,7 +124,7 @@ auto services			   = std::to_array<interface::BLEService *>({&service_file_excha
 auto blekit				   = BLEKit {};
 auto should_stop		   = true;
 
-auto sealStrategy = SealStrategy {event_loop, battery::cells, motors::left::motor, motors::right::motor, imukit};
+auto sealStrategy = HappyFishy {event_loop, battery::cells, motors::left::motor, motors::right::motor, imukit};
 
 auto main() -> int
 {