From 9dfc885e5e8cfb8634099f852e0dae9adf1c2056 Mon Sep 17 00:00:00 2001
From: pokormat <matous.pokorny@me.com>
Date: Fri, 22 Apr 2011 23:54:02 +0200
Subject: [PATCH] eb_lift_11: add irc and end switches, homing

---
 src/eb_lift_11/fsm.h      |  3 +++
 src/eb_lift_11/fsm_lift.c | 18 ++++++++++----
 src/eb_lift_11/main.c     | 52 +++++++++++++++++++++++++++++++++------
 3 files changed, 61 insertions(+), 12 deletions(-)

diff --git a/src/eb_lift_11/fsm.h b/src/eb_lift_11/fsm.h
index 56e59de0..4dc921f9 100644
--- 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 cc20998a..afcc91ab 100644
--- 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 e565fd62..afd130d7 100644
--- 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);
 		}
 
-- 
2.39.2