Improve arm control with IK validation, synchronized movements, and vision tracking

[williankeller]

Summary

This PR significantly enhances the robotic arm control system by adding inverse kinematics validation, implementing synchronized multi-joint movements, improving vision-based tracking with proportional control and smoothing, and refactoring servo control with easing functions.

Key Changes

Inverse Kinematics & Safety

• Added reachability validation to solveRZ() - now returns bool to indicate if target position is achievable
• Prevents invalid asin() calls by checking if the ratio is within [-1.0, 1.0] bounds
• Added PI definition guard to prevent redefinition conflicts

Synchronized Joint Movement

• Introduced moveJointsSynchronized() function to move multiple joints simultaneously with coordinated timing
• All joints start and finish together, producing natural coordinated motion
• Uses the longest travel distance to maintain 1° resolution across all joints
• Refactored moveInitialPosition() and grab() sequences to use synchronized movements with clear phase comments

Servo Control Improvements

• Enhanced setServoPosition() with configurable step delay and smooth easing
• Implemented smoothStep() S-curve easing function for natural acceleration/deceleration
• Improved logging output format for better readability

Vision Tracking Enhancements

• Replaced hard-coded threshold logic with configurable constants (dead bands, smoothing factor, frame center)
• Implemented proportional control for horizontal and vertical tracking with dead bands
• Added exponential smoothing (0.15 factor) for fluid, responsive motion
• Changed tracking variables from int to float for sub-degree precision
• Improved distance calculation and depth-based adjustments

Cartesian Trajectory Interpolation

• Implemented moveToPosition() with linear Cartesian space interpolation (10 waypoints)
• Tracks last position to enable smooth straight-line end-effector paths
• Validates each interpolated waypoint using IK reachability check
• Falls back to direct movement on first call

Code Quality

• Fixed gripper control bug: changed else if (value == 1) to else if (value == 0) for open command
• Fixed position parsing: corrected comma indices for proper x,y,z,gripAngle extraction
• Added F() macro for string literals to reduce RAM usage
• Improved comments and documentation throughout

Implementation Details

• Vision tracking now uses direct servo writes for responsive feedback during tracking
• Synchronized movements use the same easing function as individual servo moves for consistency
• All angle constraints are applied at the servo level to prevent out-of-bounds writes
• Trajectory interpolation validates reachability at each step and gracefully handles unreachable positions