Published December 18, 2022

Arduino Based 4-Legged Mobile Robot Built From Scratch

A 4-legged mobile robot that tracks and wiggles its tail when it sees a human face. It can be controlled from remote PC through WiFi.

IntermediateWork in progress2 days1,390

Arduino Based 4-Legged Mobile Robot Built From Scratch

Things used in this project

Hardware components

Arduino Nano RP2040 Connect

Raspberry Pi Pico

Actually it is Raspberry Pi Pico W

M5Stack ESP32 Camera Module Development Board

12V to 6V DC Converter

High Torque Servo Motors

DIYables Servo Motor SG90 180 degree

Pimoroni Pan-Tilt HAT

Toggle Switch, Toggle

9V to 12V Battery, 2500mAH

Servo Horns

Robot mechanical componants

DIYables Jumper Wires

DIYables DC Power Jack Connector

Software apps and online services

Computer Aided Simulation Program (CASP)

Story

The development of terrestrial locomotion of legged robots has been continuously growing over the past few decades irrespective of the complexities involved in the design and development of these robots. This is because of the advantages in terms of maneuverability, transverse ability, ability to navigate on different terrains, efficiency, navigation over obstacles etc they have over wheel robot vehicles. The advantages of legged locomotion depend on the postures, the number of legs, and the functionality of the leg.

The quadruped robots are the best choice among all legged robots related to mobility and stability of locomotion. The four legs of the robot can be easily controlled, designed, and maintained as compared to two or six legs.

This example project demonstrates how to build a basic 4-legged WiFi remote controlled mobile robot with an on-board auto face tracking camera from scratch. It is completely controlled by the user manually from host PC through WiFi with following features

Forward, backward, left and right walking
Left and right rotation along its center
Takes left and right turn while moving forward and backward
Camera manual pan and tilt control along with flash LED
Camera auto face tracking when a human face is detected
Wiggle its tail when a human face is detected by the camera

Getting Started

Step 1: Build the hardware as described in the ‘Hardware Development’ section.

Step 2: Install latest version of CASP from this link: https://aadhuniklabs.com/?page_id=550. Please go through this link: https://aadhuniklabs.com/?page_id=554 for video tutorials on CASP.

Step 3: Download the example project on ‘4-Legged Mobile Robot with on-board Face Tracking Camera’ at this link: https://aadhuniklabs.com/casp/casp_web_projects/robotics/04_four_leg_robot.zip and follow the steps mentioned in the ‘Software Development’ section.

Step 4: Some adjustments as described in ‘Adjustments’ section are required for tuning the software to match your developed hardware. You may also further enhance the performance and features of the robot by modifying the source code from the custom blocks.

Step 5: Keyboard and mouse controls to control the robot are described in ‘Control Methodology’ section.

Step 6: Finally, safety precautions to be followed are described in 'Safety' section.

Basic Concepts of Development

1) Leg Axes

The mobile robot has 4 legs named as left front, left rear, right front and right rear. Each leg is considered as a 4 axis robotic arm. From the below figures, it can be evident that Axis-1 moves the leg along X direction (side ward movement) and Axes-2 & 3 moves the leg along Y direction (forward & backward movement). Axis-4 is the end effector that touches the ground. By controlling the angles of the servos mounted at each axis, the end effector can be moved to any desired co-ordinates along X, Y, Z within its range. Alternatively, we specify the X, Y, Z co-ordinates of the end effector of each leg based on gait generation and corresponding axes angles are calculated by applying forward and inverse kinematics.

2) Gait Generation

The below figure shows the path to be followed by the end effector of each leg. Polar co-ordinates are indicated with magnitude and angle specified for each point with respect to origin (Point-B). The corresponding array created in the software is also shown.

For a standing posture all four end effectors are at Point-B. The left front leg end effector is taken as reference for all walking and rotation movements. For most of the movements, the left front leg end effector path traversal starts from Point-B through Point-C, Point-D, Point-A, and back to Point-B. This completes one gait cycle. The right rear end effector path is same as the left front leg. The other two end effectors (left rear and right front) paths are offset by 180deg. That is, they path starts from Point-D, through Point-A, Point-B, Point-C and ends at Point-D to complete a gait cycle.The robot motion is taken into consideration for arriving at the target co-ordinates (X, Y, Z) of the end effectors as explained below.

