Published September 17, 2022

Self Balancing WiFi Controlled Robot with PanTilt Camera

This self balancing mobile robot with pan & tilt camera can be navigated remotely from host PC without the need to be in line of sight.

IntermediateFull instructions provided3 hours3,186

Self Balancing WiFi Controlled Robot with PanTilt Camera

Things used in this project

Hardware components

Arduino Nano RP2040 Connect

Raspberry Pi Pico

DFRobot 6 DOF Sensor - MPU6050

Espressif ESP8266 ESP-01

M5Stack ESP32 Camera Module Development Board

12V to 5V Step Down DC Converter

or any general purpose 5V dc output converter

Geared DC Motor, 12 V

or any general purpose 6 to 12V geared DC motor

DIYables Servo Motor SG90 180 degree

Pimoroni Pan-Tilt HAT

9V to 12V Battery

Robot Base Frame / Chassis

DIYables Jumper Wires

DIYables DC Power Jack Connector

Software apps and online services

Computer Aided Simulation Program (CASP)

Story

Self-balancing mobile robots (SBMRs) are unique among others, just because of their ability to balance on a given fixed position. A typical SBMR measures inclination angle with the combination of accelerometer and gyroscope measurements and corrects itself by moving forward or backward to make the angle zero. Proportional-Integral-Derivative (PID) controller is used for deciding how much movement is required and at what rate to quickly correct the angle drift and stabilize the robot. Things become challenging when such a robot is to be navigated (moved or rotated) manually from remote location.

This project demonstrates how to build a basic WiFi remote controlled 2 wheeled SBMR that has an on-board WiFi camera mounted on a servos powered pan & tilt assembly. It is manually controlled by the user from host PC through WiFi. Because of the live video feed available from the on-board camera, the SBMR need not be inline of sight from the user.

Getting Started

Step 1: Build the hardware as described in the ‘Hardware Development’ section.

Step 2: Install latest version of CASP from this link: https://aadhuniklabs.com/?page_id=550. Please go through this link: https://aadhuniklabs.com/?page_id=554 for video tutorials on CASP. Please note that CASP version 0.9.5.3 dated 19.09.2022 and above is required for this project.

Step 3: Download example project on ‘Self Balancing Remote Controlled Mobile Robot with On-board Camera’ at this link: https://aadhuniklabs.com/casp/casp_web_projects/robotics/02_self_bal_wheels.zip and follow the steps mentioned in the ‘Software Development’ section.

Step 4: Some adjustments as described in ‘Adjustments’ section are required for tuning the software to match your developed hardware. You may also further enhance the performance of the robot by modifying the source code from the custom blocks.

Step 5: Finally, keyboard and mouse controls to control the SBMR are described in ‘Control Methodology’ section.

Hardware Development

Two DC motors are fitted to a suitable base frame along with the wheels. Pan & tilt assembly (fitted with two micro servos) is mounted at suitable location of the frame. A 12V battery is mounted on the base frame. Required electronic modules are suitably placed on the base frame and are connected as per the connection diagram shown in ‘Schematic’ section.Typical arrangement is shown in below figure

Typical arrangement - Arduino Nano RP2040 Connect (left) and Raspberry Pi Pico (right)

This project is build and tested separately on two micro-controllers, Arduino RP2040 Connect and Raspberry Pi Pico. These micro-controller boards are used as the main controller for the robot. They are used in dual core mode and are over clocked to 200MHz and 250MHz for Arduino RP2040 and Raspberry Pi Pico respectively to meet the response times required for balancing the robot and simultaneously communicating with the host PC through WiFi.

Arduino RP2040 is having on-board IMU and WiFi, as such external components required are minimum. Core-0 is used for WiFi communication and all PWM blocks for driving wheels and camera pan & tilt servos. Core-1 is extensively used for IMU and blocks required for balancing the robot.

