From 5ab6943adf48ea9b74d8bd15d45552e3e85c98c2 Mon Sep 17 00:00:00 2001 From: FluffyFTW Date: Sat, 27 Jan 2024 14:21:51 -0800 Subject: [PATCH] :sparkles: Arm Homing - Added homing.hpp - changed joint router to match the gear ratio --- arm/conanfile.py | 12 ++++---- arm/demos/conanfile.py | 2 +- arm/demos/platforms/lpc4078.cpp | 4 +-- arm/implementations/joint_router.hpp | 4 +-- arm/platform-implementations/home.hpp | 43 +++++++++++++-------------- arm/platforms/lpc4078.cpp | 6 ++-- 6 files changed, 35 insertions(+), 36 deletions(-) diff --git a/arm/conanfile.py b/arm/conanfile.py index 04c5bce..6de90de 100644 --- a/arm/conanfile.py +++ b/arm/conanfile.py @@ -28,16 +28,16 @@ def build_requirements(self): def requirements(self): # Application requirements + self.requires("libhal/[2.2.0]") self.requires("libhal-util/[^3.0.0]") - self.requires("libhal-rmd/[3.0.0]") - self.requires("libhal-soft/[3.0.1]") - self.requires("libhal-esp8266/[2.0.1]") - self.requires("libhal-mpu/2.0.0") - self.requires("libhal/[2.0.3]") + self.requires("libhal-rmd/[^3.0.0]") + self.requires("libhal-soft/[^3.0.2]") + self.requires("libhal-esp8266/[^2.0.1]") + self.requires("libhal-mpu/[2.0.0]") # List of supported platforms if str(self.options.platform).startswith("lpc40"): - self.requires("libhal-lpc40/[^2.0.3]") + self.requires("libhal-lpc40/[^2.1.6]") else: raise ConanInvalidConfiguration( f"The platform '{str(self.options.platform)}' is not" diff --git a/arm/demos/conanfile.py b/arm/demos/conanfile.py index 3b33965..5a7851c 100644 --- a/arm/demos/conanfile.py +++ b/arm/demos/conanfile.py @@ -33,7 +33,7 @@ def requirements(self): self.requires("libhal-soft/[3.0.1]") self.requires("libhal-esp8266/[2.0.1]") self.requires("libhal-mpu/2.0.0") - self.requires("libhal/[2.0.3]") + self.requires("libhal/[2.2.0]") # List of supported platforms if str(self.options.platform).startswith("lpc40"): diff --git a/arm/demos/platforms/lpc4078.cpp b/arm/demos/platforms/lpc4078.cpp index 59d7d74..7aa2551 100644 --- a/arm/demos/platforms/lpc4078.cpp +++ b/arm/demos/platforms/lpc4078.cpp @@ -162,8 +162,8 @@ hal::result initialize_platform() can, counter)); - hal::delay(counter, 1s); - + hal::delay(counter, 10s); + HAL_CHECK(shoulder_mc_x.position_control(30 * 65/30, 5)); return sjsu::arm::application_framework{ .rotunda_accelerometer = &rotunda_mpu, diff --git a/arm/implementations/joint_router.hpp b/arm/implementations/joint_router.hpp index b5fdb92..184066f 100644 --- a/arm/implementations/joint_router.hpp +++ b/arm/implementations/joint_router.hpp @@ -6,8 +6,8 @@ #include "../platform-implementations/mission_control.hpp" #include "../platform-implementations/offset_servo.hpp" -static constexpr float shoulder_gear_ratio = 65/35; -static constexpr float elbow_gear_ratio = 40/35; +static constexpr float shoulder_gear_ratio = 65/30; +static constexpr float elbow_gear_ratio = 40/30; namespace sjsu::arm { class joint_router diff --git a/arm/platform-implementations/home.hpp b/arm/platform-implementations/home.hpp index 513638c..8af2067 100644 --- a/arm/platform-implementations/home.hpp +++ b/arm/platform-implementations/home.hpp @@ -12,7 +12,6 @@ inline hal::degrees atan2_d(float y, float x) return atan2(y, x) * 180 / M_PI; } - inline hal::status set_zero(hal::can::id_t p_addr, hal::can& p_can) { hal::can::message_t set_zero; @@ -51,8 +50,8 @@ inline hal::status home(hal::accelerometer& p_rotunda_accelerometer, hal::degrees elbow_error; hal::rpm elbow_speed; hal::rpm shoulder_speed; - bool shoulder_homed = false, elbow_homed = false; + bool shoulder_homed = false, elbow_homed = false; while (!shoulder_homed || !elbow_homed) { auto rotunda_read = HAL_CHECK(p_rotunda_accelerometer.read()); hal::degrees rotunda_base_yz = @@ -67,10 +66,6 @@ inline hal::status home(hal::accelerometer& p_rotunda_accelerometer, elbow_error = (atan2_d(shoulder_read.y, shoulder_read.z) + 90) - atan2_d(elbow_read.y, elbow_read.z); // shoulder error - hal::print<1024>( - p_terminal, "\nERROR: %f \t %f\n", shoulder_error, elbow_error); - hal::delay(p_clk, 100ms); - shoulder_speed = shoulder_error * -1; shoulder_speed = std::clamp(shoulder_speed, -3.0f, 3.0f); @@ -98,26 +93,30 @@ inline hal::status home(hal::accelerometer& p_rotunda_accelerometer, hal::print<1024>(p_terminal, "%f \t %f\n", shoulder_speed, elbow_speed); } - HAL_CHECK(p_shoulder_mc_x.velocity_control(0)); - hal::delay(p_clk, 10ms); - HAL_CHECK(p_elbow_mc_x.velocity_control(0)); - hal::delay(p_clk, 10ms); - + hal::print(p_terminal, "ZEROINGTHE MOTORS\n"); + HAL_CHECK(set_zero(0x143, p_can)); HAL_CHECK(set_zero(0x142, p_can)); - hal::delay(p_clk, 10ms); - HAL_CHECK(reset_mc_x(0x142, p_can)); - hal::delay(p_clk, 10ms); + hal::delay(p_clk, 1ms); + hal::delay(p_clk, 1ms); + while(!p_shoulder_mc_x.velocity_control(2)); + while(!p_elbow_mc_x.velocity_control(2)); + hal::delay(p_clk, 7s); + while(!p_shoulder_mc_x.velocity_control(0)); + while(!p_elbow_mc_x.velocity_control(0)); + hal::delay(p_clk, 1ms); + + HAL_CHECK(reset_mc_x(0x142, p_can)); HAL_CHECK(reset_mc_x(0x143, p_can)); - hal::delay(p_clk, 10ms); - HAL_CHECK(set_zero(0x143, p_can)); - hal::delay(p_clk, 10ms); + hal::delay(p_clk, 5s); - HAL_CHECK(p_shoulder_mc_x.velocity_control(0)); - hal::delay(p_clk, 10ms); - HAL_CHECK(p_elbow_mc_x.velocity_control(0)); - hal::delay(p_clk, 10ms); + hal::print(p_terminal, "MOVING TO 0 POSITION\n"); + for(int i = 0; i < 2; i ++){ + HAL_CHECK(p_shoulder_mc_x.position_control(0, 5)); + HAL_CHECK(p_elbow_mc_x.position_control(0, 5)); + hal::delay(p_clk, 10ms); + } return hal::success(); -} + } } // namespace sjsu::arm \ No newline at end of file diff --git a/arm/platforms/lpc4078.cpp b/arm/platforms/lpc4078.cpp index 1ffc94c..7a38995 100644 --- a/arm/platforms/lpc4078.cpp +++ b/arm/platforms/lpc4078.cpp @@ -77,12 +77,12 @@ hal::result initialize_platform() static auto shoulder_mc_x = HAL_CHECK( hal::rmd::mc_x::create(can_router, counter, 36.0 * 65 / 30, 0x142)); static auto shoulder_mc_x_servo = - HAL_CHECK(hal::make_servo(shoulder_mc_x, 2.0_rpm)); + HAL_CHECK(hal::make_servo(shoulder_mc_x, 5.0_rpm)); static auto elbow_mc_x = HAL_CHECK( hal::rmd::mc_x::create(can_router, counter, 36.0 * 40 / 30, 0x143)); static auto elbow_mc_x_servo = - HAL_CHECK(hal::make_servo(elbow_mc_x, 2.0_rpm)); + HAL_CHECK(hal::make_servo(elbow_mc_x, 5.0_rpm)); static auto left_wrist_mc_x = HAL_CHECK(hal::rmd::mc_x::create(can_router, counter, 36.0, 0x144)); @@ -223,7 +223,7 @@ hal::result initialize_platform() counter)); hal::delay(counter, 1s); - + return application_framework{ .rotunda_servo = &rotunda_offset_servo, .shoulder_servo = &shoulder_offset_servo,