For a forward motion the polar co-ordinates are converted to X, Y, Z along Y, Z direction with +Y pointing towards robot front.
For backward motion, this mapped co-ordinates along Y, Z direction are rotated in X, Y direction to 180deg.
For left right rotation, the mapped co-ordinates are rotated to +90deg or -90deg along X, Y direction depending on the rotation of the leg.

Below animation shows the path for various movements.

Once the end effector target point is calculated for a leg, its axes angles are calculated by applying forward and inverse kinematics.

3) Face Recognition/Tracking and Tail Wiggling

The mobile robot has an on-board camera with pan and tilt control. The software tries to recognize front human face during runtime. On detection, it generates a suitable signal to the servo that drives the robot tail to wiggle. Also, the software auto tracks the human face with the camera using two PID controllers and generates control signals that drive the pan and tilt servos of the camera. The two PID controllers one for horizontal tacking and the other for vertical tracking are tuned to align with the object to the center.

Hardware Development

User can build and assemble the mobile robot platform (i.e. chassis, legs etc) as shown in above pictures.

Arduino Nano RP2040 Connect and Raspberry Pi Pico W micro controller boards are independently used as the main controller of the robot. Connection diagram for both the micro controller boards are shown below. They control all the movements of the robot and communicates with the host PC through on-board WiFi.

The micro controller generates required PWM signals to control the leg servos. It also controls the servos of the camera pan & tilt assembly and tail wiggle assembly.

An ESP32 Camera module is mounted on the pan & tilt assembly for capturing live video and stream to the host PC.

A 12V battery with minimum 2200mAH capacity along with a 12V to 6V DC step down converter is used to power the entire circuitry on the robot.

Suitable high torque servo motors are used for robotic leg axes rotation.

Required electronic modules are suitably placed on the base frame and are connected as per the connection diagram shown in ‘Schematic’ section'.

Software Development

A) Configuring ESP32 Camera

ESP32 Camera shall be properly programmed with valid IP address before using it in the project. Please refer to our ESP32-CAM example for details on how to program the module. User may also refer to abundant material available on the internet regarding this subject.

B) Software for micro-controllers and host PC

CASP software is used to quickly create models and generate binary code for the on-board micro-controller target and native PC. This software enable users to graphically visualize the signal at any point of the model in real time. This feature is extensively used during adjusting robotic arm joint block parameters.Two models are developed to achieve desired objective.

B.1) Target Model that runs on Arduino Nano RP2040 Connect and Raspberry Pi Pico W consists of

1) Blink logic that indicates the system is running.2) WiFi101 block that receives required control signals from host PC.3) PWM and servo blocks that are mapped to the pins of the micro-controller.

Following are the steps to properly program the target board.

1) Connect the target to the host PC via a USB cable and note the serial port number from the host operating system.

2) Run CASP and load project ‘rc_arduino’ for Arduino RP2040 target or ‘rc_pico’ for Raspberry Pi Pico W target.

3) The WiFi101 block WiFi is set to Station mode. User may need to enter SSID and password of the network to which the device should be connected. The Local IP address parameter shall be configured as assigned by the network DHCP client of the network.

4) Open Home->Simulation->Setup Simulation Parameters menu item. Under TargetHW->General tabs set ‘Target Hardware Programmer Port’ parameter to the serial port to which the board is connected.

5) Build the model and program the board by clicking on Run button.

B.2) Native Model that runs on Host PC consists of

1) Face recognition logic consists of camera block and face recognition block. Camera block receives live video from the ESP32 Camera mounted on the mobile robot. IP address of the ESP32 Camera shall be entered in block parameters of this block.

2) Image display block (img) to display the live video received from the camera. It is also configured to output keyboard and mouse signals.

3) RC control block (rc_blk): It is a custom block that receives keyboard and mouse signals from the image display block and generates suitable control signals for controlling the robot motion. User can take a look at the configuration and source code of the block to better understand the logic and learn how to integrate the block in a CASP model along with other blocks. User can go through our video tutorial on how to create a custom block at this link https://aadhuniklabs.com/casp_res_videos#custom_block.

