A Simple WiFi controlled mobile robot with pan & tilt camera

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