lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

idle_check.h@4:bf278ddb8504, 2020-12-08 (annotated)

Committer:
lsh3146
Date:
Tue Dec 08 01:25:06 2020 +0000
Revision:
4:bf278ddb8504
Parent:
2:14b52dee1c35 
aaaaaqqqqq

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gohgwaja 1:7b5469bf5994 1 #ifndef _IDLE_CHECK_H_
gohgwaja 1:7b5469bf5994 2 #define _IDLE_CHECK_H_
gohgwaja 1:7b5469bf5994 3
lsh3146 4:bf278ddb8504 4 #define IDLE_CHECK_CNT 1400 //loop(2.5ms) * IDLE_CHECK_CNT = idle check time
gohgwaja 1:7b5469bf5994 5
lsh3146 4:bf278ddb8504 6 unsigned int ex_cmd_roll=0;
lsh3146 4:bf278ddb8504 7 unsigned int ex_cmd_pitch=0;
lsh3146 4:bf278ddb8504 8 unsigned int ex_cmd_heave=0;
lsh3146 4:bf278ddb8504 9 unsigned int ex_cmd_sway=0;
lsh3146 4:bf278ddb8504 10 unsigned int ex_cmd_surge=0;
lsh3146 4:bf278ddb8504 11 unsigned int ex_cmd_yaw=0;
gohgwaja 1:7b5469bf5994 12
lsh3146 4:bf278ddb8504 13 bool idle_state = false;
lsh3146 4:bf278ddb8504 14 int idle_cnt = 0;
gohgwaja 1:7b5469bf5994 15
lsh3146 4:bf278ddb8504 16 void idle_check(unsigned int roll, unsigned int pitch, unsigned int heave, unsigned int sway, unsigned int surge, unsigned int yaw)
gohgwaja 1:7b5469bf5994 17 {
gohgwaja 1:7b5469bf5994 18
lsh3146 4:bf278ddb8504 19 if( roll == ex_cmd_roll &&
lsh3146 4:bf278ddb8504 20 pitch == ex_cmd_pitch &&
lsh3146 4:bf278ddb8504 21 heave == ex_cmd_heave &&
lsh3146 4:bf278ddb8504 22 sway == ex_cmd_sway &&
lsh3146 4:bf278ddb8504 23 surge == ex_cmd_surge &&
lsh3146 4:bf278ddb8504 24 yaw == ex_cmd_yaw )
lsh3146 4:bf278ddb8504 25 {
lsh3146 4:bf278ddb8504 26 idle_cnt++;
lsh3146 4:bf278ddb8504 27 }else
gohgwaja 1:7b5469bf5994 28 {
lsh3146 4:bf278ddb8504 29 idle_cnt = idle_cnt - 50;
gohgwaja 1:7b5469bf5994 30 }
lsh3146 4:bf278ddb8504 31
lsh3146 4:bf278ddb8504 32 if(idle_cnt > IDLE_CHECK_CNT)
lsh3146 4:bf278ddb8504 33 idle_cnt = IDLE_CHECK_CNT;
lsh3146 4:bf278ddb8504 34 if(idle_cnt < 0)
lsh3146 4:bf278ddb8504 35 idle_cnt = 0;
lsh3146 4:bf278ddb8504 36
lsh3146 4:bf278ddb8504 37 if(idle_cnt > 1100)
lsh3146 4:bf278ddb8504 38 idle_state = true;
lsh3146 4:bf278ddb8504 39 else
lsh3146 4:bf278ddb8504 40 idle_state = false;
lsh3146 4:bf278ddb8504 41
lsh3146 4:bf278ddb8504 42
lsh3146 4:bf278ddb8504 43 ex_cmd_roll = roll;
lsh3146 4:bf278ddb8504 44 ex_cmd_pitch = pitch;
lsh3146 4:bf278ddb8504 45 ex_cmd_heave = heave;
lsh3146 4:bf278ddb8504 46 ex_cmd_sway = sway;
lsh3146 4:bf278ddb8504 47 ex_cmd_surge = surge;
lsh3146 4:bf278ddb8504 48 ex_cmd_yaw = yaw;
gohgwaja 1:7b5469bf5994 49 }
gohgwaja 1:7b5469bf5994 50
gohgwaja 1:7b5469bf5994 51 #endif