4) Robotic leg control block (custom_blk) embeds complete source code for proper gait generation and calculate XYZ co-ordinates for each leg end effector based on the control signals received from the’ rc_blk’. As it is a custom block, user can take a look at the configuration and source code of the block to better understand the logic.

5) Robotic leg axes blocks (4 number) takes input from the ‘custom_blk’ and calculates servo angles for each axis. It calculates forward and inverse kinematics for generating required axes angles to control the servo motors. The parameters of these blocks are configured to match with the respective leg segment dimensions/parameters.

6) 3D view blocks (4 numbers) attached to each leg block are used to plot each axis coordinates (as shown in below screen short). These can be used during testing for proper gait generation before connecting to the hardware.

7) WiFi communication block for communicating with the target hardware on the mobile robot.

Following are the steps to run the native model on the host PC

1) Before continuing, the host PC shall be connected to the same network as the device is connected.

2) Load the ‘rc_native’ project.

3) Click on Home->Simulation->Configure Simulation IO menu item.

4) ‘Configure Simulation Hardware’ window will open. Under Native Nodes and GPIO Device Nodes, change the IP addresses marked in the below figure (by double clicking on the item) to respective local and device IP addresses.

5) Click on ‘Connect Device’ button and check the ‘Online Data’ check box. The program should now communicate with the target with cycle time around 20msecs. Target board is now available as end point ‘EP0’ to the native model. Native model can use this end point to connect to respective IOs on the target.

6) After successful communication, user may need to enter a dummy address in the local IP and ensure that no communication happen during run time through this method as we don’t use this method for communicating with the target. We only use this method during testing.

7) Click on ‘Save’ button to save the configuration and close the window.

8) Now, configure the local and target IP addresses in the ‘Windows IP’ block from the model. These IP addresses shall be same as the addresses that worked while communicating to the target using the above method. A screen shot of the ‘Windows IP’ block parameter is shown below. Such communication methodology is chosen to reduce number of block count in the model.

9) Run the model by clicking on the Run button. A simulation panel window should open and communicate with the board.

10) Screen shot of the output simulation panel running on host PC is shown below. On some systems where graphics hardware cannot display the 3D plot correctly, user is advised to disable the 3D block.

Adjustments

1) The individual leg axes angle range and offset is set for each servo block in the target model based on the servo placement and alignment as shown in below figure. In the below figure each axis servo angle range (between -90deg to +90deg) with 0deg angle direction is shown. The polarities shown are indicative and vary for each leg based on the alignment of the servos. If user finds some mismatch in his/her design then it can be adjusted in the ‘Input Offset’ parameter of the ‘servo’ blocks in the target model.

2) The Axis-1/2/3/4 Settings (such as DH Arm Length, DH Arm Offset, DH Arm Twist and DH Arm Rotation Offset) of the Robotic Arm blocks for the 4 legs in the native model shall be adjusted based on the actual robot leg parameters built by the user.

3) Four 3D view blocks attached to the four legs can be used to view the axes movement while developing/building/testing the robot.

4) Manual control of each axis of the leg is possible. For this, user may need to enable the blocks grouped under ‘Logic for testing each joint of the legs’ shown in the below figure. Currently these blocks are disabled to bring down total blocks within the Free License version limitations. User can enable these block while disabling other non-related blocks such as blocks for face recognition and face tracking temporarily. The output of the above block group is given to all four leg blocks. In the leg blocks, user may also need to set ‘Runtime Control’ parameter of each axis to ‘Angle’ to enable manual control.

5) Base speed, speed limits and other parameters related to the navigation are adjustable from the ‘rc_block block and ‘custom_blk’ parameters.

Control Methodology

1) User can use keys W – to move forward, S – to move backward, A – to rotate left and D – to rotate right. Using the keys A and D along with the shift key moves the robot along left and right direction.

2) Combination of keys W/S & A/D can be used to take left and right turns while moving forward or backward.

3) Speed can be adjusted by using Page Up & Page Down keys.

4) Vertical and horizontal servo angles (from -90 to +90 deg) for head position control can be controlled by mouse movements.

5) Key ‘G’ is used to position both servos at default angle.

6) Key 'L' is used to ON/OFF the flash LED light of the ESP32 camera.

