4 DOF Mobile Arm Robot with on-board camera

Arduino Nano RP2040 or Raspberry Pi Pico W based mobile arm robot that can be controlled remotely from a host PC through WiFi.

Oct 17, 2022

•

4607 views

•

3 respects

Components and supplies

Jumper wires (generic)

Jumper Wires

Arduino Nano RP2040 Connect

Robotic Arm Kit

Raspberry Pi Pico

9V to 12V Battery

12V to 6V Buck Converter

Robot Base Frame / Chassis

ESP32 Camera Module Development Board

VNH3ASP30 DC Motor Driver

DC Power Jack

Robot Wheels

Geared DC Motor, 12 V

High Torque Servo Motors

Apps and platforms

Computer Aided Simulation Program (CASP)

Project description

Code

Custom Block Code

c_cpp

1//CustomBlock193083739.h
2
3#ifndef CUSTOMBLOCK193083739_H
4#define  CUSTOMBLOCK193083739_H
5
6#include "sim_common_definitions.h"
7
8//BLOCK_PREPROC_DEFS_HERE
9
10class  SHARED_LIB_TYPE CustomBlock193083739
11{
12public:
13    CustomBlock193083739();
14    ~CustomBlock193083739();
15    void Initialize(const volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT  *arg);
16    void PreRun(const volatile GLOBAL_PRERUN_PARAM_STRUCT *arg, _INT32  *kb_in, _FXDPT_FLOAT *j_in, _INT32 *en_out, _FXDPT_FLOAT *xyz_out, _INT32 *led_out,  _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT *jaw_out);
17    void Run(const volatile GLOBAL_RUN_PARAM_STRUCT  *arg, _INT32 *kb_in, _FXDPT_FLOAT *j_in, _INT32 *en_out, _FXDPT_FLOAT *xyz_out,  _INT32 *led_out, _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT *jaw_out);
18    void PostRun(const  volatile GLOBAL_POSTRUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FXDPT_FLOAT *j_in, _INT32  *en_out, _FXDPT_FLOAT *xyz_out, _INT32 *led_out, _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT  *jaw_out);
19public:
20    //block internal parameter variable declaration
21    _FXDPT_FLOAT32 mouse_sen, def_x, def_y, def_z;
22    //block internal initial  condition variable declaration
23private:
24    _UINT64 m_prev_tick;
25    _BYTE  m_prev_led_kb, m_prev_en_kb, m_prev_park_kb;
26    int m_en_mouse;
27    int m_prev_mouse_pos[3];
28};
29
30#endif
31
32//CustomBlock193083739.cpp
33#include  "CustomBlock193083739.h"
34#include "simcode.h"
35#include "math.h"
36#include  "sim_helper.h"
37
38CustomBlock193083739::CustomBlock193083739(){
39}
40
41CustomBlock193083739::~CustomBlock193083739(){
42}
43
44void  CustomBlock193083739::Initialize(const volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT *arg){
45}
46
47void  CustomBlock193083739::PreRun(const volatile GLOBAL_PRERUN_PARAM_STRUCT *arg, _INT32  *kb_in, _FXDPT_FLOAT *j_in, _INT32 *en_out, _FXDPT_FLOAT *xyz_out, _INT32 *led_out,  _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT *jaw_out){
48    *led_out = 0;
49    *en_out  = -2;//first run
50    xyz_out[0] = def_x;
51    xyz_out[1] = def_y;
52    xyz_out[2]  = def_z;
53    *tw_out = 0;
54    *jaw_out = 0.5;
55    //
56    m_prev_mouse_pos[0]  = kb_in[257];
57    m_prev_mouse_pos[1] = kb_in[258];
58    m_prev_mouse_pos[2]  = kb_in[259];
59    //
60    m_prev_tick = arg->rt_clk_tick;
61    m_prev_led_kb  = m_prev_en_kb = m_prev_park_kb = 0;
62}
63
64void CustomBlock193083739::Run(const  volatile GLOBAL_RUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FXDPT_FLOAT *j_in, _INT32  *en_out, _FXDPT_FLOAT *xyz_out, _INT32 *led_out, _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT  *jaw_out){
65    if(arg->rt_clk_tick-m_prev_tick < 10000)
66        return;
67    m_prev_tick = arg->rt_clk_tick;
68    int x = kb_in[257];
69    int y = kb_in[258];
70    int z = kb_in[259];
71    int diffx = x - m_prev_mouse_pos[0];m_prev_mouse_pos[0]  = x;
72    int diffy = y - m_prev_mouse_pos[1];m_prev_mouse_pos[1] = y;
73    int  diffz = z - m_prev_mouse_pos[2];m_prev_mouse_pos[2] = z;
74    //flash light led  logic
75    if(kb_in[76])//L
76    {
77        if(m_prev_led_kb == 0)
78        {
79            m_prev_led_kb = 1;
80            *led_out = (*led_out)?0:1;
81        }
82    }
83    else
84        m_prev_led_kb = 0;
85    //park logic
86    if(*en_out  > -2)
87    {
88        if(kb_in[80])//P. press P until it is parked
89        {
90            *en_out = -1;
91        }
92        else
93        {
94            //enable  disable x,y,z control
95            if(kb_in[69])//E
96            {
97                if(m_prev_en_kb  == 0)
98                    m_prev_en_kb = 1;
99            }
100            else
101                m_prev_en_kb = 0;
102            *en_out = 1;
103        }
104    }
105    //update x,y,z
106    if(*en_out < 0)
107    {
108        xyz_out[0] = def_x;
109        xyz_out[1] = def_y;
110        xyz_out[2] = def_z;
111        //
112        *tw_out  = 0;
113        *jaw_out = 0.5;
114    }
115    else if(*en_out > 0)
116    {
117        //x,y
118        if(diffx != 0 || diffy != 0)
119        {
120            //convert  curren x,y to polar
121            _FXDPT_FLOAT y = j_in[0];
122            _FXDPT_FLOAT  x = j_in[1];
123            _FXDPT_FLOAT mag = sqrt(x*x + y*y);
124            _FXDPT_FLOAT  ang = 0;
125            if(!IsZero(x))
126                ang = atan2(y,x) * MATH_RAD_TO_DEG;
127            if(diffx != 0)
128            {
129                if(kb_in[256] & 0b10)
130                {
131                    //update theta
132                    _FXDPT_FLOAT  theta = ((_FXDPT_FLOAT)diffx * mouse_sen * ((kb_in[17]/*ctrl*/)?0.01:0.1));
133                    ang += theta;
134                    //dont set to 90. sometimes  solution is out of bounds
135                    if(ang > 85) ang = 85;
136                    if(ang  < -85) ang = -85;
137                }
138
139            }
140            if(diffy  != 0)
141            {
142                if(kb_in[256] & 0b10)
143                {
144                    //update radius
145                    _FXDPT_FLOAT radius =  ((_FXDPT_FLOAT)diffy * mouse_sen * ((kb_in[17]/*ctrl*/)?0.05:0.5));
146                    mag  -= radius;
147                }
148            }
149            //convert polar  back to rectangular
150            if(kb_in[256] & 0b10)
151            {
152                _FXDPT_FLOAT  x = mag * cos(ang * MATH_DEG_TO_RAD);
153                _FXDPT_FLOAT y = mag *  sin(ang * MATH_DEG_TO_RAD);
154                xyz_out[0] = y;
155                xyz_out[1]  = x;
156            }
157        }
158        //z
159        if(diffz != 0)
160        {
161            _FXDPT_FLOAT d = (_FXDPT_FLOAT)diffz * mouse_sen;
162            d  *= ((kb_in[17]/*ctrl*/)?0.01:0.1);
163            xyz_out[2] = j_in[2]-d;
164        }
165    }
166    //twist control
167    if(kb_in[84])//T
168    {
169        *tw_out  += 1;
170        *tw_out = (*tw_out > 90)?90:*tw_out;
171    }
172    else if(kb_in[89])//Y
173    {
174        *tw_out -= 1;
175        *tw_out = (*tw_out < -90)?-90:*tw_out;
176    }
177    //jaw control
178    if(kb_in[74])//J
179    {
180        *jaw_out  += (kb_in[16]/*shift*/)?0.1:0.01;
181        *jaw_out = (*jaw_out > 1)?1:*jaw_out;
182    }
183    else if(kb_in[75])//K
184    {
185        *jaw_out -= (kb_in[16]/*shift*/)?0.1:0.01;
186        *jaw_out = (*jaw_out < 0)?0:*jaw_out;
187    }
188    if(*en_out == -2)  *en_out = -1;
189}
190
191void CustomBlock193083739::PostRun(const volatile GLOBAL_POSTRUN_PARAM_STRUCT  *arg, _INT32 *kb_in, _FXDPT_FLOAT *j_in, _INT32 *en_out, _FXDPT_FLOAT *xyz_out,  _INT32 *led_out, _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT *jaw_out){
192}
193

