A Simple WiFi controlled mobile robot with pan & tilt camera

Arduino RP2040 or Raspberry Pi Pico W or ESP32 based mobile robot with on-board pan & tilt camera can be controlled remotely from host PC.

Aug 24, 2022

•

6970 views

•

3 respects

Components and supplies

9V to 12V battery

Servo Motor SG90 180 degree

FireBeetle ESP32 IOT Microcontroller (Supports Wi-Fi & Bluetooth)

DC Power Jack

Arduino Nano RP2040 Connect

Robot Base Frame / Chassis

Geared DC Motor, 12 V

Jumper Wires

Raspberry Pi Pico

ESP32 Camera Module Development Board

Pan-Tilt HAT

12V to 5V Step Down DC Converter

Apps and platforms

Computer Aided Simulation Program (CASP)

Project description

Code

Source Code For Remote Control Car custom block used in the Native Model of this project

c_cpp

This custom block generates required control signals when user presses certain keys for controlling the movements of the robot. User can take a look at the configuration and source code of the block and learn how to integrate the block in a CASP model along with other blocks.

1//Header file code
2
3/************************CASP_COPYRIGHT**********************************
4**  Copyright (c) AadhunikLabs. All rights reserved.
5** This file is part of CASP.
6**  Licensees holding valid licenses may use this file in accordance
7** with the  license agreement provided with the software.
8*************************************************************************/
9#ifndef  REMCARMANUAL927823855_H
10#define REMCARMANUAL927823855_H
11
12#include "sim_common_definitions.h"
13
14#define  UPDATE_INTERVAL 10UL//in milli secs
15
16class SHARED_LIB_TYPE RemCarManual927823855
17{
18public:
19    RemCarManual927823855();
20    ~RemCarManual927823855();
21    void Initialize(const  volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT *arg);
22    void PreRun(const volatile  GLOBAL_PRERUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out,  _FLOAT *r_speed_out, _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out);
23    void Run(const volatile GLOBAL_RUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT  *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out, _INT32 *r_dir_out, _FLOAT  *h_angle_out, _FLOAT *v_angle_out);
24    void RunPlotStep(const volatile GLOBAL_RUN_PARAM_STRUCT  *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out,  _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out);
25    void PostRun(const  volatile GLOBAL_POSTRUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32  *l_dir_out, _FLOAT *r_speed_out, _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT  *v_angle_out);
26public:
27    //port variable declaration
28    //port dimension  and size declaration
29    //block internal parameter variable declaration
30    _FLOAT base_speed, min_speed, max_speed, speed_step;
31    _FLOAT rotate_speed;
32    _FLOAT head_speed[2];
33    _FLOAT head_hangle[3], head_vangle[3];//min,max,def
34    _BYTE head_control;
35    //block internal initial condition variable declaration
36private:
37    _FLOAT rotate_speed_inc, rotate_speed_dec;
38    _BOOLEAN m_head_speed_reverse[2];
39    int m_init_mouse_pos[2];//during last head movement
40    int m_prev_mouse_pos[2];//prev  position
41    _UINT64 m_update_count;
42};
43
44#endif
45
46//CPP file code
47
48/************************CASP_COPYRIGHT**********************************
49**  Copyright (c) AadhunikLabs. All rights reserved.
50** This file is part of CASP.
51**  Licensees holding valid licenses may use this file in accordance
52** with the  license agreement provided with the software.
53*************************************************************************/
54#include  "RemCarManual927823855.h"
55#include "math.h"
56#include "simcode.h"
57
58RemCarManual927823855::RemCarManual927823855(){
59}
60
61RemCarManual927823855::~RemCarManual927823855(){
62}
63
64void  RemCarManual927823855::Initialize(const volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT  *arg){
65}
66
67void RemCarManual927823855::PreRun(const volatile GLOBAL_PRERUN_PARAM_STRUCT  *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out,  _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out){
68    m_init_mouse_pos[0]  = kb_in[257];
69    m_init_mouse_pos[1] = kb_in[258];
70    m_prev_mouse_pos[0]  = kb_in[257];
71    m_prev_mouse_pos[1] = kb_in[258];
72    //
73    *l_speed_out  = 0;
74    *l_dir_out = 1;
75    *r_speed_out = 0;
76    *r_dir_out = 1;
77    //
78    *v_angle_out = head_vangle[2];
79    *h_angle_out = head_hangle[2];
80    //
81    rotate_speed_inc = 1.0f+(rotate_speed/100.0f);
82    rotate_speed_dec  = 1.0f-(rotate_speed/100.0f);
83    speed_step = speed_step/100.0f;
84    m_update_count  = arg->rt_clk_tick;
85    //
86    for(int i=0; i<2; ++i)
87    {
88        m_head_speed_reverse[i]  = (head_speed[i] < 0)?1:0;
89        head_speed[i] = (head_speed[i] < 0)?-head_speed[i]:head_speed[i];
90    }
91}
92
93void RemCarManual927823855::Run(const volatile GLOBAL_RUN_PARAM_STRUCT  *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out,  _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out){
94    if(arg->rt_clk_tick-m_update_count  < UPDATE_INTERVAL*1000)
95        return;
96    m_update_count = arg->rt_clk_tick;
97    //
98    *l_speed_out = 0;
99    *r_speed_out = 0;
100    *l_dir_out = 1;
101    *r_dir_out = 1;
102    //adjust speed
103    if(kb_in[34])//pg down
104    {
105        _FLOAT val = base_speed * (1.0f-speed_step);
106        if(val > min_speed)
107            base_speed = val;
108    }
109    else if(kb_in[33])//pg up
110    {
111        _FLOAT val = base_speed * (1.0f+speed_step);
112        if(val < max_speed)
113            base_speed = val;
114    }
115    //forward and backward
116    if(kb_in[87])//W-forward
117    {
118        *l_speed_out = *r_speed_out = base_speed;
119        if(kb_in[65])//A-left
120        {
121            *l_speed_out *= rotate_speed_dec;
122            *r_speed_out  *= rotate_speed_inc;
123        }
124        else if(kb_in[68])//D-right
125        {
126            *l_speed_out *= rotate_speed_inc;
127            *r_speed_out *= rotate_speed_dec;
128        }
129        //check limits
130        if(*l_speed_out > max_speed)
131            *l_speed_out = max_speed;
132        else if(*l_speed_out < min_speed)
133            *l_speed_out = min_speed;
134        if(*r_speed_out > max_speed)
135            *r_speed_out = max_speed;
136        else if(*r_speed_out < min_speed)
137            *r_speed_out = min_speed;
138        //
139        *l_dir_out = 1;
140        *r_dir_out = 0;
141    }
142    else if(kb_in[83])//S-reverse
143    {
144        *l_speed_out = *r_speed_out = base_speed;
145        if(kb_in[65])//A-left
146        {
147            *l_speed_out *= rotate_speed_dec;
148            *r_speed_out  *= rotate_speed_inc;
149        }
150        else if(kb_in[68])//D-right
151        {
152            *l_speed_out *= rotate_speed_inc;
153            *r_speed_out *= rotate_speed_dec;
154        }
155        //check limits
156        if(*l_speed_out > max_speed)
157            *l_speed_out = max_speed;
158        else if(*l_speed_out < min_speed)
159            *l_speed_out = min_speed;
160        if(*r_speed_out > max_speed)
161            *r_speed_out = max_speed;
162        else if(*r_speed_out < min_speed)
163            *r_speed_out = min_speed;
164        //
165        *l_dir_out = 0;
166        *r_dir_out = 1;
167    }
168    //if no forward or reverse pressed. then  rotate on center
169    else//if(kb_in[87] == 0 && kb_in[83] == 0)
170    {
171        if(kb_in[65])//A-left
172        {
173            *l_speed_out = *r_speed_out  = base_speed;
174            *l_dir_out = 0;
175            *r_dir_out = 0;
176        }
177        else if(kb_in[68])//D-right
178        {
179            *l_speed_out  = *r_speed_out = base_speed;
180            *l_dir_out = 1;
181            *r_dir_out  = 1;
182        }
183    }
184    //head rotation
185    if(head_control == 1)//from  mouse
186    {
187        int x = kb_in[257];
188        int y = kb_in[258];
189        int diffx = x - m_init_mouse_pos[0];
190        int diffy = y - m_init_mouse_pos[1];
191        if(diffx != 0)
192        {
193            //if(x == m_prev_mouse_pos[0])//user  stabilized mouse position. move head
194            {
195                if(kb_in[256]  & 0b10)//if right button pressed then only move mouse
196                {
197                    if(m_head_speed_reverse[0])
198                        *h_angle_out += (diffx * head_speed[0] * 0.1);
199                    else
200                        *h_angle_out -= (diffx * head_speed[0] * 0.1);
201                    if(*h_angle_out  < head_hangle[0])
202                        *h_angle_out = head_hangle[0];
203                    else if(*h_angle_out > head_hangle[1])
204                        *h_angle_out  = head_hangle[1];
205                }
206                m_init_mouse_pos[0] =  x;
207            }
208        }
209        if(diffy != 0)
210        {
211            //if(y  == m_prev_mouse_pos[1])//user stabilized mouse position. move head
212            {
213                if(kb_in[256] & 0b10)//if right button pressed then only move mouse
214                {
215                    if(m_head_speed_reverse[1])
216                        *v_angle_out  -= (diffy * head_speed[1] * 0.1);
217                    else
218                        *v_angle_out  += (diffy * head_speed[1] * 0.1);
219                    if(*v_angle_out < head_vangle[0])
220                        *v_angle_out = head_vangle[0];
221                    else  if(*v_angle_out > head_vangle[1])
222                        *v_angle_out = head_vangle[1];
223                }
224                m_init_mouse_pos[1] = y;
225            }
226        }
227        m_prev_mouse_pos[0] = x;
228        m_prev_mouse_pos[1] = y;
229    }
230    else//from arrow keys
231    {
232        //37-left, 38-up,39-right,40-bottom
233        if((kb_in[37] && m_head_speed_reverse[0] == false) || (kb_in[39] && m_head_speed_reverse[0]  == true))
234        {
235            *h_angle_out += (head_speed[0]);
236            if(*h_angle_out  > head_hangle[1])
237                *h_angle_out = head_hangle[1];
238        }
239        else if((kb_in[39] && m_head_speed_reverse[0] == false) || (kb_in[37] &&  m_head_speed_reverse[0] == true))
240        {
241            *h_angle_out += (-head_speed[0]);
242            if(*h_angle_out < head_hangle[0])
243                *h_angle_out =  head_hangle[0];
244        }
245        if((kb_in[38] && m_head_speed_reverse[1]  == false) || (kb_in[40] && m_head_speed_reverse[1] == true))
246        {
247            *v_angle_out  += (-head_speed[1]);
248            if(*v_angle_out < head_vangle[0])
249                *v_angle_out  = head_vangle[0];
250        }
251        else if((kb_in[40] && m_head_speed_reverse[1]  == false) || (kb_in[38] && m_head_speed_reverse[1] == true))
252        {
253            *v_angle_out  += (head_speed[1]);
254            if(*v_angle_out > head_vangle[1])
255                *v_angle_out  = head_vangle[1];
256        }
257    }
258    //center horizontal and vertical  heads when G is pressed
259    if(kb_in[71])
260    {
261        *h_angle_out =  head_hangle[2];
262        *v_angle_out = head_vangle[2];
263    }
264}
265
266void  RemCarManual927823855::RunPlotStep(const volatile GLOBAL_RUN_PARAM_STRUCT *arg,  _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out, _INT32  *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out){
267    Run(arg, kb_in, l_speed_out,  l_dir_out, r_speed_out, r_dir_out, h_angle_out, v_angle_out);
268}
269
270void RemCarManual927823855::PostRun(const  volatile GLOBAL_POSTRUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32  *l_dir_out, _FLOAT *r_speed_out, _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT  *v_angle_out){
271}