Safety

At times the robotic legs can go out of control. One way to slow down the leg movement is to adjust the servo block parameters (in the target model) to limit the rate of change of axes angles.Take adequate precautions to avoid injuries. Keep the piece out of reach from children at least during the development stage.

References

Please go through our video tutorials, tutorial projects and CASP main documentation for getting started with CASP. Link: https://aadhuniklabs.com/casp-resources.

Code

Source Code For native custom blocks

//CustomBlock865827210.h
#ifndef CUSTOMBLOCK865827210_H
#define CUSTOMBLOCK865827210_H

#include "sim_common_definitions.h"

//BLOCK_PREPROC_DEFS_HERE

struct LEG_STRUCT
{
    int index;
    XYZP offset, pt_prev;
    _UINT64 time_prev;
};

#define CustomBlock865827210_LEG_AR_SIZE 36
#define CustomBlock865827210_STAND_IDX 27

const _FLOAT g_CustomBlock865827210_leg_mag[CustomBlock865827210_LEG_AR_SIZE][3]/*{angle, radius, speed}*/
= {{0,700,1}, {10,900,1.2}, {20,980,1.4}, {30,1000,1.6}, {40,950,1.8}, {50,800,2}, {60,650,2}, {70,550,2}, {80,500,2}, {90,450,2},
   {100,440,2}, {110,433,2}, {120,450,2}, {130,476,2}, {140,535,2}, {150,630,1.8}, {160,800,1.6}, {170,1000,1.4}, {180,800,1.2},
   {180,720,1}, {180,640,1}, {180,560,1}, {180,480,1}, {180,400,1}, {180,320,1}, {180,240,1}, {180,160,1}, {180,80,1},
   {0,0,1}, {0,80,1}, {0,160,1}, {0,240,1}, {0,320,1}, {0,400,1}, {0,460,1}, {0,520,1}};

class SHARED_LIB_TYPE CustomBlock865827210
{
public:
    CustomBlock865827210();
    ~CustomBlock865827210();
    void Initialize(const volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT *arg);
    void PreRun(const volatile GLOBAL_PRERUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *ls_in, _INT32 *ld_in, _FLOAT *rs_in, _INT32 *rd_in, _FXDPT_FLOAT *lf_out, _FXDPT_FLOAT *lr_out, _FXDPT_FLOAT *rf_out, _FXDPT_FLOAT *rr_out, _INT32 *led_out);
    void Run(const volatile GLOBAL_RUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *ls_in, _INT32 *ld_in, _FLOAT *rs_in, _INT32 *rd_in, _FXDPT_FLOAT *lf_out, _FXDPT_FLOAT *lr_out, _FXDPT_FLOAT *rf_out, _FXDPT_FLOAT *rr_out, _INT32 *led_out);
    void PostRun(const volatile GLOBAL_POSTRUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *ls_in, _INT32 *ld_in, _FLOAT *rs_in, _INT32 *rd_in, _FXDPT_FLOAT *lf_out, _FXDPT_FLOAT *lr_out, _FXDPT_FLOAT *rf_out, _FXDPT_FLOAT *rr_out, _INT32 *led_out);
public:
    //port variable declaration
    //block internal parameter variable declaration
    _FLOAT base_speed, h_radius, v_radius;
    _FLOAT leg1_offset[3], leg2_offset[3], leg3_offset[3], leg4_offset[3];
    //block internal initial condition variable declaration
private:
    int GetPoint(int index, _FLOAT sp_factor, _FLOAT ang_xy, _FLOAT hmf, _FLOAT vmf, LEG_STRUCT *leg, XYZP *pt);
private:
    _UINT64 m_tick;
    _BYTE m_prev_led_kb;
    LEG_STRUCT m_legs[4];//lf, rf, lr, rr
};

#endif


//CustomBlock865827210.cpp
#include "CustomBlock865827210.h"
#include "simcode.h"
#include "sim_helper.h"
#include "math.h"

//each index corresponds to 10 deg starting from 0 deg.
#define LEG_AR_SIZE CustomBlock865827210_LEG_AR_SIZE
#define LEG_AR_MAX_IDX (LEG_AR_SIZE-1)
#define LEG_AR_HALF_IDX (LEG_AR_MAX_IDX/2)
#define STAND_IDX CustomBlock865827210_STAND_IDX

