✨ Arm Homing

- Added homing.hpp
- changed joint router to match the gear ratio

arm/demos/platforms/lpc4078.cpp

@@ -162,8 +162,8 @@ hal::result<sjsu::arm::application_framework> 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,

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

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

arm/platforms/lpc4078.cpp

@@ -77,12 +77,12 @@ hal::result<application_framework> 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<application_framework> initialize_platform()
                  counter));
 
   hal::delay(counter, 1s);
-
+  
   return application_framework{
     .rotunda_servo = &rotunda_offset_servo,
     .shoulder_servo = &shoulder_offset_servo,