//Header file code

/************************CASP_COPYRIGHT**********************************
**  Copyright (c) AadhunikLabs. All rights reserved.
** This file is part of CASP.
**  Licensees holding valid licenses may use this file in accordance
** with the  license agreement provided with the software.
*************************************************************************/
#ifndef  REMCARMANUAL927823855_H
#define REMCARMANUAL927823855_H

#include "sim_common_definitions.h"

#define  UPDATE_INTERVAL 10UL//in milli secs

class SHARED_LIB_TYPE RemCarManual927823855
{
public:
    RemCarManual927823855();
    ~RemCarManual927823855();
    void Initialize(const  volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT *arg);
    void PreRun(const volatile  GLOBAL_PRERUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out,  _FLOAT *r_speed_out, _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out);
    void Run(const volatile GLOBAL_RUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT  *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out, _INT32 *r_dir_out, _FLOAT  *h_angle_out, _FLOAT *v_angle_out);
    void RunPlotStep(const volatile GLOBAL_RUN_PARAM_STRUCT  *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out,  _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out);
    void PostRun(const  volatile GLOBAL_POSTRUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32  *l_dir_out, _FLOAT *r_speed_out, _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT  *v_angle_out);
public:
    //port variable declaration
    //port dimension  and size declaration
    //block internal parameter variable declaration
    _FLOAT base_speed, min_speed, max_speed, speed_step;
    _FLOAT rotate_speed;
    _FLOAT head_speed[2];
    _FLOAT head_hangle[3], head_vangle[3];//min,max,def
    _BYTE head_control;
    //block internal initial condition variable declaration
private:
    _FLOAT rotate_speed_inc, rotate_speed_dec;
    _BOOLEAN m_head_speed_reverse[2];
    int m_init_mouse_pos[2];//during last head movement
    int m_prev_mouse_pos[2];//prev  position
    _UINT64 m_update_count;
};