CustomBlock865827210::CustomBlock865827210(){
}

CustomBlock865827210::~CustomBlock865827210(){
}

void CustomBlock865827210::Initialize(const volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT *arg){
}

void CustomBlock865827210::PreRun(const volatile GLOBAL_PRERUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *ls_in, _INT32 *ld_in, _FLOAT *rs_in, _INT32 *rd_in, _FXDPT_FLOAT *lf_out, _FXDPT_FLOAT *lr_out, _FXDPT_FLOAT *rf_out, _FXDPT_FLOAT *rr_out, _INT32 *led_out){
    m_tick = arg->rt_clk_tick;
    for(int i=0; i<4; ++i)
    {
        m_legs[i].index = STAND_IDX;
        m_legs[i].time_prev = arg->rt_clk_tick;
        GetPoint(m_legs[i].index, -1, 0, 1, 1, &m_legs[i], 0);
    }
    m_legs[0].offset.x = leg1_offset[0];
    m_legs[0].offset.y = leg1_offset[1];
    m_legs[0].offset.z = leg1_offset[2];
    //
    m_legs[1].offset.x = leg2_offset[0];
    m_legs[1].offset.y = leg2_offset[1];
    m_legs[1].offset.z = leg2_offset[2];
    //
    m_legs[2].offset.x = leg3_offset[0];
    m_legs[2].offset.y = leg3_offset[1];
    m_legs[2].offset.z = leg3_offset[2];
    //
    m_legs[3].offset.x = leg4_offset[0];
    m_legs[3].offset.y = leg4_offset[1];
    m_legs[3].offset.z = leg4_offset[2];
    //
    base_speed = base_speed/1000;//convert to mm/msec
    m_prev_led_kb = 0;
}