For Raspberry Pi Pico, Core-0 is used for the blocks required for balancing the robot and core-1 is used for blocks involved in remote communication and manual control of the robot and camera assembly. Inertial measurement unit (IMU) MPU-6050 (or MPU-9250) is used for sensing the inclination (here roll angle because of the mounting direction of IMU) angle of the robot. It communicates with the on-board micro-controller using I2C interface. ESP8266 WiFi module is used for communicating with remote host PC through WiFi. It communicates with the on-board micro-controller using serial interface.

ESP32 Camera module is mounted on the pan & tilt assembly for capturing live video and stream to the host PC. Flashlight present on the ESP32-CAM module is controlled manually from the host PC during low light conditions.

A 9V to 12V battery is used to power the entire circuitry on the robot. A 9V/12V to 5V DC step down converter is used to provide required 5V supply to power the micro-controller, servos and ESP32camera module.

Software Development

A) Configuring ESP32 Camera

ESP32 Camera shall be properly programmed with valid IP address before using it in the project. Please refer to our ESP32-CAM example for details on how to program the module. User may also refer to abundant material available on the internet regarding this subject.

B) Software for micro-controller and native targets

CASP software is used to quickly create models and generate binary code for the on-board micro-controller target and native PC. This software enable users to graphically visualize the signal at any point of the model in real time. This feature is extensively used while tuning the PID controller.

A part from normal CASP blocks two custom blocks (one in the target model and the other in the native model) are used in the models to develop logics that are not possible through normal CASP blocks and to reduce total block count. Source code for the custom blocks is available to the user.

Custom block used in the native model on the host PC generates required control signals when user presses certain keys for controlling the movements of the robot.

Custom block used in the target model modify the inclination angle set point (that is fed to the error block) for navigating the robot based on the user commands from the host PC and also conditions the PID controllers output signals before driving the motors. User can take a look at the configuration and source code of these custom blocks and learn how to integrate the blocks in CASP models along with other blocks. User can go through our video tutorial on how to create a custom block at this https://aadhuniklabs.com/casp_res_videos#custom_block.

Two models are developed to achieve desired objective.

B.1) Target Model that runs on Arduino RP2040 Connect and Raspberry Pi Pico consists of

1) Blink logic that indicates the system is running as well as whether the IMU is working properly or not.

2) IMU, PID controller custom block and other supporting blocks that maintain the balance of the robot.

3) WiFi and supporting blocks that receives required control signals from host PC.

4) PWM and servo blocks that are mapped to the pins of the micro-controller.

1 / 2

Following are the steps to properly program the target board.

1) Connect the target to the host PC via a USB cable.

2) Note the serial port number to which the board that is connected to the host PC, from the host operating system.

3) Run CASP and load the ‘rc_target_arduino’ project for Arduino RP2040 target or ‘rc_target_rpi’ project for Raspberry Pi target. Open the workspace file and adjust various block parameters as described in the Adjustments section.

4) The WiFi communication block is set to Station mode. User may need to enter SSID and password of the network to which the device should be connected. The Local IP address parameter shall be configured as assigned by the network DHCP client of the network.

5) Open Home->Simulation->Setup Simulation Parameters menu item. Under TargetHW->General tabs set ‘Target Hardware Programmer Port’ parameter to the serial port to which the board is connected.

6) Build the model and program the board by clicking on Run button.

B.2) Native Model that runs on Host PC consists of

1) Camera block that receives live video from the ESP32 Camera. IP address of the ESP32 Camera shall be entered in block parameters of this block.

2) Image display block to display the live video received from the camera. It is also configured to output keyboard and mouse signals.

3) RC control block: It is a custom block that receives keyboard and mouse signals from the image display block and generates suitable control signals for controlling the robot motion and its head (pan & tilt) movements.

4) GPIO blocks that maps to the target model via the WiFi communication channel.

Native model that runs on host PC

Following are the steps to run the native model on the host PC

1) Before continuing, the host PC shall be connected to the same network as the device is connected. The robot should be up and running.

2) Load the ‘rc_native’ project.

3) Click on Home->Simulation->Configure Simulation IO menu item.

4) ‘Configure Simulation Hardware’ window will open. Under Native Nodes and GPIO Device Nodes, change the IP addresses marked in the below figure (by double clicking on the item) to respective local and device IP addresses.