#endif

//CPP file code

/************************CASP_COPYRIGHT**********************************
**  Copyright (c) AadhunikLabs. All rights reserved.
** This file is part of CASP.
**  Licensees holding valid licenses may use this file in accordance
** with the  license agreement provided with the software.
*************************************************************************/
#include  "RemCarManual927823855.h"
#include "math.h"
#include "simcode.h"

RemCarManual927823855::RemCarManual927823855(){
}

RemCarManual927823855::~RemCarManual927823855(){
}

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

void RemCarManual927823855::PreRun(const volatile GLOBAL_PRERUN_PARAM_STRUCT  *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out,  _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out){
    m_init_mouse_pos[0]  = kb_in[257];
    m_init_mouse_pos[1] = kb_in[258];
    m_prev_mouse_pos[0]  = kb_in[257];
    m_prev_mouse_pos[1] = kb_in[258];
    //
    *l_speed_out  = 0;
    *l_dir_out = 1;
    *r_speed_out = 0;
    *r_dir_out = 1;
    //
    *v_angle_out = head_vangle[2];
    *h_angle_out = head_hangle[2];
    //
    rotate_speed_inc = 1.0f+(rotate_speed/100.0f);
    rotate_speed_dec  = 1.0f-(rotate_speed/100.0f);
    speed_step = speed_step/100.0f;
    m_update_count  = arg->rt_clk_tick;
    //
    for(int i=0; i<2; ++i)
    {
        m_head_speed_reverse[i]  = (head_speed[i] < 0)?1:0;
        head_speed[i] = (head_speed[i] < 0)?-head_speed[i]:head_speed[i];
    }
}

