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
Downloadable files
Connection Diagram for Raspberry Pi Pico W
Connection Diagram for Raspberry Pi Pico W
Connection Diagram for Arduino Nano RP2040 Connect
Connection Diagram for Arduino Nano RP2040 Connect
Model for Native Target
Model for Native Target
Comments
Only logged in users can leave comments