Arduino Based 4-Legged Mobile Robot Built From Scratch

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