void RemCarManual927823855::Run(const volatile GLOBAL_RUN_PARAM_STRUCT  *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out,  _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out){
    if(arg->rt_clk_tick-m_update_count  < UPDATE_INTERVAL*1000)
        return;
    m_update_count = arg->rt_clk_tick;
    //
    *l_speed_out = 0;
    *r_speed_out = 0;
    *l_dir_out = 1;
    *r_dir_out = 1;
    //adjust speed
    if(kb_in[34])//pg down
    {
        _FLOAT val = base_speed * (1.0f-speed_step);
        if(val > min_speed)
            base_speed = val;
    }
    else if(kb_in[33])//pg up
    {
        _FLOAT val = base_speed * (1.0f+speed_step);
        if(val < max_speed)
            base_speed = val;
    }
    //forward and backward
    if(kb_in[87])//W-forward
    {
        *l_speed_out = *r_speed_out = base_speed;
        if(kb_in[65])//A-left
        {
            *l_speed_out *= rotate_speed_dec;
            *r_speed_out  *= rotate_speed_inc;
        }
        else if(kb_in[68])//D-right
        {
            *l_speed_out *= rotate_speed_inc;
            *r_speed_out *= rotate_speed_dec;
        }
        //check limits
        if(*l_speed_out > max_speed)
            *l_speed_out = max_speed;
        else if(*l_speed_out < min_speed)
            *l_speed_out = min_speed;
        if(*r_speed_out > max_speed)
            *r_speed_out = max_speed;
        else if(*r_speed_out < min_speed)
            *r_speed_out = min_speed;
        //
        *l_dir_out = 1;
        *r_dir_out = 0;
    }
    else if(kb_in[83])//S-reverse
    {
        *l_speed_out = *r_speed_out = base_speed;
        if(kb_in[65])//A-left
        {
            *l_speed_out *= rotate_speed_dec;
            *r_speed_out  *= rotate_speed_inc;
        }
        else if(kb_in[68])//D-right
        {
            *l_speed_out *= rotate_speed_inc;
            *r_speed_out *= rotate_speed_dec;
        }
        //check limits
        if(*l_speed_out > max_speed)
            *l_speed_out = max_speed;
        else if(*l_speed_out < min_speed)
            *l_speed_out = min_speed;
        if(*r_speed_out > max_speed)
            *r_speed_out = max_speed;
        else if(*r_speed_out < min_speed)
            *r_speed_out = min_speed;
        //
        *l_dir_out = 0;
        *r_dir_out = 1;
    }
    //if no forward or reverse pressed. then  rotate on center
    else//if(kb_in[87] == 0 && kb_in[83] == 0)
    {
        if(kb_in[65])//A-left
        {
            *l_speed_out = *r_speed_out  = base_speed;
            *l_dir_out = 0;
            *r_dir_out = 0;
        }
        else if(kb_in[68])//D-right
        {
            *l_speed_out  = *r_speed_out = base_speed;
            *l_dir_out = 1;
            *r_dir_out  = 1;
        }
    }
    //head rotation
    if(head_control == 1)//from  mouse
    {
        int x = kb_in[257];
        int y = kb_in[258];
        int diffx = x - m_init_mouse_pos[0];
        int diffy = y - m_init_mouse_pos[1];
        if(diffx != 0)
        {
            //if(x == m_prev_mouse_pos[0])//user  stabilized mouse position. move head
            {
                if(kb_in[256]  & 0b10)//if right button pressed then only move mouse
                {
                    if(m_head_speed_reverse[0])
                        *h_angle_out += (diffx * head_speed[0] * 0.1);
                    else
                        *h_angle_out -= (diffx * head_speed[0] * 0.1);
                    if(*h_angle_out  < head_hangle[0])
                        *h_angle_out = head_hangle[0];
                    else if(*h_angle_out > head_hangle[1])
                        *h_angle_out  = head_hangle[1];
                }
                m_init_mouse_pos[0] =  x;
            }
        }
        if(diffy != 0)
        {
            //if(y  == m_prev_mouse_pos[1])//user stabilized mouse position. move head
            {
                if(kb_in[256] & 0b10)//if right button pressed then only move mouse
                {
                    if(m_head_speed_reverse[1])
                        *v_angle_out  -= (diffy * head_speed[1] * 0.1);
                    else
                        *v_angle_out  += (diffy * head_speed[1] * 0.1);
                    if(*v_angle_out < head_vangle[0])
                        *v_angle_out = head_vangle[0];
                    else  if(*v_angle_out > head_vangle[1])
                        *v_angle_out = head_vangle[1];
                }
                m_init_mouse_pos[1] = y;
            }
        }
        m_prev_mouse_pos[0] = x;
        m_prev_mouse_pos[1] = y;
    }
    else//from arrow keys
    {
        //37-left, 38-up,39-right,40-bottom
        if((kb_in[37] && m_head_speed_reverse[0] == false) || (kb_in[39] && m_head_speed_reverse[0]  == true))
        {
            *h_angle_out += (head_speed[0]);
            if(*h_angle_out  > head_hangle[1])
                *h_angle_out = head_hangle[1];
        }
        else if((kb_in[39] && m_head_speed_reverse[0] == false) || (kb_in[37] &&  m_head_speed_reverse[0] == true))
        {
            *h_angle_out += (-head_speed[0]);
            if(*h_angle_out < head_hangle[0])
                *h_angle_out =  head_hangle[0];
        }
        if((kb_in[38] && m_head_speed_reverse[1]  == false) || (kb_in[40] && m_head_speed_reverse[1] == true))
        {
            *v_angle_out  += (-head_speed[1]);
            if(*v_angle_out < head_vangle[0])
                *v_angle_out  = head_vangle[0];
        }
        else if((kb_in[40] && m_head_speed_reverse[1]  == false) || (kb_in[38] && m_head_speed_reverse[1] == true))
        {
            *v_angle_out  += (head_speed[1]);
            if(*v_angle_out > head_vangle[1])
                *v_angle_out  = head_vangle[1];
        }
    }
    //center horizontal and vertical  heads when G is pressed
    if(kb_in[71])
    {
        *h_angle_out =  head_hangle[2];
        *v_angle_out = head_vangle[2];
    }
}

void  RemCarManual927823855::RunPlotStep(const volatile GLOBAL_RUN_PARAM_STRUCT *arg,  _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out, _INT32  *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out){
    Run(arg, kb_in, l_speed_out,  l_dir_out, r_speed_out, r_dir_out, h_angle_out, v_angle_out);
}