Configure Simulation Hardware window to change IP addresses of native node (host PC) and GPIO device (Raspberry Pi Pico)

5) Click on ‘Connect Device’ button and check the ‘Online Data’ check box. The program should now communicate with the target with cycle time around 30msecs. Target board is now available as end point ‘EP0’ to the native model. Native model can use this end point to connect to respective IOs on the target.

6) Click on ‘Save’ button to save the configuration and close the window.

7) Run the model by clicking on the Run button. A simulation panel window should open and communicate with the board.

8) Screen shot of the output simulation panel running on host PC is shown below.

Simulation Panel Window with on-board camera live video

Adjustments

On-board Micro-controller Target

1) IMU sensor offset value (sen_offset block) is required to be adjusted if the center of gravity of the robot is shifted towards one side and/or floor is having some slope.

2) Proportional, integral and derivative parameters of the PID controllers shall be tuned based on the robot wheel diameter, wheel backlash, driving motor gear ratio, IMU sensor location, robot weight, height and center of gravity.

3) ‘PWM Minimum Offset’ parameter of the custom block shall be adjusted to match with the minimum value required to just rotate the motors on load.

4) Any other required logics to improve the performance of the robot may be done in the source code of the custom block.

5) Wheel motor connections may be reversed to get forward or backward movement when W and S are pressed.

Native Target

1) Logics for generating forward and reverse motor rotation are developed to match with the motor driver IC i.e. TA6586 used in the project. User may modify the logics suitably if other driver IC is used.

2) Servo motors may require some alignments to face towards robot forward direction for the default angle specified in the rc_control block parameters.

3) Base speed, speed limits and other parameters related to the navigation are adjustable from the rc_control block parameters.

Control Methodology

1) The simulation panel window shown above accepts keyboard & mouse inputs when it is active.

2) User can use keys W – to move forward, S – to move backward, A – to rotate left on center and D – to rotate right on center.

3) Combination of keys W/S & A/D can be used to take left and right turns while moving forward or backward.

4) Speed can be increased temporarily by using the shift key along with the above keys. Base speed can be adjusted by using Page Up & Page Down keys.

5) Keys K and M can be used to adjust the offset inclination angle where the robot stabilizes at a given location. This value is required if the center of gravity of the robot is shifted towards one side and/or floor is having some slope.

6) Vertical and horizontal servo angles (from -90 to +90 deg) for head position control can be controlled by mouse movements.

7) Key ‘G’ is used to position both servos at default angle.

8) Key 'L' is used to ON/OFF the flash LED light of the ESP32 camera.

Please write to us at https://aadhuniklabs.com/contact for any queries & suggestions related to this project.

Code

Source Code For MCU target and native custom blocks

