Skip to content

Commit

Permalink
♻️ (spike): Refactor lk_imu_app to motionKit
Browse files Browse the repository at this point in the history
  • Loading branch information
@HPezz
HPezz committed Oct 28, 2022
1 parent 7026d19 commit 0f0f38b
Show file tree
Hide file tree
Showing 3 changed files with 26 additions and 168 deletions.
1 change: 1 addition & 0 deletions spikes/lk_imu_app/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@ target_link_libraries(spike_lk_imu_app
IMUKit
CorePwm
CoreMotor
MotionKit
)

target_link_custom_leka_targets(spike_lk_imu_app)
4 changes: 2 additions & 2 deletions spikes/lk_imu_app/ODRCard.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
#include "interface/drivers/LED.h"

#pragma once
namespace motion::record {
namespace record {
namespace card {
inline constexpr auto ODR_12_5HZ = 0x0023;
inline constexpr auto ODR_26HZ = 0x0024;
Expand Down Expand Up @@ -52,4 +52,4 @@ void updateODR(leka::interface::LED &led, leka::CoreIMULSM6DSOX &coreimu, const
time_odr = odr_value;
}

} // namespace motion::record
} // namespace record
189 changes: 23 additions & 166 deletions spikes/lk_imu_app/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,12 +25,13 @@
#include "HelloWorld.h"
#include "IMUKit.h"
#include "LogKit.h"
#include "MotionKit.h"
#include "ODRCard.h"
#include "RFIDKit.h"

using namespace std::literals::chrono_literals;
using namespace leka;
using namespace motion::record;
using namespace record;

namespace {

Expand Down Expand Up @@ -88,11 +89,17 @@ namespace motors {
namespace imu {

auto i2c = mbed::I2C(PinName::SENSOR_IMU_TH_I2C_SDA, PinName::SENSOR_IMU_TH_I2C_SCL);
auto coreimu = leka::CoreIMULSM6DSOX(i2c);
auto kit = leka::IMUKit(coreimu);
auto coreimu = CoreIMULSM6DSOX(i2c);
auto kit = IMUKit(coreimu);

} // namespace imu

namespace motion {

auto kit = MotionKit(motors::left::motor, motors::right::motor, imu::kit, leds::belt);

} // namespace motion

namespace rfid {

auto serial = CoreBufferedSerial(RFID_UART_TX, RFID_UART_RX, 57600);
Expand All @@ -119,179 +126,29 @@ auto time_odr = cards::odr::CARD_ODR_26HZ.getTimeODR();

auto main() -> int
{
auto id_card = 0;

logger::init();

HelloWorld hello;
hello.start();

rfidkit.init();

auto angles = imu::kit.getAngles();
static auto first_yaw = angles.at(2);

static auto dt = 60;
static auto power = 1;
static auto motor_stop_bound = 5.f;
static auto motor_slow_down_bound = 30.f;

static auto reinforcer_beginning = rtos::Kernel::Clock::now();

rfidkit.onTagActivated([&angles](const MagicCard &card) {
// id_card = card.getId();
// for (ODRCard odr_card: cards::odr::odr_cards) {
// if (odr_card.getId() == id_card) {
// updateODR<leka::CoreIMULSM6DSOX::odr_xl_t, leka::CoreIMULSM6DSOX::odr_g_t>(
// leds::belt, imu::coreimu, odr_card.getColor(), odr_card.getXlODR(), odr_card.getGyODR(), time_odr,
// odr_card.getTimeODR());
// break;
// }
// }

if (card == MagicCard::emergency_stop) {
imu::kit.reset();
angles = imu::kit.getAngles();
first_yaw = angles.at(2);
reinforcer_beginning = rtos::Kernel::Clock::now();
} else {
switch (card.getId()) {
case (MagicCard::number_0.getId()):
dt = 130;
break;
case (MagicCard::number_1.getId()):
dt = 150;
break;
case (MagicCard::number_2.getId()):
dt = 170;
break;
case (MagicCard::number_3.getId()):
dt = 190;
break;
case (MagicCard::number_4.getId()):
dt = 210;
break;
case (MagicCard::number_5.getId()):
dt = 230;
break;
case (MagicCard::number_6.getId()):
dt = 250;
break;
case (MagicCard::number_7.getId()):
dt = 270;
break;
case (MagicCard::number_8.getId()):
dt = 290;
break;
case (MagicCard::color_indigo.getId()):
power = 1;
break;
case (MagicCard::color_blue.getId()):
power = 2;
break;
case (MagicCard::color_yellow.getId()):
power = 3;
break;
case (MagicCard::color_orange.getId()):
power = 4;
break;
case (MagicCard::color_green.getId()):
motor_stop_bound = 15.f;
break;
case (MagicCard::color_red.getId()):
motor_stop_bound = 18.f;
break;
case (MagicCard::color_purple.getId()):
motor_stop_bound = 23.f;
break;
case (MagicCard::shape_circle.getId()):
motor_stop_bound = 27.f;
break;
case (MagicCard::shape_square.getId()):
motor_stop_bound = 30.f;
break;
case (MagicCard::shape_star.getId()):
motor_stop_bound = 35.f;
break;
case (MagicCard::shape_triangle.getId()):
motor_stop_bound = 40.f;
break;
case (MagicCard::reinforcer_1_blink_green.getId()):
motor_slow_down_bound = 170.f;
break;
case (MagicCard::reinforcer_2_spin_blink.getId()):
motor_slow_down_bound = 200.f;
break;
case (MagicCard::reinforcer_3_fire.getId()):
motor_slow_down_bound = 250.f;
break;
case (MagicCard::reinforcer_4_sprinkles.getId()):
motor_slow_down_bound = 270.f;
break;
case (MagicCard::reinforcer_5_rainbow.getId()):
motor_slow_down_bound = 390.f;
break;
default:
break;
}
}
});
static std::array<float, 3> angles {0.f, 0.f, 0.f};
static float current_yaw = 0.f;

while (true) {
auto start = rtos::Kernel::Clock::now();

angles = imu::kit.getAngles();
auto current_yaw = angles.at(2);

log_info("Pitch : %f, Roll : %f, Yaw : %f", angles.at(0), angles.at(1), angles.at(2));
log_info("speed : %f", std::abs((first_yaw - current_yaw) / 20.f));

if ((start - reinforcer_beginning >= 2s) && (current_yaw >= first_yaw - motor_slow_down_bound)) {
if ((current_yaw >= first_yaw - motor_stop_bound) && (current_yaw <= first_yaw + motor_stop_bound)) {
leds::belt.setColor(RGB::pure_green);
leds::belt.show();
motors::turnOff();
} else {
leds::belt.setColor(RGB::pure_blue);
leds::belt.show();
auto speed = std::abs(pow(((first_yaw - current_yaw) / dt), power));
motors::left::motor.spin(leka::Rotation::right, speed);
motors::right::motor.spin(leka::Rotation::right, speed);
}
} else if ((start - reinforcer_beginning >= 2s) && (current_yaw <= first_yaw + motor_slow_down_bound)) {
if ((current_yaw >= first_yaw - motor_stop_bound) && (current_yaw <= first_yaw + motor_stop_bound)) {
leds::belt.setColor(RGB::pure_green);
leds::belt.show();
motors::turnOff();
} else {
leds::belt.setColor(RGB::pure_blue);
leds::belt.show();
auto speed = std::abs(pow(((first_yaw - current_yaw) / dt), power));
motors::left::motor.spin(leka::Rotation::left, speed);
motors::right::motor.spin(leka::Rotation::left, speed);
}

} else {
leds::belt.setColor(RGB::black);
leds::belt.show();
}
rtos::ThisThread::sleep_until(start + time_odr);
}

// auto i = 0;
motion::kit.spinTurnPID(3, Rotation::clockwise);
rtos::ThisThread::sleep_for(10s);

// while (true) {
// ++i;
// auto start = rtos::Kernel::Clock::now();
motion::kit.spinTurnPID(3, Rotation::counterClockwise);
rtos::ThisThread::sleep_for(10s);

// auto angles = imu::kit.getAngles();
// if (i == 20) {
// log_info("Pitch : %f, Roll : %f, Yaw : %f", angles.at(0), angles.at(1), angles.at(2));
// i = 0;
// }
// angles = imu::kit.getAngles();
// current_yaw = angles.at(2);
// log_debug("Yaw : %f", current_yaw);
// rtos::ThisThread::sleep_for(38ms);

// // log_info("Pitch (rd): %f, Roll (rd): %f, Yaw (rd): %f)", angles_radians.at(0), angles_radians.at(1),
// // angles_radians.at(2));
// rtos::ThisThread::sleep_until(start + time_odr);
// }
// motion::kit.responsive(180.f);
// rtos::ThisThread::sleep_for(100s);
}
}

0 comments on commit 0f0f38b

Please sign in to comment.