void RemCarManual927823855::PostRun(const  volatile GLOBAL_POSTRUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32  *l_dir_out, _FLOAT *r_speed_out, _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT  *v_angle_out){
}

Source Code For Remote Control Car custom block used in the Native Model of this project

c_cpp

1//Header file code
2
3/************************CASP_COPYRIGHT**********************************
4**   Copyright (c) AadhunikLabs. All rights reserved.
5** This file is part of CASP.
6**   Licensees holding valid licenses may use this file in accordance
7** with the   license agreement provided with the software.
8*************************************************************************/
9#ifndef   REMCARMANUAL927823855_H
10#define REMCARMANUAL927823855_H
11
12#include "sim_common_definitions.h"
13
14#define   UPDATE_INTERVAL 10UL//in milli secs
15
16class SHARED_LIB_TYPE RemCarManual927823855
17{
18public:
19     RemCarManual927823855();
20    ~RemCarManual927823855();
21    void Initialize(const   volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT *arg);
22    void PreRun(const volatile   GLOBAL_PRERUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out,   _FLOAT *r_speed_out, _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out);
23     void Run(const volatile GLOBAL_RUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT   *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out, _INT32 *r_dir_out, _FLOAT   *h_angle_out, _FLOAT *v_angle_out);
24    void RunPlotStep(const volatile GLOBAL_RUN_PARAM_STRUCT   *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out,   _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out);
25    void PostRun(const   volatile GLOBAL_POSTRUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32   *l_dir_out, _FLOAT *r_speed_out, _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT   *v_angle_out);
26public:
27    //port variable declaration
28    //port dimension   and size declaration
29    //block internal parameter variable declaration
30     _FLOAT base_speed, min_speed, max_speed, speed_step;
31    _FLOAT rotate_speed;
32     _FLOAT head_speed[2];
33    _FLOAT head_hangle[3], head_vangle[3];//min,max,def
34     _BYTE head_control;
35    //block internal initial condition variable declaration
36private:
37     _FLOAT rotate_speed_inc, rotate_speed_dec;
38    _BOOLEAN m_head_speed_reverse[2];
39     int m_init_mouse_pos[2];//during last head movement
40    int m_prev_mouse_pos[2];//prev   position
41    _UINT64 m_update_count;
42};
43
44#endif
45
46//CPP file code
47
48/************************CASP_COPYRIGHT**********************************
49**   Copyright (c) AadhunikLabs. All rights reserved.
50** This file is part of CASP.
51**   Licensees holding valid licenses may use this file in accordance
52** with the   license agreement provided with the software.
53*************************************************************************/
54#include   "RemCarManual927823855.h"
55#include "math.h"
56#include "simcode.h"
57
58RemCarManual927823855::RemCarManual927823855(){
59}
60
61RemCarManual927823855::~RemCarManual927823855(){
62}
63
64void   RemCarManual927823855::Initialize(const volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT   *arg){
65}
66
67void RemCarManual927823855::PreRun(const volatile GLOBAL_PRERUN_PARAM_STRUCT   *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out,   _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out){
68    m_init_mouse_pos[0]   = kb_in[257];
69    m_init_mouse_pos[1] = kb_in[258];
70    m_prev_mouse_pos[0]   = kb_in[257];
71    m_prev_mouse_pos[1] = kb_in[258];
72    //
73    *l_speed_out   = 0;
74    *l_dir_out = 1;
75    *r_speed_out = 0;
76    *r_dir_out = 1;
77     //
78    *v_angle_out = head_vangle[2];
79    *h_angle_out = head_hangle[2];
80     //
81    rotate_speed_inc = 1.0f+(rotate_speed/100.0f);
82    rotate_speed_dec   = 1.0f-(rotate_speed/100.0f);
83    speed_step = speed_step/100.0f;
84    m_update_count   = arg->rt_clk_tick;
85    //
86    for(int i=0; i<2; ++i)
87    {
88        m_head_speed_reverse[i]   = (head_speed[i] < 0)?1:0;
89        head_speed[i] = (head_speed[i] < 0)?-head_speed[i]:head_speed[i];
90     }
91}
92
93void RemCarManual927823855::Run(const volatile GLOBAL_RUN_PARAM_STRUCT   *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out,   _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out){
94    if(arg->rt_clk_tick-m_update_count   < UPDATE_INTERVAL*1000)
95        return;
96    m_update_count = arg->rt_clk_tick;
97     //
98    *l_speed_out = 0;
99    *r_speed_out = 0;
100    *l_dir_out = 1;
101     *r_dir_out = 1;
102    //adjust speed
103    if(kb_in[34])//pg down
104    {
105         _FLOAT val = base_speed * (1.0f-speed_step);
106        if(val > min_speed)
107             base_speed = val;
108    }
109    else if(kb_in[33])//pg up
110    {
111         _FLOAT val = base_speed * (1.0f+speed_step);
112        if(val < max_speed)
113             base_speed = val;
114    }
115    //forward and backward
116    if(kb_in[87])//W-forward
117     {
118        *l_speed_out = *r_speed_out = base_speed;
119        if(kb_in[65])//A-left
120         {
121            *l_speed_out *= rotate_speed_dec;
122            *r_speed_out   *= rotate_speed_inc;
123        }
124        else if(kb_in[68])//D-right
125        {
126             *l_speed_out *= rotate_speed_inc;
127            *r_speed_out *= rotate_speed_dec;
128         }
129        //check limits
130        if(*l_speed_out > max_speed)
131             *l_speed_out = max_speed;
132        else if(*l_speed_out < min_speed)
133             *l_speed_out = min_speed;
134        if(*r_speed_out > max_speed)
135             *r_speed_out = max_speed;
136        else if(*r_speed_out < min_speed)
137             *r_speed_out = min_speed;
138        //
139        *l_dir_out = 1;
140         *r_dir_out = 0;
141    }
142    else if(kb_in[83])//S-reverse
143    {
144         *l_speed_out = *r_speed_out = base_speed;
145        if(kb_in[65])//A-left
146         {
147            *l_speed_out *= rotate_speed_dec;
148            *r_speed_out   *= rotate_speed_inc;
149        }
150        else if(kb_in[68])//D-right
151        {
152             *l_speed_out *= rotate_speed_inc;
153            *r_speed_out *= rotate_speed_dec;
154         }
155        //check limits
156        if(*l_speed_out > max_speed)
157             *l_speed_out = max_speed;
158        else if(*l_speed_out < min_speed)
159             *l_speed_out = min_speed;
160        if(*r_speed_out > max_speed)
161             *r_speed_out = max_speed;
162        else if(*r_speed_out < min_speed)
163             *r_speed_out = min_speed;
164        //
165        *l_dir_out = 0;
166         *r_dir_out = 1;
167    }
168    //if no forward or reverse pressed. then   rotate on center
169    else//if(kb_in[87] == 0 && kb_in[83] == 0)
170    {
171         if(kb_in[65])//A-left
172        {
173            *l_speed_out = *r_speed_out   = base_speed;
174            *l_dir_out = 0;
175            *r_dir_out = 0;
176         }
177        else if(kb_in[68])//D-right
178        {
179            *l_speed_out   = *r_speed_out = base_speed;
180            *l_dir_out = 1;
181            *r_dir_out   = 1;
182        }
183    }
184    //head rotation
185    if(head_control == 1)//from   mouse
186    {
187        int x = kb_in[257];
188        int y = kb_in[258];
189         int diffx = x - m_init_mouse_pos[0];
190        int diffy = y - m_init_mouse_pos[1];
191         if(diffx != 0)
192        {
193            //if(x == m_prev_mouse_pos[0])//user   stabilized mouse position. move head
194            {
195                if(kb_in[256]   & 0b10)//if right button pressed then only move mouse
196                {
197                    if(m_head_speed_reverse[0])
198                         *h_angle_out += (diffx * head_speed[0] * 0.1);
199                    else
200                         *h_angle_out -= (diffx * head_speed[0] * 0.1);
201                    if(*h_angle_out   < head_hangle[0])
202                        *h_angle_out = head_hangle[0];
203                     else if(*h_angle_out > head_hangle[1])
204                        *h_angle_out   = head_hangle[1];
205                }
206                m_init_mouse_pos[0] =   x;
207            }
208        }
209        if(diffy != 0)
210        {
211            //if(y   == m_prev_mouse_pos[1])//user stabilized mouse position. move head
212            {
213                 if(kb_in[256] & 0b10)//if right button pressed then only move mouse
214                 {
215                    if(m_head_speed_reverse[1])
216                        *v_angle_out   -= (diffy * head_speed[1] * 0.1);
217                    else
218                        *v_angle_out   += (diffy * head_speed[1] * 0.1);
219                    if(*v_angle_out < head_vangle[0])
220                         *v_angle_out = head_vangle[0];
221                    else   if(*v_angle_out > head_vangle[1])
222                        *v_angle_out = head_vangle[1];
223                 }
224                m_init_mouse_pos[1] = y;
225            }
226         }
227        m_prev_mouse_pos[0] = x;
228        m_prev_mouse_pos[1] = y;
229     }
230    else//from arrow keys
231    {
232        //37-left, 38-up,39-right,40-bottom
233         if((kb_in[37] && m_head_speed_reverse[0] == false) || (kb_in[39] && m_head_speed_reverse[0]   == true))
234        {
235            *h_angle_out += (head_speed[0]);
236            if(*h_angle_out   > head_hangle[1])
237                *h_angle_out = head_hangle[1];
238        }
239         else if((kb_in[39] && m_head_speed_reverse[0] == false) || (kb_in[37] &&   m_head_speed_reverse[0] == true))
240        {
241            *h_angle_out += (-head_speed[0]);
242             if(*h_angle_out < head_hangle[0])
243                *h_angle_out =   head_hangle[0];
244        }
245        if((kb_in[38] && m_head_speed_reverse[1]   == false) || (kb_in[40] && m_head_speed_reverse[1] == true))
246        {
247            *v_angle_out   += (-head_speed[1]);
248            if(*v_angle_out < head_vangle[0])
249                *v_angle_out   = head_vangle[0];
250        }
251        else if((kb_in[40] && m_head_speed_reverse[1]   == false) || (kb_in[38] && m_head_speed_reverse[1] == true))
252        {
253            *v_angle_out   += (head_speed[1]);
254            if(*v_angle_out > head_vangle[1])
255                *v_angle_out   = head_vangle[1];
256        }
257    }
258    //center horizontal and vertical   heads when G is pressed
259    if(kb_in[71])
260    {
261        *h_angle_out =   head_hangle[2];
262        *v_angle_out = head_vangle[2];
263    }
264}
265
266void   RemCarManual927823855::RunPlotStep(const volatile GLOBAL_RUN_PARAM_STRUCT *arg,   _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out, _INT32   *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out){
267    Run(arg, kb_in, l_speed_out,   l_dir_out, r_speed_out, r_dir_out, h_angle_out, v_angle_out);
268}
269
270void RemCarManual927823855::PostRun(const   volatile GLOBAL_POSTRUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32   *l_dir_out, _FLOAT *r_speed_out, _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT   *v_angle_out){
271}