// MCU target custome block header file code
//CustomBlockSelfBalancing720378270.h
/************************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 CUSTOMBLOCKSELFBALANCING720378270_H
#define CUSTOMBLOCKSELFBALANCING720378270_H

#include "sim_common_definitions.h"

//BLOCK_PREPROC_DEFS_HERE

class SHARED_LIB_TYPE CustomBlockSelfBalancing720378270
{
public:
    CustomBlockSelfBalancing720378270();
    ~CustomBlockSelfBalancing720378270();
    void Initialize(const volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT *arg);
    void PreRun(const volatile GLOBAL_PRERUN_PARAM_STRUCT *arg, _FXDPT_FLOAT *pid_in0, _FXDPT_FLOAT *pid_in1, _INT32 *estop, _BYTE *data_in, _INT32 *comm_len, _INT32 *left_out0, _INT32 *left_out1, _INT32 *right_out0, _INT32 *right_out1, _FXDPT_FLOAT *servo0, _FXDPT_FLOAT *servo1, _INT32 *led0, _FXDPT_FLOAT *sen_offset, _FXDPT_FLOAT *sp_l, _FXDPT_FLOAT *sp_r, _INT32 *rst_i);
    void Run(const volatile GLOBAL_RUN_PARAM_STRUCT *arg, _FXDPT_FLOAT *pid_in0, _FXDPT_FLOAT *pid_in1, _INT32 *estop, _BYTE *data_in, _INT32 *comm_len, _INT32 *left_out0, _INT32 *left_out1, _INT32 *right_out0, _INT32 *right_out1, _FXDPT_FLOAT *servo0, _FXDPT_FLOAT *servo1, _INT32 *led0, _FXDPT_FLOAT *sen_offset, _FXDPT_FLOAT *sp_l, _FXDPT_FLOAT *sp_r, _INT32 *rst_i);
    void PostRun(const volatile GLOBAL_POSTRUN_PARAM_STRUCT *arg, _FXDPT_FLOAT *pid_in0, _FXDPT_FLOAT *pid_in1, _INT32 *estop, _BYTE *data_in, _INT32 *comm_len, _INT32 *left_out0, _INT32 *left_out1, _INT32 *right_out0, _INT32 *right_out1, _FXDPT_FLOAT *servo0, _FXDPT_FLOAT *servo1, _INT32 *led0, _FXDPT_FLOAT *sen_offset, _FXDPT_FLOAT *sp_l, _FXDPT_FLOAT *sp_r, _INT32 *rst_i);
public:
    //char* _dependent_obj;
    //char m_blk_path[SIM_BLK_PATH_MAX_SIZE];
    //port variable declaration
    //block internal parameter variable declaration
    _FLOAT pwm_min_offset, pwm_max_limit;
    _INT32 em_stop_value;
    //block internal initial condition variable declaration
private:
    _BYTE m_prerun_count;
    _UINT64 m_comm_tick_prev, m_speed_tick_prev;
    _INT32 m_left_dir_prev, m_right_dir_prev;
    _INT32 m_left_speed_prev, m_right_speed_prev;
    //
    _FLOAT m_angle_offset_cum[2];
    _FLOAT m_usr_offset;
};

#endif



//MCU target custom block cpp file source code
//CustomBlockSelfBalancing720378270.cpp
/************************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 "CustomBlockSelfBalancing720378270.h"
#include "simcode.h"
#include "math.h"
#include "sim_helper_fg.h"

CustomBlockSelfBalancing720378270::CustomBlockSelfBalancing720378270(){
}

CustomBlockSelfBalancing720378270::~CustomBlockSelfBalancing720378270(){
}

void CustomBlockSelfBalancing720378270::Initialize(const volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT *arg){
    m_prerun_count = 0;
}

void CustomBlockSelfBalancing720378270::PreRun(const volatile GLOBAL_PRERUN_PARAM_STRUCT *arg, _FXDPT_FLOAT *pid_in0, _FXDPT_FLOAT *pid_in1, _INT32 *estop, _BYTE *data_in, _INT32 *comm_len, _INT32 *left_out0, _INT32 *left_out1, _INT32 *right_out0, _INT32 *right_out1, _FXDPT_FLOAT *servo0, _FXDPT_FLOAT *servo1, _INT32 *led0, _FXDPT_FLOAT *sen_offset, _FXDPT_FLOAT *sp_l, _FXDPT_FLOAT *sp_r, _INT32 *rst_i){
    if(m_prerun_count++ != 1)
    {
        *pid_in0 = 0;
        *pid_in1 = 0;
        for(int i=0; i<20; ++i)
            data_in[i] = 0;
        *comm_len = 0;
        for(int i=0; i<3; ++i)
            estop[i] = 0;
        return;
    }
    m_left_dir_prev = 0;
    m_right_dir_prev = 0;
    m_left_speed_prev = 0;
    m_right_speed_prev = 0;
    //
    *servo0 = *((_FXDPT_FLOAT*)&data_in[20]);
    *servo1 = *((_FXDPT_FLOAT*)&data_in[24]);
    *led0 = data_in[28];
    //
    *left_out0 = *left_out1 = *right_out0 = *right_out1 = 0;
    *rst_i = 1000;
    *sp_l = *sp_r = 0;
    m_comm_tick_prev = m_speed_tick_prev = arg->rt_clk_tick;
    m_usr_offset = 0;
}

void CustomBlockSelfBalancing720378270::Run(const volatile GLOBAL_RUN_PARAM_STRUCT *arg, _FXDPT_FLOAT *pid_in0, _FXDPT_FLOAT *pid_in1, _INT32 *estop, _BYTE *data_in, _INT32 *comm_len, _INT32 *left_out0, _INT32 *left_out1, _INT32 *right_out0, _INT32 *right_out1, _FXDPT_FLOAT *servo0, _FXDPT_FLOAT *servo1, _INT32 *led0, _FXDPT_FLOAT *sen_offset, _FXDPT_FLOAT *sp_l, _FXDPT_FLOAT *sp_r, _INT32 *rst_i){
    //Note: run this block in sync with PID blocks otherwise PID integral reset signal may not work
    *servo0 = *((_FXDPT_FLOAT*)&data_in[20]);
    *servo1 = *((_FXDPT_FLOAT*)&data_in[24]);
    *led0 = data_in[28];
    //adjust set point angle w.r.t manual control
    _BIT comm_stopped = false;
    if(*comm_len > 0)
        m_comm_tick_prev = arg->rt_clk_tick;
    else
    {
        if(arg->rt_clk_tick - m_comm_tick_prev > 500000)
            comm_stopped = true;
    }
    //sensor offsets
    m_usr_offset += ((_FLOAT)*((_INT32*)&data_in[16])) * 0.01;
    _FLOAT offset = *sen_offset + m_usr_offset;
    //
    _FLOAT &spl = *sp_l;
    _FLOAT &spr = *sp_r;
    _INT32 left_dir = *((_INT32*)&data_in[0]);
    _INT32 left_speed = *((_INT32*)&data_in[4]);
    _INT32 right_dir = *((_INT32*)&data_in[8]);
    _INT32 right_speed = *((_INT32*)&data_in[12]);
    spl = spr = 0;
    //initially, speed is required to overcome static friction. reduce the speed after some time for stability
    if(left_speed != 0 || right_speed != 0)
    {
        if(m_left_speed_prev == 0 && m_right_speed_prev == 0)//user just given command to move or rotate
            m_speed_tick_prev = arg->rt_clk_tick;
        if(arg->rt_clk_tick - m_speed_tick_prev > 900000)
        {
            left_speed /= 4;
            right_speed /= 4;
        }
        else if(arg->rt_clk_tick - m_speed_tick_prev > 500000)
        {
            left_speed /= 2;
            right_speed /= 2;
        }
        else if(arg->rt_clk_tick - m_speed_tick_prev > 300000)
        {
            left_speed = left_speed*2/3;
            right_speed = right_speed*2/3;
        }
    }
    //reset manual control when user is not connected
    if(comm_stopped)
    {
        spl = 0;
        spr = 0;
        //*servo0 = *servo1 = 0;//we dont know offsets and it is not mandatory to make is zero
        *led0 = 0;
    }
    else
    {
        spl = left_speed;
        spr = right_speed;
        //set direction
        spl = (left_dir)?spl:-spl;
        spr = (right_dir)?spr:-spr;
        //to actual
        spl /= 19.0;// /19.0 for arduino rp2040 and /10.0 for Rpi Pico
        spr /= 19.0;// /19.0 for arduino rp2040 and /10.0 for Rpi Pico
    }
    spl += offset;
    spr += offset;
    //reset pid block integral term on following logics
    *rst_i = 0;
    if(left_speed == 0 && right_speed == 0)//only during braking
    {
        if(left_dir != m_left_dir_prev || right_dir != m_right_dir_prev)//when change in direction
            *rst_i = 10;
        if(left_speed != m_left_speed_prev || right_speed != m_right_speed_prev)
            *rst_i = 10;
    }
    *rst_i -= 1;
    //
    m_left_dir_prev = left_dir;
    m_right_dir_prev = right_dir;
    m_left_speed_prev = left_speed;
    m_right_speed_prev = right_speed;

    //process outputs
    _FLOAT in[2], out[2];
    in[0] = -*pid_in0;
    in[1] = -*pid_in1;
    for(int i=0; i<2; ++i)
    {
        out[i] = in[i];
        //absolute
        if(out[i] < 0) out[i] = -out[i];
        //map to minimum & maximum pwm outputs to turn the wheel
        out[i] = MapRangeT(out[i], (_FLOAT)pwm_max_limit, (_FLOAT)pwm_min_offset, (_FLOAT)100, (_FLOAT)0);
        //stop when the input angle is greater than set angle
        if(estop[1] > em_stop_value || estop[1] < -em_stop_value)
            out[i] = 0;
    }
    //set motor speed and direction
    if(in[0] < 0)
    {
        *left_out0 = out[0];
        *left_out1 = 0;
    }
    else
    {
        *left_out0 = 0;
        *left_out1 = out[0];
    }
    if(in[1] < 0)
    {
        *right_out0 = 0;
        *right_out1 = out[1];
    }
    else
    {
        *right_out0 = out[1];
        *right_out1 = 0;
    }
}

void CustomBlockSelfBalancing720378270::PostRun(const volatile GLOBAL_POSTRUN_PARAM_STRUCT *arg, _FXDPT_FLOAT *pid_in0, _FXDPT_FLOAT *pid_in1, _INT32 *estop, _BYTE *data_in, _INT32 *comm_len, _INT32 *left_out0, _INT32 *left_out1, _INT32 *right_out0, _INT32 *right_out1, _FXDPT_FLOAT *servo0, _FXDPT_FLOAT *servo1, _INT32 *led0, _FXDPT_FLOAT *sen_offset, _FXDPT_FLOAT *sp_l, _FXDPT_FLOAT *sp_r, _INT32 *rst_i){
}



//native (host PC) target custom block header file code
//RemCarManual927823855.h
/************************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
#define RC_SHIFT_GAIN (1.3f)

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


//native (host PC) target custom block cpp file code
//RemCarManual927823855.cpp
/************************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;
    //
    _FLOAT bspeed = base_speed;
    if(kb_in[16])//shift pressed increase speed temporarly
        bspeed *= RC_SHIFT_GAIN;
    //adjust speed
    if(kb_in[34])//pg down
    {
        _FLOAT val = bspeed * (1.0f-speed_step);
        if(val > min_speed)
            bspeed = val;
    }
    else if(kb_in[33])//pg up
    {
        _FLOAT val = bspeed * (1.0f+speed_step);
        if(val < max_speed)
            bspeed = val;
    }
    //forward and backward
    if(kb_in[87])//W-forward
    {
        *l_speed_out = *r_speed_out = bspeed;
        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 = bspeed;
        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 = bspeed/2;
            *l_dir_out = 0;
            *r_dir_out = 0;
        }
        else if(kb_in[68])//D-right
        {
            *l_speed_out = *r_speed_out = bspeed/2;
            *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){
}

Credits

Aadhuniklabs

7 projects • 15 followers

Makers and tech enthusiast since 2022

Self Balancing WiFi Controlled Robot with PanTilt Camera

Things used in this project

Hardware components

Software apps and online services

Story

Getting Started

Hardware Development

Software Development

Adjustments

Control Methodology

Schematics

Connection Diagram for Arduino Nano RP2040 Connect

Connection Diagram for Raspberry Pi Pico

Models for Arduino Nano RP2040 Connect

Models for Raspberry Pi Pico

Code

Source Code For MCU target and native custom blocks

Credits

Aadhuniklabs

Comments

Embed the widget on your own site

Self Balancing WiFi Controlled Robot with PanTilt Camera

Self Balancing WiFi Controlled Robot with PanTilt Camera

Things used in this project

Hardware components

Software apps and online services

Story

Getting Started

Hardware Development

Software Development

Adjustments

Control Methodology

Schematics

Connection Diagram for Arduino Nano RP2040 Connect

Connection Diagram for Raspberry Pi Pico

Models for Arduino Nano RP2040 Connect

Models for Raspberry Pi Pico

Code

Source Code For MCU target and native custom blocks

Credits

Aadhuniklabs

Comments

Related channels and tags