//CustomBlock193083739.h

#ifndef CUSTOMBLOCK193083739_H
#define  CUSTOMBLOCK193083739_H

#include "sim_common_definitions.h"

//BLOCK_PREPROC_DEFS_HERE

class  SHARED_LIB_TYPE CustomBlock193083739
{
public:
    CustomBlock193083739();
    ~CustomBlock193083739();
    void Initialize(const volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT  *arg);
    void PreRun(const volatile GLOBAL_PRERUN_PARAM_STRUCT *arg, _INT32  *kb_in, _FXDPT_FLOAT *j_in, _INT32 *en_out, _FXDPT_FLOAT *xyz_out, _INT32 *led_out,  _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT *jaw_out);
    void Run(const volatile GLOBAL_RUN_PARAM_STRUCT  *arg, _INT32 *kb_in, _FXDPT_FLOAT *j_in, _INT32 *en_out, _FXDPT_FLOAT *xyz_out,  _INT32 *led_out, _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT *jaw_out);
    void PostRun(const  volatile GLOBAL_POSTRUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FXDPT_FLOAT *j_in, _INT32  *en_out, _FXDPT_FLOAT *xyz_out, _INT32 *led_out, _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT  *jaw_out);
public:
    //block internal parameter variable declaration
    _FXDPT_FLOAT32 mouse_sen, def_x, def_y, def_z;
    //block internal initial  condition variable declaration
private:
    _UINT64 m_prev_tick;
    _BYTE  m_prev_led_kb, m_prev_en_kb, m_prev_park_kb;
    int m_en_mouse;
    int m_prev_mouse_pos[3];
};