//Header file code

/************************CASP_COPYRIGHT**********************************
**   Copyright (c) AadhunikLabs. All rights reserved.
** This file is part of CASP.
**   Licensees holding valid licenses may use this file in accordance
** with the   license agreement provided with the software.
*************************************************************************/
#ifndef   REMCARMANUAL927823855_H
#define REMCARMANUAL927823855_H

#include "sim_common_definitions.h"

#define   UPDATE_INTERVAL 10UL//in milli secs

class SHARED_LIB_TYPE RemCarManual927823855
{
public:
     RemCarManual927823855();
    ~RemCarManual927823855();
    void Initialize(const   volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT *arg);
    void PreRun(const volatile   GLOBAL_PRERUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out,   _FLOAT *r_speed_out, _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out);
     void Run(const volatile GLOBAL_RUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT   *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out, _INT32 *r_dir_out, _FLOAT   *h_angle_out, _FLOAT *v_angle_out);
    void RunPlotStep(const volatile GLOBAL_RUN_PARAM_STRUCT   *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out,   _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out);
    void PostRun(const   volatile GLOBAL_POSTRUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32   *l_dir_out, _FLOAT *r_speed_out, _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT   *v_angle_out);
public:
    //port variable declaration
    //port dimension   and size declaration
    //block internal parameter variable declaration
     _FLOAT base_speed, min_speed, max_speed, speed_step;
    _FLOAT rotate_speed;
     _FLOAT head_speed[2];
    _FLOAT head_hangle[3], head_vangle[3];//min,max,def
     _BYTE head_control;
    //block internal initial condition variable declaration
private:
     _FLOAT rotate_speed_inc, rotate_speed_dec;
    _BOOLEAN m_head_speed_reverse[2];
     int m_init_mouse_pos[2];//during last head movement
    int m_prev_mouse_pos[2];//prev   position
    _UINT64 m_update_count;
};

