eb_lift_11: Better version of IRC counter, reverse switch funtion.

diff --git a/src/eb_lift_11/fsm_lift.c b/src/eb_lift_11/fsm_lift.c
index 360f21b1dc5c007c908486864420146c279f26c4..3561a7acc8831fa40b0cdfc1c6ba922ee319d342 100644 (file)
--- a/src/eb_lift_11/fsm_lift.c
+++ b/src/eb_lift_11/fsm_lift.c
@@ -37,7 +37,7 @@ static void homing(struct fsm *fsm, enum event event)
                engine_A_pwm(50);
 
                if(fsm->flags & CAN_LIFT_SWITCH_DOWN){
-                       stop();
+                       engine_A_pwm(0);
                        fsm->act_pos = 0;
                        fsm->current_state = wait_for_cmd;
                        fsm->flags |= CAN_LIFT_HOMED;

diff --git a/src/eb_lift_11/main.c b/src/eb_lift_11/main.c
index 4175533d72dbf923bc8b406f5d935d216eb13528..5fef3acf926f1016b6523dc105bd2100ee1bf576 100644 (file)
--- a/src/eb_lift_11/main.c
+++ b/src/eb_lift_11/main.c
@@ -48,64 +48,72 @@ struct fsm fsm_lift;
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
-#define END_SWITCH_UP   9       //pin 4, exp. port on board 
-#define END_SWITCH_DOWN 8       //pin 3, exp. port on board
-#define SWITCH_HOME 19       //pin 6, exp. port on board
+#define END_SWITCH_UP_PIN      9       //pin 4, exp. port on board
+#define END_SWITCH_DOWN_PIN    8       //pin 3, exp. port on board
+#define SWITCH_HOME_PIN                19      //pin 6, exp. port on board
+#define START_PIN              15      //pin 7, exp. port on board
+#define COLOR_PIN              18      //pin 5, exp. port on board
+
+#define START_SEND_PRIOD_FAST  50      /* [miliseconds] */
+#define START_SEND_PRIOD_SLOW  300     /* [miliseconds] */
+#define START_SEND_FAST_COUNT  10      /* How many times to send start with small period (after a change) */
+
+#define LIFT_IRC_VAL_MAX 0x19C
+#define LIFT_IRC_VAL_MIN 0x0
+
+#define IRC_A_PIN  2   //pin 1, exp. port on board
+#define IRC_B_PIN  3   //pin 2, exp. port on board
+
+#define IRC_A_MASK     0x04 //(1<<IRC_A)
+#define IRC_B_MASK     0x08 //(1<<IRC_B)
+#define IRC_AB_MASK    0x0C //((1<<IRC_A)&(1<<IRC_B))
 
 void lift_switches_handler(void);
 
 void lift_switches_handler()
 {
-       if (IO0PIN & (1<<END_SWITCH_UP)){
-               fsm_lift.flags |= CAN_LIFT_SWITCH_UP;
-               deb_led_on(LEDR);
-       } else {
+       if (IO0PIN & (1<<END_SWITCH_UP_PIN)){
                fsm_lift.flags &= ~CAN_LIFT_SWITCH_UP;
                deb_led_off(LEDR);
+       } else {
+               fsm_lift.flags |= CAN_LIFT_SWITCH_UP;
+               deb_led_on(LEDR);
        }
 
-       if (IO0PIN & (1<<END_SWITCH_DOWN)){
-               fsm_lift.flags |= CAN_LIFT_SWITCH_DOWN;
-               deb_led_on(LEDR);
-       } else {
+       if (IO0PIN & (1<<END_SWITCH_DOWN_PIN)){
                fsm_lift.flags &= ~CAN_LIFT_SWITCH_DOWN;
                deb_led_off(LEDR);
+       } else {
+               fsm_lift.flags |= CAN_LIFT_SWITCH_DOWN;
+               deb_led_on(LEDR);
        }
 
-       if (IO0PIN & (1<<SWITCH_HOME)){
-               fsm_lift.flags |= CAN_LIFT_SWITCH_HOME;
-               deb_led_on(LEDR);
-       } else {
+       if (IO0PIN & (1<<SWITCH_HOME_PIN)){
                fsm_lift.flags &= ~CAN_LIFT_SWITCH_HOME;
                deb_led_off(LEDR);
+       } else {
+               fsm_lift.flags |= CAN_LIFT_SWITCH_HOME;
+               deb_led_on(LEDR);
        }
-}
-
-#define LIFT_IRC_VAL_MAX 0x19C
-#define LIFT_IRC_VAL_MIN 0x0
-
-#define IRC_A  2       //pin 1, exp. port on board
-#define IRC_B  3       //pin 2, exp. port on board
-
-#define IRC_A_MASK     0x04 //(1<<IRC_A)
-#define IRC_B_MASK     0x08 //(1<<IRC_B)
-#define IRC_AB_MASK    0x0C //((1<<IRC_A)&(1<<IRC_B))
-
-int irc_read_tick(uint16_t *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);
+//     if (IO0PIN & (1<<IRC_A_PIN)){
+//             deb_led_off(LEDR);
+//     } else {
+//             deb_led_on(LEDR);
+//     }
+//     if (IO0PIN & (1<<IRC_B_PIN)){
+//             deb_led_off(LEDR);
+//     } else {
+//             deb_led_on(LEDR);
+//     }
 }
 
 //source code from http://www.vosrk.cz/robotika/Stavba/Enkoder.pdf
-int irc_read_tick(uint16_t *last_irc){
+int32_t irc_read_tick(){
 
-       static char cnt = 0;
+       static uint16_t cnt_up = 0;
+       static uint16_t cnt_down = 0;
+       static uint16_t last_irc = 0;
+       static int32_t position = 0;
        uint16_t irc, temp;
 
        irc = IO0PIN & IRC_AB_MASK;
@@ -113,24 +121,28 @@ int irc_read_tick(uint16_t *last_irc){
        irc ^= IRC_A_MASK;
        }
 
-       temp = (irc - *last_irc) & IRC_AB_MASK;
+       temp = (irc - last_irc) & IRC_AB_MASK;
 
-       *last_irc = irc;
+       last_irc = irc;
 
        if (temp == IRC_A_MASK){
                /* count 100times slower - we do not need 250 ticks per milimeter*/
-               if (--cnt < 100) {
-                       cnt = 0;
-                       return -1;
+               if (++cnt_down >= 100) {
+                       cnt_down = 0;
+                       cnt_up = 0;
+                       deb_led_change(LEDB);
+                       return --position;
                }
        } else if (temp == IRC_AB_MASK){
                /* count 100times slower - we do not need 250 ticks per milimeter*/
-               if (++cnt > 100) {
-                       cnt = 0;
-                       return 1;
+               if (++cnt_up >= 100) {
+                       cnt_up = 0;
+                       cnt_down = 0;
+                       deb_led_change(LEDB);
+                       return ++position;
                }
        }
-       return 0;
+       return position;
 }
 
 void init_motors(void){
@@ -187,13 +199,6 @@ void timer0_irq() {
 
 
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-
-#define START_PIN      15              //pin 7, exp. port on board
-#define START_SEND_PRIOD_FAST  50      /* [miliseconds] */
-#define START_SEND_PRIOD_SLOW  300     /* [miliseconds] */
-#define START_SEND_FAST_COUNT  10      /* How many times to send start with small period (after a change) */
-
-
 void start_button(void)
 {
        can_msg_t msg;
@@ -281,9 +286,6 @@ void init_periphery(void){
 
        init_uart();
 //     init_adc(ADC_ISR);
-       irc_init();
-       
-       
 } 
 /*********************************************************/
 void can_send_status(void)
@@ -345,10 +347,6 @@ void blink_led()
        }
 }
 
-
-
-#define COLOR_PIN 18           //pin 5, exp. port on board
-
 void color_handler()
 {
        static uint32_t color_time = 0;
@@ -395,16 +393,23 @@ int main(void)
 {
        uint32_t main_time = timer_usec;
        uint32_t status_time = timer_usec;
-       
-        static uint16_t last_irc=0;
-        
+
        //lift motor is motor A, MOTA connctor on board
-       init_periphery();       
-       
-       SET_PIN(PINSEL0, START_PIN, PINSEL_0);          //init of start pin
-       SET_PIN(PINSEL0, 3, PINSEL_0);  //init of color pin
-        SET_PIN(PINSEL0, 4, PINSEL_0);  //init of home pin
-       
+       init_periphery();
+
+       SET_PIN(PINSEL0, IRC_A_PIN, PINSEL_0);
+       SET_PIN(PINSEL0, IRC_B_PIN, PINSEL_0);
+
+       SET_PIN(PINSEL0, END_SWITCH_UP_PIN, PINSEL_0);
+       SET_PIN(PINSEL0, END_SWITCH_DOWN_PIN, PINSEL_0);
+
+       SET_PIN(PINSEL0, START_PIN, PINSEL_0);  //init of start pin
+       SET_PIN(PINSEL1, (COLOR_PIN - 16), PINSEL_0);   //init of color pin
+        SET_PIN(PINSEL1, (SWITCH_HOME_PIN - 16), PINSEL_0);  //init of home pin
+
+       IO0DIR &= ~((1<<START_PIN) | (1<<SWITCH_HOME_PIN) | (1 << COLOR_PIN));
+       IO0DIR &= ~((1<<END_SWITCH_UP_PIN) | (1<<END_SWITCH_DOWN_PIN));
+       IO0DIR &= ~((1<<IRC_A_PIN) | (1<<IRC_B_PIN));
        send_rs_str("Lift started\n");
 
        fsm_lift.act_pos = 0;
@@ -431,7 +436,7 @@ int main(void)
                        can_send_status();
                }
 
-               fsm_lift.act_pos = fsm_lift.act_pos + irc_read_tick(&last_irc);
+               fsm_lift.act_pos = irc_read_tick();
 
                start_button();
                color_handler();