lift: added zero owerflow detection

	added pusher lenght measurement

diff --git a/src/eb_vytah_09/pusher.c b/src/eb_vytah_09/pusher.c
index 548996423adcd5404196c22e11c37454cbe782f5..d9929f210ae6efa5d24962cce7293a700add92a5 100644 (file)
--- a/src/eb_vytah_09/pusher.c
+++ b/src/eb_vytah_09/pusher.c
@@ -16,8 +16,12 @@
 #define IRC_TIMOUT 5
 volatile uint32_t irc_timeout =0;
 static uint32_t irc_last = 0;
+uint32_t pusher_irc_lenght = 0;                /// contains auto measured lenght of pusher in IRC ticks
 #define ENG_SPEED_PUSH         100
 
+
+#define IRC_PUSHER     irc1_count
+
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 //~~~~~~~~~~~~~~ Interrupt service rutines ~~~~~~~~~~~~~~~
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
@@ -38,6 +42,8 @@ void EINT2_rutine(void){
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 error_messages pusher_init(void){
        error_messages err = NO_ERROR;
+       uint32_t irc_pom;
+       
        //if(tbit(flags,PUCK_LOADED))
        //      return(ERROR_PUCK_LOADED);// no initialization is possible if puck is loaded in holder
 //     if(!(tbit(flags,HOLDER_OPEN)))
@@ -54,7 +60,7 @@ error_messages pusher_init(void){
        
        // dummy wait
        
-       cnt_IRC_pusher = 0;     //clear counter of IRC for pusher
+       irc_pom = IRC_PUSHER;   //clear counter of IRC for pusher
        cbit(flags,TIME_ERR);
        last_time = get_timer_msec();//save time when motore start
        while((tbit(flags,TIME_ERR) == 0)){//move pusher until bit is zero/ bit is switch input
@@ -71,6 +77,7 @@ error_messages pusher_init(void){
                        sbit(flags, TIME_ERR);
        } 
        move_pusher(ENGINE_BW, 0);// stop moving to back bound width max velocitycbit(flags, TIME_ERR);
+       pusher_irc_lenght = IRC_PUSHER - irc_pom;
        
        // TODO: dodelat spocitani delky trasy
        return(err);
@@ -104,7 +111,7 @@ void stop_pusher(struct fsm *fsm, events my_event){
        switch(my_event){
                case EVENT_ENTRY:
                        move_pusher(ENGINE_FW, 0);// stop moving 
-                       set_holder(60);//close holder  TODO: ?????
+                       set_holder(60);//close holder  TODO  ????
                        uart_send_char('S');    //Stop Lift Do
                        uart_send_char('P');
                        uart_send_char('E');
@@ -132,13 +139,18 @@ void move_pusher_pos(struct fsm *fsm, events my_event){
        switch(my_event){
                case EVENT_ENTRY:
                        cnt_IRC_pusher = 0;
-                       irc_last = irc1_count;
+                       irc_last = IRC_PUSHER;
                        match_IRC_pusher =irc_last + count_pusher_IRC(req_position_pusher, act_position_pusher);
                                send_rs_str("match_IRC_pusher:");
                                send_rs_int(match_IRC_pusher);
                                uart_send_char('\n');
                        move_pusher(pusher_dir, ENG_SPEED_PUSH);
                        last_time = get_timer_msec();//save time when moving start
+                       
+                       if (act_position_pusher > 0x10000)              
+                                 // zero overflow FIXME: should be changed to end switch detection
+                                             act_position_pusher = 0;
+               
                break;
                case EVENT_DO:
                        //wait for commands
@@ -146,14 +158,14 @@ void move_pusher_pos(struct fsm *fsm, events my_event){
                        {
                                fsm->current_state = &stop_pusher;      //stop pusher if timeout
                                if(pusher_dir == ENGINE_FW)
-                                       act_position_pusher += (irc1_count - irc_last);
+                                       act_position_pusher += (IRC_PUSHER - irc_last);
                                else
-                                       act_position_pusher -= (irc1_count - irc_last); 
+                                       act_position_pusher -= (IRC_PUSHER - irc_last); 
                                
                                send_rs_str("move_pusher_pos: timedout\n");
                                send_rs_str("do_act_pos_tim:");
                                      send_rs_int(act_position_pusher);
-                                     uart_send_char('\n');
+                               uart_send_char('\n');
                        }
                        
                        
@@ -164,9 +176,9 @@ void move_pusher_pos(struct fsm *fsm, events my_event){
                                      send_rs_int(act_position_pusher);
                                      uart_send_char('\n');
                                if(pusher_dir == ENGINE_FW)
-                                       act_position_pusher += (irc1_count - irc_last);
+                                       act_position_pusher += (IRC_PUSHER - irc_last);
                                else
-                                       act_position_pusher -= (irc1_count - irc_last); 
+                                       act_position_pusher -= (IRC_PUSHER - irc_last); 
                                    send_rs_str("do_act_pos_post:");
                                    send_rs_int(act_position_pusher);
                                    uart_send_char('\n');