#endif

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

CustomBlock193083739::CustomBlock193083739(){
}

CustomBlock193083739::~CustomBlock193083739(){
}

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

void  CustomBlock193083739::PreRun(const volatile GLOBAL_PRERUN_PARAM_STRUCT *arg, _INT32  *kb_in, _FXDPT_FLOAT *j_in, _INT32 *en_out, _FXDPT_FLOAT *xyz_out, _INT32 *led_out,  _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT *jaw_out){
    *led_out = 0;
    *en_out  = -2;//first run
    xyz_out[0] = def_x;
    xyz_out[1] = def_y;
    xyz_out[2]  = def_z;
    *tw_out = 0;
    *jaw_out = 0.5;
    //
    m_prev_mouse_pos[0]  = kb_in[257];
    m_prev_mouse_pos[1] = kb_in[258];
    m_prev_mouse_pos[2]  = kb_in[259];
    //
    m_prev_tick = arg->rt_clk_tick;
    m_prev_led_kb  = m_prev_en_kb = m_prev_park_kb = 0;
}

void CustomBlock193083739::Run(const  volatile GLOBAL_RUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FXDPT_FLOAT *j_in, _INT32  *en_out, _FXDPT_FLOAT *xyz_out, _INT32 *led_out, _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT  *jaw_out){
    if(arg->rt_clk_tick-m_prev_tick < 10000)
        return;
    m_prev_tick = arg->rt_clk_tick;
    int x = kb_in[257];
    int y = kb_in[258];
    int z = kb_in[259];
    int diffx = x - m_prev_mouse_pos[0];m_prev_mouse_pos[0]  = x;
    int diffy = y - m_prev_mouse_pos[1];m_prev_mouse_pos[1] = y;
    int  diffz = z - m_prev_mouse_pos[2];m_prev_mouse_pos[2] = z;
    //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;
    //park logic
    if(*en_out  > -2)
    {
        if(kb_in[80])//P. press P until it is parked
        {
            *en_out = -1;
        }
        else
        {
            //enable  disable x,y,z control
            if(kb_in[69])//E
            {
                if(m_prev_en_kb  == 0)
                    m_prev_en_kb = 1;
            }
            else
                m_prev_en_kb = 0;
            *en_out = 1;
        }
    }
    //update x,y,z
    if(*en_out < 0)
    {
        xyz_out[0] = def_x;
        xyz_out[1] = def_y;
        xyz_out[2] = def_z;
        //
        *tw_out  = 0;
        *jaw_out = 0.5;
    }
    else if(*en_out > 0)
    {
        //x,y
        if(diffx != 0 || diffy != 0)
        {
            //convert  curren x,y to polar
            _FXDPT_FLOAT y = j_in[0];
            _FXDPT_FLOAT  x = j_in[1];
            _FXDPT_FLOAT mag = sqrt(x*x + y*y);
            _FXDPT_FLOAT  ang = 0;
            if(!IsZero(x))
                ang = atan2(y,x) * MATH_RAD_TO_DEG;
            if(diffx != 0)
            {
                if(kb_in[256] & 0b10)
                {
                    //update theta
                    _FXDPT_FLOAT  theta = ((_FXDPT_FLOAT)diffx * mouse_sen * ((kb_in[17]/*ctrl*/)?0.01:0.1));
                    ang += theta;
                    //dont set to 90. sometimes  solution is out of bounds
                    if(ang > 85) ang = 85;
                    if(ang  < -85) ang = -85;
                }

            }
            if(diffy  != 0)
            {
                if(kb_in[256] & 0b10)
                {
                    //update radius
                    _FXDPT_FLOAT radius =  ((_FXDPT_FLOAT)diffy * mouse_sen * ((kb_in[17]/*ctrl*/)?0.05:0.5));
                    mag  -= radius;
                }
            }
            //convert polar  back to rectangular
            if(kb_in[256] & 0b10)
            {
                _FXDPT_FLOAT  x = mag * cos(ang * MATH_DEG_TO_RAD);
                _FXDPT_FLOAT y = mag *  sin(ang * MATH_DEG_TO_RAD);
                xyz_out[0] = y;
                xyz_out[1]  = x;
            }
        }
        //z
        if(diffz != 0)
        {
            _FXDPT_FLOAT d = (_FXDPT_FLOAT)diffz * mouse_sen;
            d  *= ((kb_in[17]/*ctrl*/)?0.01:0.1);
            xyz_out[2] = j_in[2]-d;
        }
    }
    //twist control
    if(kb_in[84])//T
    {
        *tw_out  += 1;
        *tw_out = (*tw_out > 90)?90:*tw_out;
    }
    else if(kb_in[89])//Y
    {
        *tw_out -= 1;
        *tw_out = (*tw_out < -90)?-90:*tw_out;
    }
    //jaw control
    if(kb_in[74])//J
    {
        *jaw_out  += (kb_in[16]/*shift*/)?0.1:0.01;
        *jaw_out = (*jaw_out > 1)?1:*jaw_out;
    }
    else if(kb_in[75])//K
    {
        *jaw_out -= (kb_in[16]/*shift*/)?0.1:0.01;
        *jaw_out = (*jaw_out < 0)?0:*jaw_out;
    }
    if(*en_out == -2)  *en_out = -1;
}

void CustomBlock193083739::PostRun(const volatile GLOBAL_POSTRUN_PARAM_STRUCT  *arg, _INT32 *kb_in, _FXDPT_FLOAT *j_in, _INT32 *en_out, _FXDPT_FLOAT *xyz_out,  _INT32 *led_out, _FXDPT_FLOAT *tw_out, _FXDPT_FLOAT *jaw_out){
}

Downloadable files

Connection Diagram for Arduino Nano RP2040 Connect

Connection Diagram for Raspberry Pi Pico W

Comments

Only logged in users can leave comments

aadhuniklabs

0 Followers

•

0 Projects

Intro