Self Balancing WiFi Controlled Robot with PanTilt Camera

1// MCU target custome block header file code
2//CustomBlockSelfBalancing720378270.h
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  CUSTOMBLOCKSELFBALANCING720378270_H
10#define CUSTOMBLOCKSELFBALANCING720378270_H
11
12#include  "sim_common_definitions.h"
13
14//BLOCK_PREPROC_DEFS_HERE
15
16class SHARED_LIB_TYPE  CustomBlockSelfBalancing720378270
17{
18public:
19    CustomBlockSelfBalancing720378270();
20    ~CustomBlockSelfBalancing720378270();
21    void Initialize(const volatile  BLOCK_CONSTRUCTOR_PARAM_STRUCT *arg);
22    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);
23    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);
24    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);
25public:
26    //char* _dependent_obj;
27    //char m_blk_path[SIM_BLK_PATH_MAX_SIZE];
28    //port variable declaration
29    //block internal parameter variable declaration
30    _FLOAT pwm_min_offset, pwm_max_limit;
31    _INT32 em_stop_value;
32    //block  internal initial condition variable declaration
33private:
34    _BYTE m_prerun_count;
35    _UINT64 m_comm_tick_prev, m_speed_tick_prev;
36    _INT32 m_left_dir_prev,  m_right_dir_prev;
37    _INT32 m_left_speed_prev, m_right_speed_prev;
38    //
39    _FLOAT m_angle_offset_cum[2];
40    _FLOAT m_usr_offset;
41};
42
43#endif
44
45
46
47//MCU  target custom block cpp file source code
48//CustomBlockSelfBalancing720378270.cpp
49/************************CASP_COPYRIGHT**********************************
50**  Copyright (c) AadhunikLabs. All rights reserved.
51** This file is part of CASP.
52**  Licensees holding valid licenses may use this file in accordance
53** with the  license agreement provided with the software.
54*************************************************************************/
55#include  "CustomBlockSelfBalancing720378270.h"
56#include "simcode.h"
57#include "math.h"
58#include  "sim_helper_fg.h"
59
60CustomBlockSelfBalancing720378270::CustomBlockSelfBalancing720378270(){
61}
62
63CustomBlockSelfBalancing720378270::~CustomBlockSelfBalancing720378270(){
64}
65
66void  CustomBlockSelfBalancing720378270::Initialize(const volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT  *arg){
67    m_prerun_count = 0;
68}
69
70void 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){
71    if(m_prerun_count++ != 1)
72    {
73        *pid_in0 =  0;
74        *pid_in1 = 0;
75        for(int i=0; i<20; ++i)
76            data_in[i]  = 0;
77        *comm_len = 0;
78        for(int i=0; i<3; ++i)
79            estop[i]  = 0;
80        return;
81    }
82    m_left_dir_prev = 0;
83    m_right_dir_prev  = 0;
84    m_left_speed_prev = 0;
85    m_right_speed_prev = 0;
86    //
87    *servo0 = *((_FXDPT_FLOAT*)&data_in[20]);
88    *servo1 = *((_FXDPT_FLOAT*)&data_in[24]);
89    *led0 = data_in[28];
90    //
91    *left_out0 = *left_out1 = *right_out0  = *right_out1 = 0;
92    *rst_i = 1000;
93    *sp_l = *sp_r = 0;
94    m_comm_tick_prev  = m_speed_tick_prev = arg->rt_clk_tick;
95    m_usr_offset = 0;
96}
97
98void  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){
99    //Note: run this  block in sync with PID blocks otherwise PID integral reset signal may not work
100    *servo0 = *((_FXDPT_FLOAT*)&data_in[20]);
101    *servo1 = *((_FXDPT_FLOAT*)&data_in[24]);
102    *led0 = data_in[28];
103    //adjust set point angle w.r.t manual control
104    _BIT comm_stopped = false;
105    if(*comm_len > 0)
106        m_comm_tick_prev  = arg->rt_clk_tick;
107    else
108    {
109        if(arg->rt_clk_tick - m_comm_tick_prev  > 500000)
110            comm_stopped = true;
111    }
112    //sensor offsets
113    m_usr_offset += ((_FLOAT)*((_INT32*)&data_in[16])) * 0.01;
114    _FLOAT offset  = *sen_offset + m_usr_offset;
115    //
116    _FLOAT &spl = *sp_l;
117    _FLOAT  &spr = *sp_r;
118    _INT32 left_dir = *((_INT32*)&data_in[0]);
119    _INT32 left_speed  = *((_INT32*)&data_in[4]);
120    _INT32 right_dir = *((_INT32*)&data_in[8]);
121    _INT32 right_speed = *((_INT32*)&data_in[12]);
122    spl = spr = 0;
123    //initially,  speed is required to overcome static friction. reduce the speed after some time  for stability
124    if(left_speed != 0 || right_speed != 0)
125    {
126        if(m_left_speed_prev  == 0 && m_right_speed_prev == 0)//user just given command to move or rotate
127            m_speed_tick_prev = arg->rt_clk_tick;
128        if(arg->rt_clk_tick  - m_speed_tick_prev > 900000)
129        {
130            left_speed /= 4;
131            right_speed  /= 4;
132        }
133        else if(arg->rt_clk_tick - m_speed_tick_prev > 500000)
134        {
135            left_speed /= 2;
136            right_speed /= 2;
137        }
138        else if(arg->rt_clk_tick - m_speed_tick_prev > 300000)
139        {
140            left_speed = left_speed*2/3;
141            right_speed = right_speed*2/3;
142        }
143    }
144    //reset manual control when user is not connected
145    if(comm_stopped)
146    {
147        spl = 0;
148        spr = 0;
149        //*servo0  = *servo1 = 0;//we dont know offsets and it is not mandatory to make is zero
150        *led0 = 0;
151    }
152    else
153    {
154        spl = left_speed;
155        spr = right_speed;
156        //set direction
157        spl = (left_dir)?spl:-spl;
158        spr = (right_dir)?spr:-spr;
159        //to actual
160        spl /= 19.0;//  /19.0 for arduino rp2040 and /10.0 for Rpi Pico
161        spr /= 19.0;// /19.0  for arduino rp2040 and /10.0 for Rpi Pico
162    }
163    spl += offset;
164    spr  += offset;
165    //reset pid block integral term on following logics
166    *rst_i  = 0;
167    if(left_speed == 0 && right_speed == 0)//only during braking
168    {
169        if(left_dir != m_left_dir_prev || right_dir != m_right_dir_prev)//when change  in direction
170            *rst_i = 10;
171        if(left_speed != m_left_speed_prev  || right_speed != m_right_speed_prev)
172            *rst_i = 10;
173    }
174    *rst_i  -= 1;
175    //
176    m_left_dir_prev = left_dir;
177    m_right_dir_prev = right_dir;
178    m_left_speed_prev = left_speed;
179    m_right_speed_prev = right_speed;
180
181    //process outputs
182    _FLOAT in[2], out[2];
183    in[0] = -*pid_in0;
184    in[1] = -*pid_in1;
185    for(int i=0; i<2; ++i)
186    {
187        out[i]  = in[i];
188        //absolute
189        if(out[i] < 0) out[i] = -out[i];
190        //map  to minimum & maximum pwm outputs to turn the wheel
191        out[i] = MapRangeT(out[i],  (_FLOAT)pwm_max_limit, (_FLOAT)pwm_min_offset, (_FLOAT)100, (_FLOAT)0);
192        //stop  when the input angle is greater than set angle
193        if(estop[1] > em_stop_value  || estop[1] < -em_stop_value)
194            out[i] = 0;
195    }
196    //set motor  speed and direction
197    if(in[0] < 0)
198    {
199        *left_out0 = out[0];
200        *left_out1 = 0;
201    }
202    else
203    {
204        *left_out0 = 0;
205        *left_out1 = out[0];
206    }
207    if(in[1] < 0)
208    {
209        *right_out0  = 0;
210        *right_out1 = out[1];
211    }
212    else
213    {
214        *right_out0  = out[1];
215        *right_out1 = 0;
216    }
217}
218
219void 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){
220}
221
222
223
224//native (host PC) target custom block header  file code
225//RemCarManual927823855.h
226/************************CASP_COPYRIGHT**********************************
227**  Copyright (c) AadhunikLabs. All rights reserved.
228** This file is part of CASP.
229**  Licensees holding valid licenses may use this file in accordance
230** with the  license agreement provided with the software.
231*************************************************************************/
232#ifndef  REMCARMANUAL927823855_H
233#define REMCARMANUAL927823855_H
234
235#include "sim_common_definitions.h"
236
237#define  UPDATE_INTERVAL 10UL//in milli secs
238#define RC_SHIFT_GAIN (1.3f)
239
240class  SHARED_LIB_TYPE RemCarManual927823855
241{
242public:
243    RemCarManual927823855();
244    ~RemCarManual927823855();
245    void Initialize(const volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT  *arg);
246    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);
247    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);
248    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);
249    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);
250public:
251    //port  variable declaration
252    //port dimension and size declaration
253    //block  internal parameter variable declaration
254    _FLOAT base_speed, min_speed, max_speed,  speed_step;
255    _FLOAT rotate_speed;
256    _FLOAT head_speed[2];
257    _FLOAT  head_hangle[3], head_vangle[3];//min,max,def
258    _BYTE head_control;
259    //block  internal initial condition variable declaration
260private:
261    _FLOAT rotate_speed_inc,  rotate_speed_dec;
262    _BOOLEAN m_head_speed_reverse[2];
263    int m_init_mouse_pos[2];//during  last head movement
264    int m_prev_mouse_pos[2];//prev position
265    _UINT64  m_update_count;
266};
267
268#endif
269
270
271//native (host PC) target custom  block cpp file code
272//RemCarManual927823855.cpp
273/************************CASP_COPYRIGHT**********************************
274**  Copyright (c) AadhunikLabs. All rights reserved.
275** This file is part of CASP.
276**  Licensees holding valid licenses may use this file in accordance
277** with the  license agreement provided with the software.
278*************************************************************************/
279#include  "RemCarManual927823855.h"
280#include "math.h"
281#include "simcode.h"
282
283RemCarManual927823855::RemCarManual927823855(){
284}
285
286RemCarManual927823855::~RemCarManual927823855(){
287}
288
289void  RemCarManual927823855::Initialize(const volatile BLOCK_CONSTRUCTOR_PARAM_STRUCT  *arg){
290}
291
292void 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){
293    m_init_mouse_pos[0]  = kb_in[257];
294    m_init_mouse_pos[1] = kb_in[258];
295    m_prev_mouse_pos[0]  = kb_in[257];
296    m_prev_mouse_pos[1] = kb_in[258];
297    //
298    *l_speed_out  = 0;
299    *l_dir_out = 1;
300    *r_speed_out = 0;
301    *r_dir_out = 1;
302    //
303    *v_angle_out = head_vangle[2];
304    *h_angle_out = head_hangle[2];
305    //
306    rotate_speed_inc = 1.0f+(rotate_speed/100.0f);
307    rotate_speed_dec  = 1.0f-(rotate_speed/100.0f);
308    speed_step = speed_step/100.0f;
309    m_update_count  = arg->rt_clk_tick;
310    //
311    for(int i=0; i<2; ++i)
312    {
313        m_head_speed_reverse[i]  = (head_speed[i] < 0)?1:0;
314        head_speed[i] = (head_speed[i] < 0)?-head_speed[i]:head_speed[i];
315    }
316}
317
318void 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){
319    if(arg->rt_clk_tick-m_update_count  < UPDATE_INTERVAL*1000)
320        return;
321    m_update_count = arg->rt_clk_tick;
322    //
323    *l_speed_out = 0;
324    *r_speed_out = 0;
325    *l_dir_out = 1;
326    *r_dir_out = 1;
327    //
328    _FLOAT bspeed = base_speed;
329    if(kb_in[16])//shift  pressed increase speed temporarly
330        bspeed *= RC_SHIFT_GAIN;
331    //adjust  speed
332    if(kb_in[34])//pg down
333    {
334        _FLOAT val = bspeed * (1.0f-speed_step);
335        if(val > min_speed)
336            bspeed = val;
337    }
338    else if(kb_in[33])//pg  up
339    {
340        _FLOAT val = bspeed * (1.0f+speed_step);
341        if(val  < max_speed)
342            bspeed = val;
343    }
344    //forward and backward
345    if(kb_in[87])//W-forward
346    {
347        *l_speed_out = *r_speed_out = bspeed;
348        if(kb_in[65])//A-left
349        {
350            *l_speed_out *= rotate_speed_dec;
351            *r_speed_out *= rotate_speed_inc;
352        }
353        else if(kb_in[68])//D-right
354        {
355            *l_speed_out *= rotate_speed_inc;
356            *r_speed_out  *= rotate_speed_dec;
357        }
358        //check limits
359        if(*l_speed_out  > max_speed)
360            *l_speed_out = max_speed;
361        else if(*l_speed_out  < min_speed)
362            *l_speed_out = min_speed;
363        if(*r_speed_out  > max_speed)
364            *r_speed_out = max_speed;
365        else if(*r_speed_out  < min_speed)
366            *r_speed_out = min_speed;
367        //
368        *l_dir_out  = 1;
369        *r_dir_out = 0;
370    }
371    else if(kb_in[83])//S-reverse
372    {
373        *l_speed_out = *r_speed_out = bspeed;
374        if(kb_in[65])//A-left
375        {
376            *l_speed_out *= rotate_speed_dec;
377            *r_speed_out  *= rotate_speed_inc;
378        }
379        else if(kb_in[68])//D-right
380        {
381            *l_speed_out *= rotate_speed_inc;
382            *r_speed_out *= rotate_speed_dec;
383        }
384        //check limits
385        if(*l_speed_out > max_speed)
386            *l_speed_out = max_speed;
387        else if(*l_speed_out < min_speed)
388            *l_speed_out = min_speed;
389        if(*r_speed_out > max_speed)
390            *r_speed_out = max_speed;
391        else if(*r_speed_out < min_speed)
392            *r_speed_out = min_speed;
393        //
394        *l_dir_out = 0;
395        *r_dir_out = 1;
396    }
397    //if no forward or reverse pressed. then  rotate on center
398    else//if(kb_in[87] == 0 && kb_in[83] == 0)
399    {
400        if(kb_in[65])//A-left
401        {
402            *l_speed_out = *r_speed_out  = bspeed/2;
403            *l_dir_out = 0;
404            *r_dir_out = 0;
405        }
406        else if(kb_in[68])//D-right
407        {
408            *l_speed_out = *r_speed_out  = bspeed/2;
409            *l_dir_out = 1;
410            *r_dir_out = 1;
411        }
412    }
413    //head rotation
414    if(head_control == 1)//from mouse
415    {
416        int x = kb_in[257];
417        int y = kb_in[258];
418        int diffx  = x - m_init_mouse_pos[0];
419        int diffy = y - m_init_mouse_pos[1];
420        if(diffx  != 0)
421        {
422            //if(x == m_prev_mouse_pos[0])//user stabilized  mouse position. move head
423            {
424                if(kb_in[256] & 0b10)//if  right button pressed then only move mouse
425                {
426                    if(m_head_speed_reverse[0])
427                        *h_angle_out += (diffx * head_speed[0] * 0.1);
428                    else
429                        *h_angle_out -= (diffx * head_speed[0] * 0.1);
430                    if(*h_angle_out  < head_hangle[0])
431                        *h_angle_out = head_hangle[0];
432                    else if(*h_angle_out > head_hangle[1])
433                        *h_angle_out  = head_hangle[1];
434                }
435                m_init_mouse_pos[0] =  x;
436            }
437        }
438        if(diffy != 0)
439        {
440            //if(y  == m_prev_mouse_pos[1])//user stabilized mouse position. move head
441            {
442                if(kb_in[256] & 0b10)//if right button pressed then only move mouse
443                {
444                    if(m_head_speed_reverse[1])
445                        *v_angle_out  -= (diffy * head_speed[1] * 0.1);
446                    else
447                        *v_angle_out  += (diffy * head_speed[1] * 0.1);
448                    if(*v_angle_out < head_vangle[0])
449                        *v_angle_out = head_vangle[0];
450                    else  if(*v_angle_out > head_vangle[1])
451                        *v_angle_out = head_vangle[1];
452                }
453                m_init_mouse_pos[1] = y;
454            }
455        }
456        m_prev_mouse_pos[0] = x;
457        m_prev_mouse_pos[1] = y;
458    }
459    else//from arrow keys
460    {
461        //37-left, 38-up,39-right,40-bottom
462        if((kb_in[37] && m_head_speed_reverse[0] == false) || (kb_in[39] && m_head_speed_reverse[0]  == true))
463        {
464            *h_angle_out += (head_speed[0]);
465            if(*h_angle_out  > head_hangle[1])
466                *h_angle_out = head_hangle[1];
467        }
468        else if((kb_in[39] && m_head_speed_reverse[0] == false) || (kb_in[37] &&  m_head_speed_reverse[0] == true))
469        {
470            *h_angle_out += (-head_speed[0]);
471            if(*h_angle_out < head_hangle[0])
472                *h_angle_out =  head_hangle[0];
473        }
474        if((kb_in[38] && m_head_speed_reverse[1]  == false) || (kb_in[40] && m_head_speed_reverse[1] == true))
475        {
476            *v_angle_out  += (-head_speed[1]);
477            if(*v_angle_out < head_vangle[0])
478                *v_angle_out  = head_vangle[0];
479        }
480        else if((kb_in[40] && m_head_speed_reverse[1]  == false) || (kb_in[38] && m_head_speed_reverse[1] == true))
481        {
482            *v_angle_out  += (head_speed[1]);
483            if(*v_angle_out > head_vangle[1])
484                *v_angle_out  = head_vangle[1];
485        }
486    }
487    //center horizontal and vertical  heads when G is pressed
488    if(kb_in[71])
489    {
490        *h_angle_out =  head_hangle[2];
491        *v_angle_out = head_vangle[2];
492    }
493}
494
495void  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){
496    Run(arg, kb_in, l_speed_out,  l_dir_out, r_speed_out, r_dir_out, h_angle_out, v_angle_out);
497}
498
499void 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){
500}
501
502