#endif

//CPP file code

/************************CASP_COPYRIGHT**********************************
**   Copyright (c) AadhunikLabs. All rights reserved.
** This file is part of CASP.
**   Licensees holding valid licenses may use this file in accordance
** with the   license agreement provided with the software.
*************************************************************************/
#include   "RemCarManual927823855.h"
#include "math.h"
#include "simcode.h"

RemCarManual927823855::RemCarManual927823855(){
}

RemCarManual927823855::~RemCarManual927823855(){
}

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

void RemCarManual927823855::PreRun(const volatile GLOBAL_PRERUN_PARAM_STRUCT   *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out,   _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out){
    m_init_mouse_pos[0]   = kb_in[257];
    m_init_mouse_pos[1] = kb_in[258];
    m_prev_mouse_pos[0]   = kb_in[257];
    m_prev_mouse_pos[1] = kb_in[258];
    //
    *l_speed_out   = 0;
    *l_dir_out = 1;
    *r_speed_out = 0;
    *r_dir_out = 1;
     //
    *v_angle_out = head_vangle[2];
    *h_angle_out = head_hangle[2];
     //
    rotate_speed_inc = 1.0f+(rotate_speed/100.0f);
    rotate_speed_dec   = 1.0f-(rotate_speed/100.0f);
    speed_step = speed_step/100.0f;
    m_update_count   = arg->rt_clk_tick;
    //
    for(int i=0; i<2; ++i)
    {
        m_head_speed_reverse[i]   = (head_speed[i] < 0)?1:0;
        head_speed[i] = (head_speed[i] < 0)?-head_speed[i]:head_speed[i];
     }
}

