{"id":48832311,"url":"https://github.com/1999azzar/body_kinematics","last_synced_at":"2026-04-14T21:01:59.675Z","repository":{"id":323951946,"uuid":"1095166015","full_name":"1999AZZAR/body_kinematics","owner":"1999AZZAR","description":null,"archived":false,"fork":false,"pushed_at":"2025-12-27T12:24:16.000Z","size":194,"stargazers_count":0,"open_issues_count":0,"forks_count":0,"subscribers_count":0,"default_branch":"main","last_synced_at":"2025-12-29T07:06:24.627Z","etag":null,"topics":[],"latest_commit_sha":null,"homepage":null,"language":"C++","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":null,"status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/1999AZZAR.png","metadata":{"files":{"readme":"README.md","changelog":null,"contributing":null,"funding":null,"license":null,"code_of_conduct":null,"threat_model":null,"audit":null,"citation":null,"codeowners":null,"security":null,"support":null,"governance":null,"roadmap":null,"authors":null,"dei":null,"publiccode":null,"codemeta":null,"zenodo":null,"notice":null,"maintainers":null,"copyright":null,"agents":null,"dco":null,"cla":null}},"created_at":"2025-11-12T17:18:58.000Z","updated_at":"2025-12-27T12:24:19.000Z","dependencies_parsed_at":null,"dependency_job_id":null,"html_url":"https://github.com/1999AZZAR/body_kinematics","commit_stats":null,"previous_names":["1999azzar/body_kinematics"],"tags_count":0,"template":false,"template_full_name":null,"purl":"pkg:github/1999AZZAR/body_kinematics","repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/1999AZZAR%2Fbody_kinematics","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/1999AZZAR%2Fbody_kinematics/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/1999AZZAR%2Fbody_kinematics/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/1999AZZAR%2Fbody_kinematics/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/1999AZZAR","download_url":"https://codeload.github.com/1999AZZAR/body_kinematics/tar.gz/refs/heads/main","sbom_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/1999AZZAR%2Fbody_kinematics/sbom","scorecard":null,"host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":286080680,"owners_count":31815080,"icon_url":"https://github.com/github.png","version":null,"created_at":"2022-05-30T11:31:42.601Z","updated_at":"2026-04-14T18:05:02.291Z","status":"ssl_error","status_checked_at":"2026-04-14T18:05:01.765Z","response_time":153,"last_error":"SSL_read: unexpected eof while reading","robots_txt_status":"success","robots_txt_updated_at":"2025-07-24T06:49:26.215Z","robots_txt_url":"https://github.com/robots.txt","online":false,"can_crawl_api":true,"host_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub","repositories_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories","repository_names_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repository_names","owners_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners"}},"keywords":[],"created_at":"2026-04-14T21:01:57.499Z","updated_at":"2026-04-14T21:01:59.657Z","avatar_url":"https://github.com/1999AZZAR.png","language":"C++","funding_links":[],"categories":[],"sub_categories":[],"readme":"# Hexagonal 3-Wheel Omni Robot Control with 4 PG28 Motors\n\nArduino Mega code for controlling a **hexagonal-shaped 3-wheel omni robot** using **YFROBOT v2 Motor Driver Shield** with PID control and pre-built movement patterns.\n\n## Hardware Setup\n\n### YFROBOT v2 Shield Connections\n\n**Important**: The YFROBOT v2 shield uses **I2C communication** with an onboard microcontroller. Motor control is handled via I2C, not direct PWM pins!\n\n**Connections Required:**\n\n**I2C Communication (Shield ↔ Arduino Mega):**\n\n- **SDA**: Arduino SDA (Pin 20)\n- **SCL**: Arduino SCL (Pin 21)\n- **GND**: Connect to Arduino GND\n- **5V/VCC**: Connect to Arduino 5V\n\n**Encoder Connections (Motor → Arduino Direct):**\n\n- **Motor 1 (Lifter)**: Data A (d3) → Arduino D3, Data B (d5) → Arduino D5\n- **Motor 2 (Front Right)**: Data A (d2) → Arduino D2, Data B (d4) → Arduino D4\n- **Motor 3 (Front Left)**: Data A (d7) → Arduino D7, Data B (d6) → Arduino D6\n- **Motor 4 (Back)**: Data A (d9) → Arduino D9, Data B (d8) → Arduino D8\n\n**Power Connections:**\n\n- **Motor Power**: Connect to YFROBOT v2 shield motor terminals (M+, M-)\n- **Motor Signal**: Connect motor encoder pins (d#) to Arduino pins as above\n- **Main Power**: Shield gets power from Arduino 5V/GND via I2C\n\n**Note**: No PWM or DIR pins need to be connected - the shield handles motor control internally via I2C!\n\n### Important Notes\n\n- The shield uses I2C communication - ensure SDA/SCL connections are secure\n- Encoder feedback provides closed-loop PID control for precise movement\n- If motor directions are reversed, adjust the `motorConfig()` call in setup()\n- Ensure proper power supply for motors (YFROBOT v2 supports the current draw)\n\n## Features\n\n### PID Control\n\n- Each motor has independent PID control for precise speed regulation\n- Encoder feedback for closed-loop control\n- Adjustable PID parameters (Kp, Ki, Kd)\n- **Separated Control**: Lifter and omni motors are controlled independently\n- **Stop Protection**: PID control is disabled when motors are stopped to ensure true stopping\n- **State Management**: System tracks motor states (omni active/stopped, lifter active/inactive)\n- **Acceleration Limiting**: Smooth motor speed transitions prevent jerky movements\n- **Lifter Safety**: Hardware limit switches prevent over-travel with automatic motor shutdown\n\n### Lifter Safety System\n\nThe lifter mechanism includes comprehensive safety features to prevent damage and ensure reliable operation:\n\n- **Hardware Limit Switches**: Top and bottom limit switches (D26, D27) prevent over-travel\n- **Automatic Motor Shutdown**: Motor stops immediately when limit switch is triggered\n- **Movement Timeout**: 5-second safety timeout prevents motor stalls\n- **Smart Movement Logic**: When at a limit, only allows movement away from that limit\n- **Continuous Monitoring**: Limit switches are checked during movement for immediate response\n- **Virtual Bumper Exclusion**: Lifter operates independently of obstacle avoidance system\n\n### System Logging\n\nThe robot uses compact logging codes for memory efficiency. All logging codes are documented in `LOGGING_CODES.md` with detailed explanations and examples.\n\n### Compact Logging Codes\n\nThe system uses memory-efficient compact logging codes for minimal memory usage. All codes are documented in detail in `LOGGING_CODES.md`.\n\n**Quick Reference:**\n- `VFF:x,y,m|vx,vy` = Virtual Force Field forces and resulting velocity\n- `LFT:c` = Lifter status and safety codes\n- `LSD:t,b` = Limit Switch Debug (raw values)\n- `LS:t,b` = Limit Switch status check\n\n### Servo Integration\n\n**Servo control is implemented** through the YFROBOT shield's dedicated servo channels (08 and 09). Servos are controlled via I2C communication with the shield's microcontroller.\n\n#### Servo Hardware Setup\n\n- **Channels**: Tilt servo on Channel 8 (SERVO 01), Gripper servo on Channel 9 (SERVO 02)\n- **Control**: I2C communication through YFROBOT shield microcontroller with 50Hz PWM\n- **No Arduino Pins Used**: Control handled entirely by shield (no PWM pins required)\n- **Power**: Servos powered directly from YFROBOT shield\n- **Angle Range**: 0-180° for both servos\n- **PWM Frequency**: Set to 50Hz during initialization for proper servo operation\n- **Real-time Updates**: Servo angles displayed in status output with channel info\n\n#### Servo Features\n\n- **Tilt Control**: Vertical tilt servo for camera/lidar positioning\n- **Gripper Control**: Gripper opening/closing for manipulation tasks\n- **Angle Feedback**: Current angles displayed in status commands\n- **Preset Positions**: Quick commands for common positions (up/down/center/open/close)\n- **Direct Angle Control**: Precise angle setting with `ta\u003cangle\u003e` and `ga\u003cangle\u003e` commands\n- **Integration**: Servo status integrated into main status (`p`) command\n\n#### Servo Commands\n\n- **`mu`**: Tilt Up (0°)\n- **`md`**: Tilt Down (180°)\n- **`mc`**: Tilt Center (90°)\n- **`no`**: Gripper Open (0°)\n- **`nc`**: Gripper Close (180°)\n- **`nh`**: Gripper Half-Open (90°)\n- **`ta\u003cangle\u003e`**: Set tilt angle (0-180°)\n- **`ga\u003cangle\u003e`**: Set gripper angle (0-180°)\n\n#### Servo Integration Details\n\n- **Channel-based Control**: Uses YFROBOT shield's dedicated servo channels\n- **I2C Communication**: All servo control routed through shield microcontroller\n- **Status Display**: Servo angles shown in main status (`p`) command\n- **No Arduino Resources**: Doesn't consume PWM pins or timers\n- **Reliable Control**: Shield handles PWM generation for stable servo operation\n\n#### Servo Troubleshooting\n\nIf servo commands are inconsistent or don't respond:\n\n1. **PWM Frequency**: Ensure `setPWMFreq(50)` is called during setup (50Hz required for servos)\n2. **Channel Numbers**: Check your shield labeling - may be channels 0/1, 10/11, or others\n3. **Power Supply**: Verify YFROBOT shield has adequate power for servo operation\n4. **I2C Connection**: Test SDA/SCL connections and I2C bus health\n5. **Servo Compatibility**: Ensure servos are standard hobby servos (1500μs center pulse)\n6. **Debug Output**: Check serial output for \"Setting servo (channel X) to Y°\" messages\n7. **Alternative Channels**: Try different channel numbers if 8/9 don't work:\n   ```cpp\n   const int TILT_SERVO_CHANNEL = 0;      // Try 0, 1, 10, 11, etc.\n   const int GRIPPER_SERVO_CHANNEL = 1;\n   ```\n8. **Test Commands**: Start with `mc` (center) and `nc` (close) to verify basic function\n\n## Sensor Integration\n\n### Sensor Hardware Setup\n\n**Important**: The YFROBOT v2 Motor Driver Shield does NOT have built-in sensor interfaces. All sensors must be connected directly to Arduino pins.\n\n**6x IR Distance Sensors (Sharp GP2Y0A02YK0F)** - Wall Alignment\n\n- **Range**: 20-150cm (200-1500mm)\n- **Output**: Analog voltage 0.4V-2.7V (linear relationship with distance)\n- **Pins**: A0-A2, A9-A11 (Left1, Left2, Right1, Right2, Back1, Back2)\n- **Purpose**: Wall alignment and obstacle detection on left/right/back sides\n- **Connection**: Connect to Arduino analog pins (A0-A2 direct on shield, A9-A11 via jumper wires)\n- **Power**: Connect sensor VCC to Arduino 5V, GND to Arduino GND\n- **Note**: Shield headers only expose A0-A2; remaining sensors need jumper wires to A9-A11\n\n**2x HC-SR04 Ultrasonic Sensors** - Front Distance\n\n- **Range**: 2-400cm\n- **Pins**: Trig(22,24), Echo(23,25) for Front Left/Right\n- **Purpose**: Front obstacle detection and precise front distance measurement\n- **Connection**: Connect directly to Arduino digital pins as specified\n- **Power**: Connect sensor VCC to Arduino 5V, GND to Arduino GND\n\n**3x Line Sensors** - Line Following/Navigation\n\n- **Pins**: A6-A8 (Left, Center, Right)\n- **Purpose**: Line following, alignment with floor lines, navigation assistance\n- **Connection**: Connect sensor output pins directly to Arduino analog pins A6-A8\n- **Power**: Connect sensor VCC to Arduino 5V, GND to Arduino GND\n\n### Sensor Features\n\n#### IR Distance Sensor Processing\n\n- **Voltage-to-Distance Conversion**: Automatic conversion using Sharp GP2Y0A02YK0F formula\n- **Validity Checking**: Only valid readings (within 200-1500mm range) are used\n- **Real-time Updates**: Sensors updated every 100ms in main loop\n- **Wall Alignment**: 2 sensors per side provide redundancy and better alignment\n\n#### Ultrasonic Sensor Processing\n\n- **Pulse Timing**: Standard HC-SR04 pulse timing with 30ms timeout\n- **Distance Calculation**: Automatic conversion using speed of sound (343 m/s)\n- **Timeout Handling**: Invalid readings flagged when no echo received\n- **Front Detection**: Left/right front sensors for obstacle avoidance\n\n#### Line Sensor Processing\n\n- **Threshold Detection**: Configurable threshold (default 512) for line detection\n- **Raw Value Reading**: Analog readings provide sensitivity information\n- **Three-Sensor Array**: Left/Center/Right for precise line following\n- **Navigation Aid**: Can be used for both line following and general navigation\n\n#### Sensor Integration Features\n\n- **Automatic Updates**: All sensors updated periodically in main loop\n- **Data Structures**: Organized sensor data with validity flags\n- **Status Display**: Sensor readings shown in `p` (status) command\n- **Detailed Readings**: `sr` command provides comprehensive sensor data\n- **Error Handling**: Invalid readings properly flagged and handled\n\n### Sensor Commands\n\n#### Detailed Sensor Readings (`sr` command)\n\n```\n=== Detailed Sensor Readings ===\nIR Distance Sensors (Sharp GP2Y0A02YK0F):\n  Left 1: 450.5mm (2.10V) - VALID\n  Left 2: 432.1mm (2.15V) - VALID\n  Right 1: 0.0mm (0.00V) - INVALID\n  ...\n\nHC-SR04 Ultrasonic Sensors:\n  Front Left: 125.4cm (7352us) - VALID\n  Front Right: 0.0cm (0us) - TIMEOUT\n\nLine Sensors (Threshold: 512):\n  Left: 234 - OFF LINE\n  Center: 678 - ON LINE\n  Right: 345 - OFF LINE\n========================\n```\n\n### Sensor Applications\n\n#### Wall Alignment\n\n- **Left/Right IR Sensors**: Maintain consistent distance from walls\n- **Redundant Sensors**: 2 sensors per side provide reliable alignment\n- **Range**: 20-150cm optimal for wall following in corridors\n\n#### Obstacle Avoidance\n\n- **Front Ultrasonic**: Detect obstacles before collision\n- **Wide Coverage**: Left/right front sensors cover approach angles\n- **Long Range**: Up to 4m detection range for navigation planning\n\n#### Line Following/Navigation\n\n- **Three-Sensor Array**: Precise line detection and centering\n- **Floor Navigation**: Follow lines or navigate to marked positions\n- **Dual Purpose**: Can be used for line following or general navigation\n\n### Sensor Troubleshooting\n\n#### IR Sensor Issues\n\n- **Invalid Readings**: Check voltage range (0.4-2.7V expected)\n- **Noisy Readings**: Add filtering or averaging if needed\n- **Range Limits**: Sensors may give invalid readings outside 20-150cm\n\n#### Ultrasonic Sensor Issues\n\n- **No Echo**: Check power, connections, and obstacle angles\n- **Timeout Errors**: May occur with absorbent surfaces or extreme angles\n- **Interference**: Multiple ultrasonics may interfere - use timing offsets\n\n#### Line Sensor Issues\n\n- **Threshold Tuning**: Adjust threshold (default 512) for your surface\n- **Lighting**: Consistent lighting important for reliable detection\n- **Surface Contrast**: Ensure good contrast between line and floor\n\n### Complete Command Set for Raspberry Pi Integration\n\n#### Basic Movements (Cardinal Directions - 2 Wheels Each)\n\n- **`f`/`F`**: Forward (FL \u0026 FR wheels only)\n- **`b`/`B`**: Backward (FL \u0026 FR wheels only)\n- **`l`/`L`**: Strafe Left (FL \u0026 Back wheels only)\n- **`r`/`R`**: Strafe Right (FR \u0026 Back wheels only)\n\n#### Motor Usage \u0026 Power Matrix\n\n| Command               | Movement Type       | Motors Used                            | Power Level                 | Notes                                    |\n| --------------------- | ------------------- | -------------------------------------- | --------------------------- | ---------------------------------------- |\n| **`f`/`F`** | Pure Forward        | M2(FR) + M3(FL)                        | 50%-100% × speedMultiplier | 2-wheel cardinal                         |\n| **`b`/`B`** | Pure Backward       | M2(FR) + M3(FL)                        | 50%-100% × speedMultiplier | 2-wheel cardinal                         |\n| **`l`/`L`** | Strafe Left         | M3(FL) + M4(Back)                      | 50%-100% × speedMultiplier | 2-wheel cardinal                         |\n| **`r`/`R`** | Strafe Right        | M2(FR) + M4(Back)                      | 50%-100% × speedMultiplier | 2-wheel cardinal                         |\n| **`q`/`Q`** | Forward-Left        | M2(FR) + M4(Back)                      | 50%-100% × speedMultiplier | 2-wheel diagonal                         |\n| **`e`/`E`** | Forward-Right       | M3(FL) + M4(Back)                      | 50%-100% × speedMultiplier | 2-wheel diagonal                         |\n| **`z`/`Z`** | Backward-Left       | M3(FL) + M4(Back)                      | 50%-100% × speedMultiplier | 2-wheel diagonal                         |\n| **`x`/`X`** | Backward-Right      | M2(FR) + M4(Back)                      | 50%-100% × speedMultiplier | 2-wheel diagonal                         |\n| **`t`/`T`** | Turn Left           | M2(FR) + M3(FL) ± M4(Back)            | 35%-80% × speedMultiplier  | Forward + left turn                      |\n| **`y`/`Y`** | Turn Right          | M2(FR) + M3(FL) ± M4(Back)            | 35%-80% × speedMultiplier  | Forward + right turn                     |\n| **`a`/`A`** | Arc Left            | M2(FR) + M3(FL) ± M4(Back)            | 40%-80% × speedMultiplier  | Forward + left curve                     |\n| **`j`/`J`** | Arc Right           | M2(FR) + M3(FL) ± M4(Back)            | 40%-80% × speedMultiplier  | Forward + right curve                    |\n| **`c`/`C`** | Rotate CW           | M2(FR) + M3(FL) + M4(Back)             | 50%-100% × speedMultiplier | All wheels spin                          |\n| **`w`/`W`** | Rotate CCW          | M2(FR) + M3(FL) + M4(Back)             | 50%-100% × speedMultiplier | All wheels spin                          |\n| **`u`/`U`** | Lift Up             | M1(Lifter) only                        | 100%                        | Independent of omni motors               |\n| **`d`/`D`** | Lift Down           | M1(Lifter) only                        | 100%                        | Independent of omni motors               |\n| **`1-4`**     | Motor Test          | Single motor                           | 97% max (4000/4096)         | Direct control, no PID                   |\n| **`g`/`G`** | Encoder Calibration | Reset all encoder data                 | None                        | Zero positions \u0026 smoothing               |\n| **`h`/`H`** | Figure-8            | M2(FR) + M3(FL) ± M4(Back)            | 30%-80% × speedMultiplier  | Smooth encoder-controlled                |\n| **`o`/`O`** | Turbo Mode Toggle   | Enable/disable permanent fast response | None                        | Maximum responsiveness for all movements |\n\n### Motor Speed Calculations\n\n**Inverse Kinematics Formula (CORRECTED for physical motor mounting):**\n\n```\nmotor_speed = (-vx) × cos(θ) × speedMultiplier + vy × sin(θ) × speedMultiplier + ω × R × speedMultiplier\n```\n\n**Motor Angles (hexagonal robot):**\n\n- **M2 (FR)**: θ = 45° (0.785 rad)\n- **M3 (FL)**: θ = 135° (2.356 rad)\n- **M4 (Back)**: θ = 180° (3.14 rad)\n\n**Example Calculations (speedMultiplier = 1.0) - CORRECTED FOR PHYSICAL MOTOR MOUNTING:**\n\n| Command         | vx   | vy   | ω   | M2 Speed | M3 Speed | M4 Speed | Back Used? |\n| --------------- | ---- | ---- | ---- | -------- | -------- | -------- | ---------- |\n| **`f`** | 1.0  | 0.0  | 0.0  | -0.707   | +0.707   | 0        | No         |\n| **`b`** | -1.0 | 0.0  | 0.0  | +0.707   | -0.707   | 0        | No         |\n| **`l`** | 0.0  | -1.0 | 0.0  | +0.707   | -0.707   | +1.0     | Yes        |\n| **`r`** | 0.0  | 1.0  | 0.0  | -0.707   | +0.707   | -1.0     | Yes        |\n| **`c`** | 0.0  | 0.0  | 1.0  | +1.0     | +1.0     | +1.0     | Yes        |\n| **`w`** | 0.0  | 0.0  | -1.0 | -1.0     | -1.0     | -1.0     | Yes        |\n\n**Note:** Motor mounting direction required vx negation in inverse kinematics formula\n\n### Enhanced Encoder Processing, Motor Synchronization \u0026 Fast Rotation\n\nThe system now includes advanced encoder processing, **motor synchronization**, and **fast rotation response** for **smoother and more uniform motor control**:\n\n- **Moving Average Filter**: 5-sample RPM averaging reduces noise\n- **Exponential Smoothing**: α=0.6 smoothing factor for responsive velocity tracking\n- **Encoder-based Acceleration Limiting**: Prevents jerky movements (200 RPM normal, 500 RPM rotation)\n- **Position Tracking**: Monitors motor position in revolutions\n- **Motor Synchronization**: PID-based speed matching for uniform RPM across active motors\n- **Fast Rotation Mode**: Bypasses acceleration limits for 2 seconds after rotation commands\n- **Optimized PID Gains**: Higher gains (Kp=4.0, Ki=1.5, Kd=0.5) for omni motors\n- **Real-time Status**: Enhanced `p` command shows smoothed RPM, sync errors, and position data\n\n**Fast Rotation Features:**\n\n- **TURBO MODE**: Permanent fast response toggle with `o` command\n- **Extended Fast Response**: 5 seconds of unlimited acceleration for rotation commands\n- **Aggressive PID Gains**: Kp=8.0, Ki=3.0, Kd=0.2 for omni motors (4x more responsive)\n- **Adaptive Smoothing**: α=0.9 during fast rotation, α=0.6 normal\n- **Synchronization Bypass**: Disabled during fast rotation for maximum speed\n- **Higher Acceleration Limits**: 1000 RPM/s for rotation vs 200 RPM/s normal\n\n**Synchronization Features:**\n\n- **Intended Active Motor Tracking**: Explicit flags track which motors are intended to move\n- **Strict 2/3-Wheel Enforcement**: Only intended motors participate in synchronization\n- **Rotation-to-Translation Fix**: Prevents motors from staying active after 3-wheel rotation commands\n- **Average RPM Calculation**: Computes target synchronized RPM from intended active motors\n- **PID Correction**: Applies proportional correction (Kp=0.3) to maintain uniformity\n- **Error Monitoring**: Displays synchronization errors and active motors in status output\n\n**Benefits:**\n\n- ✅ **Smoother Motor Response**: Filtered encoder feedback reduces oscillations\n- ✅ **Uniform Speed Control**: Motors maintain consistent RPM through PID synchronization\n- ✅ **Fast Rotation Response**: Immediate acceleration for spin commands\n- ✅ **Better Acceleration**: Gradual speed changes prevent wheel slip\n- ✅ **Position Awareness**: Track motor positions for advanced control\n- ✅ **Noise Reduction**: Multiple filtering layers eliminate encoder jitter\n- ✅ **Motor Synchronization**: Active motors automatically match speeds for perfect uniformity\n\n### Speed Control System\n\n**Global Speed Multiplier:** `speedMultiplier` (default: 1.0 = 100%)\n\n- **`5`**: 0.5× speed (50%)\n- **`6`**: 0.6× speed (60%)\n- **`7`**: 0.7× speed (70%)\n- **`8`**: 0.8× speed (80%)\n- **`9`**: 0.9× speed (90%)\n- **`0`**: 1.0× speed (100%)\n\n**Formula:** `final_speed = base_speed × speedMultiplier`\n\n- Example: Forward at 70% speed = `f` command + `7` command\n\n### Back Wheel Activation Rules\n\n**STRICT RULES:** Back wheel (M4) is **idle** unless:\n\n- `|vy| \u003e 0.1` (significant sideways movement \u003e10%)\n- `|ω| \u003e 0.1` (significant rotation \u003e10%)\n\n**Examples:**\n\n- **`f` (1.0, 0.0, 0.0)**: `|vy|=0, |ω|=0` → Back wheel idle ✅\n- **`l` (0.0, -1.0, 0.0)**: `|vy|=1.0, |ω|=0` → Back wheel active ✅\n- **`c` (0.0, 0.0, 1.0)**: `|vy|=0, |ω|=1.0` → Back wheel active ✅\n- **`t` (0.5, 0.0, -0.8)**: `|vy|=0, |ω|=0.8` → Back wheel active ✅\n\n#### Diagonal Movements (2-wheel combinations)\n\n- **`q`/`Q`**: Forward-Left (FR + Back wheels)\n- **`e`/`E`**: Forward-Right (FL + Back wheels)\n- **`z`/`Z`**: Backward-Left (FL + Back wheels)\n- **`x`/`X`**: Backward-Right (FR + Back wheels)\n\n#### Turning \u0026 Rotation\n\n- **`t`**: Turn Left (forward + left rotation)\n- **`y`**: Turn Right (forward + right rotation)\n- **`c`/`C`**: Rotate Clockwise (spin in place)\n- **`w`/`W`**: Rotate Counter-Clockwise (spin in place)\n\n#### Arc Movements (Curved paths)\n\n- **`a`/`A`**: Arc Left (gentle left curve)\n- **`j`/`J`**: Arc Right (gentle right curve)\n\n#### Speed Control (50%-100%)\n\n- **`5`**: 50% speed\n- **`6`**: 60% speed\n- **`7`**: 70% speed\n- **`8`**: 80% speed\n- **`9`**: 90% speed\n- **`0`**: 100% speed (full speed)\n\n#### Lifter Control\n\n- **`u`/`U`**: Lift Up\n- **`d`/`D`**: Lift Down\n\n#### System Commands\n\n- **`s`/`S`**: Emergency Stop (all motors)\n- **`p`/`P`**: Print Status (motor RPM, PID values)\n- **`1-4`**: Test individual motors (FULL POWER - 4000/4096 max speed)\n- **`g`/`G`**: Full calibration sequence\n- **`h`/`H`**: Figure-8 test pattern\n\n### Command Categories for Easy Integration:\n\n**Navigation**: `f,b,l,r,q,e,z,x` (8 directional movements - all use 2 wheels)\n**Rotation**: `t,y,c,w,a,j` (6 rotation options - turns use 2-3 wheels, pure spin uses 3)\n**Precision**: `5,6,7,8,9,0` (6 speed levels)\n**Utility**: `s,p,u,d` (essential controls)\n**Servo Control**: `mu,md,mc,no,nc,nh,ta\u003cangle\u003e,ga\u003cangle\u003e` (servo tilt and gripper control)\n**Sensor Control**: `sr` (detailed sensor readings)\n**Testing**: `1,2,3,4,g,h,o` (diagnostics and turbo mode)\n\n### Raspberry Pi Integration Examples\n\n#### Python Serial Control:\n\n```python\nimport serial\nimport time\n\n# Connect to Arduino Mega\nser = serial.Serial('/dev/ttyUSB0', 115200, timeout=1)\n\n# Send commands\ndef send_command(cmd):\n    ser.write(cmd.encode())\n    time.sleep(0.1)  # Small delay\n\n# Movement examples\nsend_command('f')  # Forward\ntime.sleep(2)\nsend_command('s')  # Stop\n\n# Speed control\nsend_command('7')  # 70% speed\nsend_command('f')  # Forward at 70% speed\n\n# Diagonal movement\nsend_command('q')  # Forward-left\ntime.sleep(1.5)\nsend_command('s')  # Stop\n\n# Arc movement\nsend_command('j')  # Arc right\ntime.sleep(2)\nsend_command('s')  # Stop\n\n# Lifter control\nsend_command('u')  # Lift up\ntime.sleep(3)\nsend_command('d')  # Lift down\n```\n\n#### Simple Movement Sequences:\n\n```python\n# Square pattern\nmoves = ['f', 'r', 'f', 'r', 'f', 'r', 'f', 'r', 's']\nfor move in moves:\n    send_command(move)\n    time.sleep(2 if move != 's' else 0.5)\n\n# Figure-8 autonomous\nsend_command('h')  # Built-in figure-8 pattern\n```\n\n## Configuration\n\n### Motor Parameters\n\n```cpp\nconst double MAX_RPM = 100.0;        // Maximum RPM for motors\nconst double BASE_SPEED = 60.0;      // Base speed for movements\nconst double TURN_SPEED = 40.0;      // Speed for turning movements\nconst double LIFT_SPEED = 50.0;      // Speed for lifter motor\n```\n\n### Encoder Settings\n\n```cpp\nconst int ENCODER_CPR = 28;          // Counts per revolution for PG28\nconst double GEAR_RATIO = 1.0;       // Adjust if motors are geared\n```\n\n### PID Settings\n\n```cpp\n// Default PID values (adjust based on your system)\nPID pid[4] = {\n  PID(\u0026input[0], \u0026output[0], \u0026setpoint[0], 2.0, 0.5, 0.1, DIRECT),  // Kp=2.0, Ki=0.5, Kd=0.1\n  // ... other motors with same settings\n}\n```\n\n### Inverse Kinematics Tuning\n\nThe code uses **strict selective wheel usage inverse kinematics** optimized for your hexagonal robot:\n\n- **Forward/Backward Dominant**: Only uses Front Left \u0026 Front Right wheels (Back wheel idle)\n- **Sideways/Rotation Dominant**: Uses all three wheels for full omnidirectional movement\n- **Movement Threshold**: Back wheel activates only when sideways or rotation \u003e 10% of max speed\n\n**Strict Rules:**\n\n- If `|vy| \u003e 0.1` OR `|ω| \u003e 0.1`: Use all 3 wheels (omnidirectional)\n- Otherwise: Use only FL \u0026 FR wheels (efficient forward/backward)\n\nIf movement directions are incorrect, adjust the `MOTOR_ANGLES` array in the code:\n\n```cpp\nconst double MOTOR_ANGLES[4] = {0, 45, 135, 180};  // Lifter, FR, FL, Back\n```\n\n- **FR (Front Right)**: 45° (positive for forward movement)\n- **FL (Front Left)**: 135° (negative for forward movement - counter-rotates with FR)\n- **Back**: 180° (activated only for significant sideways/rotation movement)\n\n**Key Improvements:**\n\n- ✅ **Strict Selective Usage**: Back wheel idle unless significant sideways/rotation needed\n- ✅ **Threshold-Based Control**: 10% threshold prevents unwanted wheel activation\n- ✅ **Proper Inverse Kinematics**: Uses `vx * cos(θ) + vy * sin(θ) + ω * R` formula\n- ✅ **Speed Normalization**: Prevents motor saturation when combining movements\n- ✅ **Acceleration Limiting**: Smooth transitions (adjust `max_acceleration` in code)\n- ✅ **Hexagonal Optimization**: Tuned for 135° wheel spacing\n\n**Motor Testing:** Use individual motor tests (`1-4` commands) to verify each wheel spins in the correct direction. Tests run at **near-maximum power (4000/4096)** for 3 seconds each direction with 1-second pauses.\n\n## Omni Wheel Configuration\n\nThe code is configured for your **3-wheel hexagonal omni robot**:\n\n- **Motor 1 (Lifter)**: Not used for navigation (angle = 0°)\n- **Motor 2 (Front Right)**: 45° from forward\n- **Motor 3 (Front Left)**: 135° from forward (for counter-rotation with FR)\n- **Motor 4 (Back)**: 180° from forward\n\n**Wheel Spacing**: 135° between adjacent wheels (hexagonal robot geometry)\n**Note**: This is a non-standard 3-wheel omni configuration. The inverse kinematics are calculated accordingly.\n\nAdjust `MOTOR_ANGLES[]` array if your physical wheel placement differs.\n\n## Usage\n\n1. **Upload the code** to Arduino Mega\n2. **Open Serial Monitor** (115200 baud)\n3. **Send commands** using single characters:\n   - `f` - Forward\n   - `b` - Backward\n   - `l` - Left\n   - `r` - Right\n   - `t` - Turn Left\n   - `y` - Turn Right\n   - `c` - Rotate CW\n   - `w` - Rotate CCW\n   - `u` - Lift Up\n   - `d` - Lift Down\n   - `s` - Stop\n   - `p` - Print Status\n\n## Tuning\n\n### PID Tuning\n\n1. Start with Kp=2.0, Ki=0.0, Kd=0.0\n2. Increase Ki until steady-state error is eliminated\n3. Add Kd to reduce overshoot\n4. Fine-tune Kp for faster response\n\n### Speed Tuning\n\nAdjust `BASE_SPEED`, `TURN_SPEED`, and `LIFT_SPEED` constants based on your requirements and motor capabilities.\n\n## Troubleshooting\n\n### Common Issues\n\n1. **Motors not moving**: Check power connections and motor driver enable pins\n2. **Erratic movement**: Verify encoder connections and polarities\n3. **Poor PID performance**: Tune PID parameters for your specific motors\n4. **Wrong direction**: Check motor wiring and `MOTOR_ANGLES` configuration\n\n### Debug Information\n\nUse the `p` command to print current motor status including:\n\n- Target RPM (setpoint)\n- Current RPM (input)\n- PID output values\n\n## Dependencies\n\n- **PID_v1 library**: Install via Arduino IDE Library Manager\n- **YFROBOT Motor Driver Library**: [https://github.com/YFROBOT-TM/Yfrobot-Motor-Driver-Library](https://github.com/YFROBOT-TM/Yfrobot-Motor-Driver-Library) - Required for YFROBOT v2 Motor Driver Shield control\n- Arduino Mega board\n- R27889 motor driver shield (YFROBOT v2 Shield)\n- 4x PG28 motors with encoders\n\n## License\n\nThis code is provided as-is for educational and robotics projects.\n\n---\n\n*Last updated: November 13, 2025*\n","project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2F1999azzar%2Fbody_kinematics","html_url":"https://awesome.ecosyste.ms/projects/github.com%2F1999azzar%2Fbody_kinematics","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2F1999azzar%2Fbody_kinematics/lists"}