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,