Aadhuniklabs
Published

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.

IntermediateFull instructions provided12 hours3,093
4 DOF Mobile Arm Robot with on-board camera

Things used in this project

Hardware components

Arduino Nano RP2040 Connect
Arduino Nano RP2040 Connect
×1
Raspberry Pi Pico
Raspberry Pi Pico
Actually it is Raspberry Pi Pico W
×1
VNH3ASP30 DC Motor Driver
or any suitable dual motor H-bridge driver
×1
12V to 6V Buck Converter
or any dc voltage regulator with output voltage 6V
×1
High Torque Servo Motors
×5
ESP32 Camera Module Development Board
M5Stack ESP32 Camera Module Development Board
×1
Robotic Arm Kit
with servo brackets, mounting base and gripper
×1
Geared DC Motor, 12 V
Geared DC Motor, 12 V
×4
Robot Base Frame / Chassis
×1
Robot Wheels
×4
9V to 12V Battery
×1
Jumper Wires
DIYables Jumper Wires
×1
DC Power Jack Connector
DIYables DC Power Jack Connector
×1

Software apps and online services

Computer Aided Simulation Program (CASP)

Story

Read more

Schematics

Connection Diagram for Arduino Nano RP2040 Connect

Connection Diagram for Raspberry Pi Pico W

Code

Custom Block Code

C/C++
//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){
}

Credits

Aadhuniklabs

Aadhuniklabs

7 projects • 17 followers
Makers and tech enthusiast since 2022

Comments

Related channels and tags
  • Arduino
  • DIYables
  • M5Stack
  • Raspberry Pi
  • Robotics
  • Robotics