eb_lift_11: add irc and end switches, homing

diff --git a/src/eb_lift_11/fsm.h b/src/eb_lift_11/fsm.h
index 56e59de0c32abde57dbf603d4be92748a5912468..4dc921f9b3979a2a674a199dda97341a10171afd 100644 (file)
--- a/src/eb_lift_11/fsm.h
+++ b/src/eb_lift_11/fsm.h
@@ -31,6 +31,9 @@ struct fsm {
        bool trigger_can_send;
 };
 
+#define END_SWITCH_UP  9       //pin 4, exp. port on board, nc contact (rozpinaci)
+#define END_SWITCH_DOWN        8       //pin 3, exp. port on board, nc contact (rozpinaci)
+
 void init_fsm(struct fsm *fsm, state_fcn initial_state);
 void run_fsm(struct fsm *fsm);
 

diff --git a/src/eb_lift_11/fsm_lift.c b/src/eb_lift_11/fsm_lift.c
index cc20998a474c2ff4070dc45d936292b6499212be..afcc91ab8f5abb39575ee6f0e1b98d09bf5b2a5c 100644 (file)
--- a/src/eb_lift_11/fsm_lift.c
+++ b/src/eb_lift_11/fsm_lift.c
@@ -28,7 +28,12 @@ void fsm_lift_init(struct fsm *fsm, enum event event)
                fsm->flags |= CAN_LIFT_INITIALIZING;
                break;
        case EVENT_DO:
-               /* TODO: Homing */
+               //homing, TODO add timeout
+               while(IO0PIN & (1<<END_SWITCH_UP)){
+                 engine_A_pwm(50);
+               }
+               engine_A_pwm(0);
+               fsm->act_pos=0;
                fsm->flags &= ~CAN_LIFT_INITIALIZING;
                fsm->current_state = wait_for_cmd;
                break;
@@ -58,10 +63,13 @@ static bool do_control(struct fsm *fsm, int P)
        action = action > 40 ? 100 : 0;
 #else
        action = action > 0 ? action : -action;
-#endif
-       engine_A_pwm(action);
-       engine_A_en(ENGINE_EN_ON);
-
+#endif 
+       if((IO0PIN & (1<<END_SWITCH_UP))&&(IO0PIN & (1<<END_SWITCH_DOWN))){
+         engine_A_pwm(action);
+         engine_A_en(ENGINE_EN_ON);
+       }else {
+         stop();
+       }
 
        if (fsm->req_target > fsm->start_pos)
                finished = fsm->act_pos > fsm->req_target - DEAD_ZONE;

diff --git a/src/eb_lift_11/main.c b/src/eb_lift_11/main.c
index e565fd6222bce5df7485d519ab0f4f6ebba76e52..afd130d7c4587266c2856c79afb36614d8334e55 100644 (file)
--- a/src/eb_lift_11/main.c
+++ b/src/eb_lift_11/main.c
@@ -43,13 +43,49 @@
 
 
 struct fsm fsm_lift;
+static int last_irc;
 
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+#define IRC_A  2       //pin 1, exp. port on board
+#define IRC_B  3       //pin 2, exp. port on board
 
+#define IRC_A_MASK     (1<<IRC_A)
+#define IRC_B_MASK     (1<<IRC_B)
+#define IRC_AB_MASK    ((1<<IRC_A)&(1<<IRC_B))
 
+int irc_read_tick(int last_irc);
+void irc_init(void);
 
+void irc_init(void){
+  SET_PIN(PINSEL0, IRC_A, PINSEL_0);           
+  SET_PIN(PINSEL0, IRC_B, PINSEL_0);   
+  
+  SET_PIN(PINSEL0, END_SWITCH_UP, PINSEL_0);           
+  SET_PIN(PINSEL0, END_SWITCH_DOWN, PINSEL_0);
+  
+  last_irc=0;
+}
+
+int irc_read_tick(int last_irc){
+  
+  int irc, temp;
+  
+  irc = IO0PIN & IRC_AB_MASK;
+  if ((irc & IRC_B_MASK) != 0)
+    irc ^= IRC_A_MASK;
+
+  temp = (irc - last_irc) & IRC_AB_MASK;
+  last_irc= irc; 
+  if (temp == IRC_A_MASK){ // nebo zaporny (-1)
+    return (1);
+  }
+  else if (temp == IRC_AB_MASK){
+    return (-1);
+  }
+  return 0;
+}
 
 void init_motors(void){
   
@@ -91,6 +127,8 @@ void timer0_irq() {
        /* reset timer irq */
        T0IR = -1;
        
+       fsm_lift.act_pos=fsm_lift.act_pos+irc_read_tick(last_irc);
+       
        /* increment timer_usec */
        timer_usec += 10;
        /* increment msec @1kHz */
@@ -195,7 +233,8 @@ void init_periphery(void){
        set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
 
        init_uart();
-       init_adc(ADC_ISR);
+//     init_adc(ADC_ISR);
+       irc_init();
        
        
 } 
@@ -256,7 +295,7 @@ void blink_led()
 
 
 
-#define COLOR_PIN      3               // change color of dress pin  (SDA1/P0_3)
+#define COLOR_PIN 18           //pin 5, exp. port on board
 
 void color_hadler()
 {
@@ -309,11 +348,11 @@ int main(void)
        init_periphery();       
        
        SET_PIN(PINSEL0, START_PIN, PINSEL_0);          // inicializace start pinu
-       SET_PIN(PINSEL0, COLOR_PIN, PINSEL_0);
+       SET_PIN(PINSEL0, 3, PINSEL_0);  //init of color pin
        
        send_rs_str("Lift started\n");
 
-       fsm_lift.act_pos = adc_val[0];
+       fsm_lift.act_pos = 0;
        init_fsm(&fsm_lift, &fsm_lift_init);
 
 /*     return; */
@@ -325,9 +364,8 @@ int main(void)
 
                        //dbg_print_time();
 
-                       fsm_lift.act_pos = adc_val[0];
-                       
-                   
+//                     fsm_lift.act_pos = adc_val[0];
+                         
                        run_fsm(&fsm_lift);
                }