void RemCarManual927823855::Run(const volatile GLOBAL_RUN_PARAM_STRUCT   *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out,   _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out){
    if(arg->rt_clk_tick-m_update_count   < UPDATE_INTERVAL*1000)
        return;
    m_update_count = arg->rt_clk_tick;
     //
    *l_speed_out = 0;
    *r_speed_out = 0;
    *l_dir_out = 1;
     *r_dir_out = 1;
    //adjust speed
    if(kb_in[34])//pg down
    {
         _FLOAT val = base_speed * (1.0f-speed_step);
        if(val > min_speed)
             base_speed = val;
    }
    else if(kb_in[33])//pg up
    {
         _FLOAT val = base_speed * (1.0f+speed_step);
        if(val < max_speed)
             base_speed = val;
    }
    //forward and backward
    if(kb_in[87])//W-forward
     {
        *l_speed_out = *r_speed_out = base_speed;
        if(kb_in[65])//A-left
         {
            *l_speed_out *= rotate_speed_dec;
            *r_speed_out   *= rotate_speed_inc;
        }
        else if(kb_in[68])//D-right
        {
             *l_speed_out *= rotate_speed_inc;
            *r_speed_out *= rotate_speed_dec;
         }
        //check limits
        if(*l_speed_out > max_speed)
             *l_speed_out = max_speed;
        else if(*l_speed_out < min_speed)
             *l_speed_out = min_speed;
        if(*r_speed_out > max_speed)
             *r_speed_out = max_speed;
        else if(*r_speed_out < min_speed)
             *r_speed_out = min_speed;
        //
        *l_dir_out = 1;
         *r_dir_out = 0;
    }
    else if(kb_in[83])//S-reverse
    {
         *l_speed_out = *r_speed_out = base_speed;
        if(kb_in[65])//A-left
         {
            *l_speed_out *= rotate_speed_dec;
            *r_speed_out   *= rotate_speed_inc;
        }
        else if(kb_in[68])//D-right
        {
             *l_speed_out *= rotate_speed_inc;
            *r_speed_out *= rotate_speed_dec;
         }
        //check limits
        if(*l_speed_out > max_speed)
             *l_speed_out = max_speed;
        else if(*l_speed_out < min_speed)
             *l_speed_out = min_speed;
        if(*r_speed_out > max_speed)
             *r_speed_out = max_speed;
        else if(*r_speed_out < min_speed)
             *r_speed_out = min_speed;
        //
        *l_dir_out = 0;
         *r_dir_out = 1;
    }
    //if no forward or reverse pressed. then   rotate on center
    else//if(kb_in[87] == 0 && kb_in[83] == 0)
    {
         if(kb_in[65])//A-left
        {
            *l_speed_out = *r_speed_out   = base_speed;
            *l_dir_out = 0;
            *r_dir_out = 0;
         }
        else if(kb_in[68])//D-right
        {
            *l_speed_out   = *r_speed_out = base_speed;
            *l_dir_out = 1;
            *r_dir_out   = 1;
        }
    }
    //head rotation
    if(head_control == 1)//from   mouse
    {
        int x = kb_in[257];
        int y = kb_in[258];
         int diffx = x - m_init_mouse_pos[0];
        int diffy = y - m_init_mouse_pos[1];
         if(diffx != 0)
        {
            //if(x == m_prev_mouse_pos[0])//user   stabilized mouse position. move head
            {
                if(kb_in[256]   & 0b10)//if right button pressed then only move mouse
                {
                    if(m_head_speed_reverse[0])
                         *h_angle_out += (diffx * head_speed[0] * 0.1);
                    else
                         *h_angle_out -= (diffx * head_speed[0] * 0.1);
                    if(*h_angle_out   < head_hangle[0])
                        *h_angle_out = head_hangle[0];
                     else if(*h_angle_out > head_hangle[1])
                        *h_angle_out   = head_hangle[1];
                }
                m_init_mouse_pos[0] =   x;
            }
        }
        if(diffy != 0)
        {
            //if(y   == m_prev_mouse_pos[1])//user stabilized mouse position. move head
            {
                 if(kb_in[256] & 0b10)//if right button pressed then only move mouse
                 {
                    if(m_head_speed_reverse[1])
                        *v_angle_out   -= (diffy * head_speed[1] * 0.1);
                    else
                        *v_angle_out   += (diffy * head_speed[1] * 0.1);
                    if(*v_angle_out < head_vangle[0])
                         *v_angle_out = head_vangle[0];
                    else   if(*v_angle_out > head_vangle[1])
                        *v_angle_out = head_vangle[1];
                 }
                m_init_mouse_pos[1] = y;
            }
         }
        m_prev_mouse_pos[0] = x;
        m_prev_mouse_pos[1] = y;
     }
    else//from arrow keys
    {
        //37-left, 38-up,39-right,40-bottom
         if((kb_in[37] && m_head_speed_reverse[0] == false) || (kb_in[39] && m_head_speed_reverse[0]   == true))
        {
            *h_angle_out += (head_speed[0]);
            if(*h_angle_out   > head_hangle[1])
                *h_angle_out = head_hangle[1];
        }
         else if((kb_in[39] && m_head_speed_reverse[0] == false) || (kb_in[37] &&   m_head_speed_reverse[0] == true))
        {
            *h_angle_out += (-head_speed[0]);
             if(*h_angle_out < head_hangle[0])
                *h_angle_out =   head_hangle[0];
        }
        if((kb_in[38] && m_head_speed_reverse[1]   == false) || (kb_in[40] && m_head_speed_reverse[1] == true))
        {
            *v_angle_out   += (-head_speed[1]);
            if(*v_angle_out < head_vangle[0])
                *v_angle_out   = head_vangle[0];
        }
        else if((kb_in[40] && m_head_speed_reverse[1]   == false) || (kb_in[38] && m_head_speed_reverse[1] == true))
        {
            *v_angle_out   += (head_speed[1]);
            if(*v_angle_out > head_vangle[1])
                *v_angle_out   = head_vangle[1];
        }
    }
    //center horizontal and vertical   heads when G is pressed
    if(kb_in[71])
    {
        *h_angle_out =   head_hangle[2];
        *v_angle_out = head_vangle[2];
    }
}

void   RemCarManual927823855::RunPlotStep(const volatile GLOBAL_RUN_PARAM_STRUCT *arg,   _INT32 *kb_in, _FLOAT *l_speed_out, _INT32 *l_dir_out, _FLOAT *r_speed_out, _INT32   *r_dir_out, _FLOAT *h_angle_out, _FLOAT *v_angle_out){
    Run(arg, kb_in, l_speed_out,   l_dir_out, r_speed_out, r_dir_out, h_angle_out, v_angle_out);
}

void RemCarManual927823855::PostRun(const   volatile GLOBAL_POSTRUN_PARAM_STRUCT *arg, _INT32 *kb_in, _FLOAT *l_speed_out, _INT32   *l_dir_out, _FLOAT *r_speed_out, _INT32 *r_dir_out, _FLOAT *h_angle_out, _FLOAT   *v_angle_out){
}

Downloadable files

Connection Diagram for Arduino Nano RP2040 Connect

Connection Diagram for ESP32S

Connection Diagram for Raspberry Pi Pico W

Project File

https://aadhuniklabs.com/casp/casp_web_projects/robotics/01_rc_robot.zip

Comments

Only logged in users can leave comments

aadhuniklabs

0 Followers

•

0 Projects

Intro