void CustomBlock865827210::Run(const volatile GLOBAL_RUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *ls_in, _INT32 *ld_in, _FLOAT *rs_in, _INT32 *rd_in, _FXDPT_FLOAT *lf_out, _FXDPT_FLOAT *lr_out, _FXDPT_FLOAT *rf_out, _FXDPT_FLOAT *rr_out, _INT32 *led_out){
    //if(arg->rt_clk_tick-m_tick < 1000)
    //    return;
    m_tick = arg->rt_clk_tick;
    //flash light led logic
    if(kb_in[76])//L
    {
        if(m_prev_led_kb == 0)
        {
            m_prev_led_kb = 1;
            *led_out = (*led_out)?0:1;
        }
    }
    else
        m_prev_led_kb = 0;
    //
    _FLOAT ls = *ls_in;
    _FLOAT rs = *rs_in;
    _FLOAT sp = (ls + rs)/2;//average speed
    int ldir = *ld_in;//1-forward, 0-backward
    int rdir = (*rd_in)?0:1;
    if(sp < 0.001)//standing
    {
        sp = 1.0;
        m_legs[0].index = m_legs[1].index = m_legs[2].index = m_legs[3].index = STAND_IDX;
        GetPoint(m_legs[0].index, sp, 0, 1, 1, &m_legs[0], (XYZP*)lf_out);
        GetPoint(m_legs[1].index, sp, 0, 1, 1, &m_legs[1], (XYZP*)lr_out);
        GetPoint(m_legs[2].index, sp, 0, 1, 1, &m_legs[2], (XYZP*)rf_out);
        GetPoint(m_legs[3].index, sp, 0, 1, 1, &m_legs[3], (XYZP*)rr_out);
    }
    else//walk
    {
        int leg_mag_idx[4];
        _FLOAT leg_ang[4];
        _FLOAT hmul, vmul;
        //
        leg_mag_idx[0] = m_legs[0].index+0;//left front leg
        leg_mag_idx[1] = m_legs[1].index+LEG_AR_HALF_IDX;//left rear leg
        leg_mag_idx[2] = m_legs[2].index+LEG_AR_HALF_IDX;//right front leg
        leg_mag_idx[3] = m_legs[3].index+0;//right rear leg
        //
        if(ldir == 1 && rdir == 0)//rotate right
        {
            if(kb_in[16])
            {
                leg_ang[0] = leg_ang[2] = 90;
                leg_ang[1] = leg_ang[3] = 90;
            }
            else
            {
                leg_ang[0] = leg_ang[2] = 90;
                leg_ang[1] = leg_ang[3] = -90;
            }
            //
            hmul = 0.5;
            vmul = 1.0;
            sp *= 1.2;
        }
        else if(ldir == 0 && rdir == 1)//rotate left
        {
            if(kb_in[16])
            {
                leg_ang[0] = leg_ang[2] = -90;
                leg_ang[1] = leg_ang[3] = -90;
            }
            else
            {
                leg_ang[0] = leg_ang[2] = -90;
                leg_ang[1] = leg_ang[3] = 90;
            }
            //
            hmul = 0.5;
            vmul = 1.0;
            sp *= 1.2;
        }
        else if(ldir == 1 && rdir == 1)//move forward
        {
            if(IsEqual(ls, rs))
            {
                leg_ang[0] = leg_ang[1] = leg_ang[2] = leg_ang[3] = 0;
            }
            else if(ls > rs)//go right
            {
                leg_ang[0] = leg_ang[2] = 30;
                leg_ang[1] = leg_ang[3] = -30;
            }
            else//ls < rs, go left
            {
                leg_ang[0] = leg_ang[2] = -30;
                leg_ang[1] = leg_ang[3] = 30;
            }
            hmul = vmul = 1.0;
        }
        else//move backward
        {
            if(IsEqual(ls, rs))
            {
                leg_ang[0] = leg_ang[1] = leg_ang[2] = leg_ang[3] = 180;
            }
            else if(ls > rs)//go right
            {
                leg_ang[0] = leg_ang[2] = (180+30);
                leg_ang[1] = leg_ang[3] = (180-30);
            }
            else//ls < rs, go left
            {
                leg_ang[0] = leg_ang[2] = (180-30);
                leg_ang[1] = leg_ang[3] = (180+30);
            }
            hmul = vmul = 1.0;
        }
        //get current x,y,z co-ordinates for all 4 legs
        int ret = GetPoint(leg_mag_idx[0], sp, leg_ang[0], hmul, vmul, &m_legs[0], (XYZP*)lf_out);
        m_legs[0].index += (ret && (m_legs[0].index<LEG_AR_MAX_IDX))?1:0;
        //
        ret = GetPoint(leg_mag_idx[1], sp, leg_ang[1], hmul, vmul, &m_legs[1], (XYZP*)lr_out);
        m_legs[1].index += (ret && (m_legs[1].index<LEG_AR_MAX_IDX))?1:0;
        //
        ret = GetPoint(leg_mag_idx[2], sp, leg_ang[2], hmul, vmul, &m_legs[2], (XYZP*)rf_out);
        m_legs[2].index += (ret && (m_legs[2].index<LEG_AR_MAX_IDX))?1:0;
        //
        ret = GetPoint(leg_mag_idx[3], sp, leg_ang[3], hmul, vmul, &m_legs[3], (XYZP*)rr_out);
        m_legs[3].index += (ret && (m_legs[3].index<LEG_AR_MAX_IDX))?1:0;
        //synchronise all four legs
        if(m_legs[0].index == LEG_AR_MAX_IDX &&  m_legs[1].index == LEG_AR_MAX_IDX && m_legs[2].index == LEG_AR_MAX_IDX && m_legs[3].index == LEG_AR_MAX_IDX)
        {
            m_legs[0].index = 0;
            m_legs[1].index = 0;
            m_legs[2].index = 0;
            m_legs[3].index = 0;
        }
    }
}

void CustomBlock865827210::PostRun(const volatile GLOBAL_POSTRUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *ls_in, _INT32 *ld_in, _FLOAT *rs_in, _INT32 *rd_in, _FXDPT_FLOAT *lf_out, _FXDPT_FLOAT *lr_out, _FXDPT_FLOAT *rf_out, _FXDPT_FLOAT *rr_out, _INT32 *led_out){
    //standing
    _FLOAT sp = 1.0;
    m_legs[0].index = m_legs[1].index = m_legs[2].index = m_legs[3].index = STAND_IDX;
    GetPoint(m_legs[0].index, sp, 0, 1, 1, &m_legs[0], (XYZP*)lf_out);
    GetPoint(m_legs[1].index, sp, 0, 1, 1, &m_legs[1], (XYZP*)lr_out);
    GetPoint(m_legs[2].index, sp, 0, 1, 1, &m_legs[2], (XYZP*)rf_out);
    GetPoint(m_legs[3].index, sp, 0, 1, 1, &m_legs[3], (XYZP*)rr_out);
}

