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.

Dec 18, 2022

•

1426 views

•

2 respects

Components and supplies

Jumper Wires

ESP32 Camera Module Development Board

Arduino Nano RP2040 Connect

Robot mechanical componants

Raspberry Pi Pico

High Torque Servo Motors

DC Power Jack

Toggle Switch, Toggle

Servo Horns

12V to 6V DC Converter

9V to 12V Battery, 2500mAH

Servo Motor SG90 180 degree

Pan-Tilt HAT

Apps and platforms

Computer Aided Simulation Program (CASP)

Project description

Code

Source Code For native custom blocks

c_cpp

1//CustomBlock865827210.h
2#ifndef CUSTOMBLOCK865827210_H
3#define  CUSTOMBLOCK865827210_H
4
5#include "sim_common_definitions.h"
6
7//BLOCK_PREPROC_DEFS_HERE
8
9struct  LEG_STRUCT
10{
11    int index;
12    XYZP offset, pt_prev;
13    _UINT64 time_prev;
14};
15
16#define  CustomBlock865827210_LEG_AR_SIZE 36
17#define CustomBlock865827210_STAND_IDX 27
18
19const  _FLOAT g_CustomBlock865827210_leg_mag[CustomBlock865827210_LEG_AR_SIZE][3]/*{angle,  radius, speed}*/
20= {{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},
21   {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},
22   {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},
23   {0,0,1}, {0,80,1}, {0,160,1},  {0,240,1}, {0,320,1}, {0,400,1}, {0,460,1}, {0,520,1}};
24
25class SHARED_LIB_TYPE  CustomBlock865827210
26{
27public:
28    CustomBlock865827210();
29    ~CustomBlock865827210();
30    void Initialize(const volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT *arg);
31    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);
32    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);
33    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);
34public:
35    //port  variable declaration
36    //block internal parameter variable declaration
37    _FLOAT base_speed, h_radius, v_radius;
38    _FLOAT leg1_offset[3], leg2_offset[3],  leg3_offset[3], leg4_offset[3];
39    //block internal initial condition variable  declaration
40private:
41    int GetPoint(int index, _FLOAT sp_factor, _FLOAT  ang_xy, _FLOAT hmf, _FLOAT vmf, LEG_STRUCT *leg, XYZP *pt);
42private:
43    _UINT64  m_tick;
44    _BYTE m_prev_led_kb;
45    LEG_STRUCT m_legs[4];//lf, rf, lr, rr
46};
47
48#endif
49
50
51//CustomBlock865827210.cpp
52#include  "CustomBlock865827210.h"
53#include "simcode.h"
54#include "sim_helper.h"
55#include  "math.h"
56
57//each index corresponds to 10 deg starting from 0 deg.
58#define  LEG_AR_SIZE CustomBlock865827210_LEG_AR_SIZE
59#define LEG_AR_MAX_IDX (LEG_AR_SIZE-1)
60#define  LEG_AR_HALF_IDX (LEG_AR_MAX_IDX/2)
61#define STAND_IDX CustomBlock865827210_STAND_IDX
62
63CustomBlock865827210::CustomBlock865827210(){
64}
65
66CustomBlock865827210::~CustomBlock865827210(){
67}
68
69void  CustomBlock865827210::Initialize(const volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT *arg){
70}
71
72void  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){
73    m_tick = arg->rt_clk_tick;
74    for(int i=0; i<4; ++i)
75    {
76        m_legs[i].index = STAND_IDX;
77        m_legs[i].time_prev = arg->rt_clk_tick;
78        GetPoint(m_legs[i].index, -1, 0, 1, 1, &m_legs[i], 0);
79    }
80    m_legs[0].offset.x  = leg1_offset[0];
81    m_legs[0].offset.y = leg1_offset[1];
82    m_legs[0].offset.z  = leg1_offset[2];
83    //
84    m_legs[1].offset.x = leg2_offset[0];
85    m_legs[1].offset.y  = leg2_offset[1];
86    m_legs[1].offset.z = leg2_offset[2];
87    //
88    m_legs[2].offset.x  = leg3_offset[0];
89    m_legs[2].offset.y = leg3_offset[1];
90    m_legs[2].offset.z  = leg3_offset[2];
91    //
92    m_legs[3].offset.x = leg4_offset[0];
93    m_legs[3].offset.y  = leg4_offset[1];
94    m_legs[3].offset.z = leg4_offset[2];
95    //
96    base_speed  = base_speed/1000;//convert to mm/msec
97    m_prev_led_kb = 0;
98}
99
100void  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){
101    //if(arg->rt_clk_tick-m_tick < 1000)
102    //    return;
103    m_tick = arg->rt_clk_tick;
104    //flash light led logic
105    if(kb_in[76])//L
106    {
107        if(m_prev_led_kb  == 0)
108        {
109            m_prev_led_kb = 1;
110            *led_out = (*led_out)?0:1;
111        }
112    }
113    else
114        m_prev_led_kb = 0;
115    //
116    _FLOAT  ls = *ls_in;
117    _FLOAT rs = *rs_in;
118    _FLOAT sp = (ls + rs)/2;//average  speed
119    int ldir = *ld_in;//1-forward, 0-backward
120    int rdir = (*rd_in)?0:1;
121    if(sp < 0.001)//standing
122    {
123        sp = 1.0;
124        m_legs[0].index  = m_legs[1].index = m_legs[2].index = m_legs[3].index = STAND_IDX;
125        GetPoint(m_legs[0].index,  sp, 0, 1, 1, &m_legs[0], (XYZP*)lf_out);
126        GetPoint(m_legs[1].index, sp,  0, 1, 1, &m_legs[1], (XYZP*)lr_out);
127        GetPoint(m_legs[2].index, sp, 0,  1, 1, &m_legs[2], (XYZP*)rf_out);
128        GetPoint(m_legs[3].index, sp, 0, 1,  1, &m_legs[3], (XYZP*)rr_out);
129    }
130    else//walk
131    {
132        int  leg_mag_idx[4];
133        _FLOAT leg_ang[4];
134        _FLOAT hmul, vmul;
135        //
136        leg_mag_idx[0] = m_legs[0].index+0;//left front leg
137        leg_mag_idx[1]  = m_legs[1].index+LEG_AR_HALF_IDX;//left rear leg
138        leg_mag_idx[2] = m_legs[2].index+LEG_AR_HALF_IDX;//right  front leg
139        leg_mag_idx[3] = m_legs[3].index+0;//right rear leg
140        //
141        if(ldir == 1 && rdir == 0)//rotate right
142        {
143            if(kb_in[16])
144            {
145                leg_ang[0] = leg_ang[2] = 90;
146                leg_ang[1]  = leg_ang[3] = 90;
147            }
148            else
149            {
150                leg_ang[0]  = leg_ang[2] = 90;
151                leg_ang[1] = leg_ang[3] = -90;
152            }
153            //
154            hmul = 0.5;
155            vmul = 1.0;
156            sp  *= 1.2;
157        }
158        else if(ldir == 0 && rdir == 1)//rotate left
159        {
160            if(kb_in[16])
161            {
162                leg_ang[0]  = leg_ang[2] = -90;
163                leg_ang[1] = leg_ang[3] = -90;
164            }
165            else
166            {
167                leg_ang[0] = leg_ang[2] = -90;
168                leg_ang[1] = leg_ang[3] = 90;
169            }
170            //
171            hmul = 0.5;
172            vmul = 1.0;
173            sp *= 1.2;
174        }
175        else if(ldir == 1 && rdir == 1)//move forward
176        {
177            if(IsEqual(ls, rs))
178            {
179                leg_ang[0] =  leg_ang[1] = leg_ang[2] = leg_ang[3] = 0;
180            }
181            else if(ls  > rs)//go right
182            {
183                leg_ang[0] = leg_ang[2] = 30;
184                leg_ang[1] = leg_ang[3] = -30;
185            }
186            else//ls  < rs, go left
187            {
188                leg_ang[0] = leg_ang[2] = -30;
189                leg_ang[1] = leg_ang[3] = 30;
190            }
191            hmul  = vmul = 1.0;
192        }
193        else//move backward
194        {
195            if(IsEqual(ls,  rs))
196            {
197                leg_ang[0] = leg_ang[1] = leg_ang[2] =  leg_ang[3] = 180;
198            }
199            else if(ls > rs)//go right
200            {
201                leg_ang[0] = leg_ang[2] = (180+30);
202                leg_ang[1]  = leg_ang[3] = (180-30);
203            }
204            else//ls < rs, go left
205            {
206                leg_ang[0] = leg_ang[2] = (180-30);
207                leg_ang[1]  = leg_ang[3] = (180+30);
208            }
209            hmul = vmul = 1.0;
210        }
211        //get current x,y,z co-ordinates for all 4 legs
212        int  ret = GetPoint(leg_mag_idx[0], sp, leg_ang[0], hmul, vmul, &m_legs[0], (XYZP*)lf_out);
213        m_legs[0].index += (ret && (m_legs[0].index<LEG_AR_MAX_IDX))?1:0;
214        //
215        ret = GetPoint(leg_mag_idx[1], sp, leg_ang[1], hmul, vmul, &m_legs[1], (XYZP*)lr_out);
216        m_legs[1].index += (ret && (m_legs[1].index<LEG_AR_MAX_IDX))?1:0;
217        //
218        ret = GetPoint(leg_mag_idx[2], sp, leg_ang[2], hmul, vmul, &m_legs[2], (XYZP*)rf_out);
219        m_legs[2].index += (ret && (m_legs[2].index<LEG_AR_MAX_IDX))?1:0;
220        //
221        ret = GetPoint(leg_mag_idx[3], sp, leg_ang[3], hmul, vmul, &m_legs[3], (XYZP*)rr_out);
222        m_legs[3].index += (ret && (m_legs[3].index<LEG_AR_MAX_IDX))?1:0;
223        //synchronise  all four legs
224        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)
225        {
226            m_legs[0].index = 0;
227            m_legs[1].index = 0;
228            m_legs[2].index = 0;
229            m_legs[3].index = 0;
230        }
231    }
232}
233
234void 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){
235    //standing
236    _FLOAT sp = 1.0;
237    m_legs[0].index  = m_legs[1].index = m_legs[2].index = m_legs[3].index = STAND_IDX;
238    GetPoint(m_legs[0].index,  sp, 0, 1, 1, &m_legs[0], (XYZP*)lf_out);
239    GetPoint(m_legs[1].index, sp, 0,  1, 1, &m_legs[1], (XYZP*)lr_out);
240    GetPoint(m_legs[2].index, sp, 0, 1, 1,  &m_legs[2], (XYZP*)rf_out);
241    GetPoint(m_legs[3].index, sp, 0, 1, 1, &m_legs[3],  (XYZP*)rr_out);
242}
243
244int CustomBlock865827210::GetPoint(int index, _FLOAT  sp_factor, _FLOAT ang_xy, _FLOAT hmf, _FLOAT vmf, LEG_STRUCT *leg, XYZP *pt)
245{
246    if(IsZero(sp_factor))
247    {
248        leg->time_prev = m_tick;
249        return  1;
250    }
251    //check limits
252    if(index < 0) index = LEG_AR_SIZE + index;
253    if(index > LEG_AR_MAX_IDX) index = index-LEG_AR_SIZE;
254    //
255    _FLOAT  ang = g_CustomBlock865827210_leg_mag[index][0];//angle
256    _FLOAT rad = g_CustomBlock865827210_leg_mag[index][1]/1000.0;//radius
257    _FLOAT speed = g_CustomBlock865827210_leg_mag[index][2];//speed
258    XYZP  p;
259    p.x = 0;
260    p.y = -rad * cos(ang * MATH_DEG_TO_RAD) * h_radius * hmf;
261    p.z = rad * sin(ang * MATH_DEG_TO_RAD) * v_radius * vmf;
262    //convert to  user angle
263    ang = ang_xy;
264    rad = p.y;
265    p.x = rad * sin(ang * MATH_DEG_TO_RAD);//sin  because we r taking along y as angle zero
266    p.y = rad * cos(ang * MATH_DEG_TO_RAD);
267    //adjust point wrt speed. only if sp_factor > 0
268    int ret = 1;
269    if(sp_factor  > 0.0001)
270    {
271        _FLOAT len = GetDistance(&p, &leg->pt_prev);
272        if(len  > 0.001)
273        {
274            _FLOAT td = (m_tick-leg->time_prev)/1000.0;//in  msec
275            _FLOAT dis = td * (base_speed*sp_factor*speed);
276            _FXDPT_FLOAT  ratio = dis/len;
277            if(ratio > 1) ratio = 1;
278            p.x = leg->pt_prev.x  + ((p.x-leg->pt_prev.x)*ratio);
279            p.y = leg->pt_prev.y + ((p.y-leg->pt_prev.y)*ratio);
280            p.z = leg->pt_prev.z + ((p.z-leg->pt_prev.z)*ratio);
281            ret  = 0;
282        }
283    }
284    leg->time_prev = m_tick;
285    leg->pt_prev.x  = p.x;
286    leg->pt_prev.y = p.y;
287    leg->pt_prev.z = p.z;
288    if(pt)
289    {
290        //translate
291        p.x = p.x + leg->offset.x;
292        p.y  = p.y + leg->offset.y;
293        p.z = p.z + leg->offset.z;
294        //
295        pt->x  = p.x;
296        pt->y = p.y;
297        pt->z = p.z;
298    }
299    return ret;
300}
301

//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;
}

Downloadable files

Connection Diagram for Raspberry Pi Pico W