int CustomBlock865827210::GetPoint(int index, _FLOAT sp_factor, _FLOAT ang_xy, _FLOAT hmf, _FLOAT vmf, LEG_STRUCT *leg, XYZP *pt)
{
    if(IsZero(sp_factor))
    {
        leg->time_prev = m_tick;
        return 1;
    }
    //check limits
    if(index < 0) index = LEG_AR_SIZE + index;
    if(index > LEG_AR_MAX_IDX) index = index-LEG_AR_SIZE;
    //
    _FLOAT ang = g_CustomBlock865827210_leg_mag[index][0];//angle
    _FLOAT rad = g_CustomBlock865827210_leg_mag[index][1]/1000.0;//radius
    _FLOAT speed = g_CustomBlock865827210_leg_mag[index][2];//speed
    XYZP p;
    p.x = 0;
    p.y = -rad * cos(ang * MATH_DEG_TO_RAD) * h_radius * hmf;
    p.z = rad * sin(ang * MATH_DEG_TO_RAD) * v_radius * vmf;
    //convert to user angle
    ang = ang_xy;
    rad = p.y;
    p.x = rad * sin(ang * MATH_DEG_TO_RAD);//sin because we r taking along y as angle zero
    p.y = rad * cos(ang * MATH_DEG_TO_RAD);
    //adjust point wrt speed. only if sp_factor > 0
    int ret = 1;
    if(sp_factor > 0.0001)
    {
        _FLOAT len = GetDistance(&p, &leg->pt_prev);
        if(len > 0.001)
        {
            _FLOAT td = (m_tick-leg->time_prev)/1000.0;//in msec
            _FLOAT dis = td * (base_speed*sp_factor*speed);
            _FXDPT_FLOAT ratio = dis/len;
            if(ratio > 1) ratio = 1;
            p.x = leg->pt_prev.x + ((p.x-leg->pt_prev.x)*ratio);
            p.y = leg->pt_prev.y + ((p.y-leg->pt_prev.y)*ratio);
            p.z = leg->pt_prev.z + ((p.z-leg->pt_prev.z)*ratio);
            ret = 0;
        }
    }
    leg->time_prev = m_tick;
    leg->pt_prev.x = p.x;
    leg->pt_prev.y = p.y;
    leg->pt_prev.z = p.z;
    if(pt)
    {
        //translate
        p.x = p.x + leg->offset.x;
        p.y = p.y + leg->offset.y;
        p.z = p.z + leg->offset.z;
        //
        pt->x = p.x;
        pt->y = p.y;
        pt->z = p.z;
    }
    return ret;
}

Credits

Aadhuniklabs

7 projects • 15 followers

Makers and tech enthusiast since 2022

Arduino Based 4-Legged Mobile Robot Built From Scratch

Things used in this project

Hardware components

Software apps and online services

Story

Getting Started

Basic Concepts of Development

Hardware Development

Software Development

Adjustments

Control Methodology

Safety

References

Schematics

Connection Diagram for Arduino Nano RP2040 Connect

Connection Diagram for Raspberry Pi Pico W

Model for Native Target

Code

Source Code For native custom blocks

Credits

Aadhuniklabs

Comments

Embed the widget on your own site

Arduino Based 4-Legged Mobile Robot Built From Scratch

Arduino Based 4-Legged Mobile Robot Built From Scratch

Things used in this project

Hardware components

Software apps and online services

Story

Getting Started

Basic Concepts of Development

Hardware Development

Software Development

Adjustments

Control Methodology

Safety

References

Schematics

Connection Diagram for Arduino Nano RP2040 Connect

Connection Diagram for Raspberry Pi Pico W

Model for Native Target

Code

Source Code For native custom blocks

Credits

Aadhuniklabs

Comments

Related channels and tags