From da66ed369b23c694ddc06c1795fd21fd63f3f780 Mon Sep 17 00:00:00 2001 From: Matous Pokorny Date: Fri, 27 Apr 2012 17:27:44 +0200 Subject: [PATCH] eb_lift: Add new project that is common lift ccntroler for year 2012 and the future using. --- build/lpceurobot/eb_lift | 1 + src/eb_lift/Makefile | 14 ++ src/eb_lift/Makefile.omk | 8 + src/eb_lift/def.h | 7 + src/eb_lift/fsm.c | 19 ++ src/eb_lift/fsm.h | 39 +++ src/eb_lift/fsm_lift.c | 223 +++++++++++++++++ src/eb_lift/main.c | 462 ++++++++++++++++++++++++++++++++++++ src/eb_lift/uar.c | 82 +++++++ src/eb_lift/uar.h | 13 + src/eb_lift_11/Makefile.omk | 6 +- 11 files changed, 871 insertions(+), 3 deletions(-) create mode 120000 build/lpceurobot/eb_lift create mode 100644 src/eb_lift/Makefile create mode 100644 src/eb_lift/Makefile.omk create mode 100644 src/eb_lift/def.h create mode 100644 src/eb_lift/fsm.c create mode 100644 src/eb_lift/fsm.h create mode 100644 src/eb_lift/fsm_lift.c create mode 100644 src/eb_lift/main.c create mode 100644 src/eb_lift/uar.c create mode 100644 src/eb_lift/uar.h diff --git a/build/lpceurobot/eb_lift b/build/lpceurobot/eb_lift new file mode 120000 index 00000000..9cc6e63e --- /dev/null +++ b/build/lpceurobot/eb_lift @@ -0,0 +1 @@ +/home/ehiker/git_ebot/src/eb_lift \ No newline at end of file diff --git a/src/eb_lift/Makefile b/src/eb_lift/Makefile new file mode 100644 index 00000000..08cf5ff3 --- /dev/null +++ b/src/eb_lift/Makefile @@ -0,0 +1,14 @@ +# Generic directory or leaf node makefile for OCERA make framework + +ifndef MAKERULES_DIR +MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) ) +endif + +ifeq ($(MAKERULES_DIR),) +all : default +.DEFAULT:: + @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n" +else +include $(MAKERULES_DIR)/Makefile.rules +endif + diff --git a/src/eb_lift/Makefile.omk b/src/eb_lift/Makefile.omk new file mode 100644 index 00000000..c6d13ce9 --- /dev/null +++ b/src/eb_lift/Makefile.omk @@ -0,0 +1,8 @@ +# -*- makefile -*- + +bin_PROGRAMS = eb_lift + +eb_lift_SOURCES = main.c fsm.c fsm_lift.c uar.c +eb_lift_LIBS = can ebb + +link_VARIANTS = flash diff --git a/src/eb_lift/def.h b/src/eb_lift/def.h new file mode 100644 index 00000000..9bd2d67f --- /dev/null +++ b/src/eb_lift/def.h @@ -0,0 +1,7 @@ +#ifndef DEFH +#define DEFH + +extern volatile uint32_t timer_msec; +extern volatile uint32_t timer_usec; /* incremented by 10 @100kHz */ + +#endif diff --git a/src/eb_lift/fsm.c b/src/eb_lift/fsm.c new file mode 100644 index 00000000..4a306a51 --- /dev/null +++ b/src/eb_lift/fsm.c @@ -0,0 +1,19 @@ +#include "fsm.h" + +void init_fsm(struct fsm *fsm, state_fcn initial_state) +{ + fsm->current_state = initial_state; + fsm->current_state(fsm, EVENT_ENTRY); +} + +void run_fsm(struct fsm *fsm){ + fsm->last_state = fsm->current_state; // set actual state + fsm->current_state(fsm, EVENT_DO); // change parameter + + if(fsm->last_state != fsm->current_state){ // if state was changed + fsm->last_state(fsm, EVENT_EXIT); // finish the old state + fsm->current_state(fsm, EVENT_ENTRY); // initialize the new state + } +} + + diff --git a/src/eb_lift/fsm.h b/src/eb_lift/fsm.h new file mode 100644 index 00000000..eb9e0b71 --- /dev/null +++ b/src/eb_lift/fsm.h @@ -0,0 +1,39 @@ +#ifndef FSM_H +#define FSM_H + +#include +#include + +// events of each state of state machine +enum event { + EVENT_ENTRY, + EVENT_DO, + EVENT_EXIT +}; + +struct fsm; + +typedef void (*state_fcn)(struct fsm *fsm, enum event my_event);//pointer to function returning void and two input parametr + +struct fsm { + state_fcn current_state; // current state + state_fcn last_state; // last state + int32_t act_pos; // actual position + int32_t req_pos; // requested position + int32_t req_spd; + int32_t req_target; + volatile int32_t can_req_spd; + volatile uint32_t can_req_position; // next requested position + int32_t start_pos; + uint32_t can_response; // when the move is done, the value here equals to the req_pos + uint8_t flags; //< CAN flags bits (defined in can_msg_def.h) + uint32_t time_start; /* For timeout detection */ + bool trigger_can_send; + uint8_t can_req_homing; +}; + + +void init_fsm(struct fsm *fsm, state_fcn initial_state); +void run_fsm(struct fsm *fsm); + +#endif diff --git a/src/eb_lift/fsm_lift.c b/src/eb_lift/fsm_lift.c new file mode 100644 index 00000000..3561a7ac --- /dev/null +++ b/src/eb_lift/fsm_lift.c @@ -0,0 +1,223 @@ +#include +#include +#include +#include +#include +#include "uar.h" +#include "fsm.h" +#include +#include +#include "def.h" +#include + +#define DBG_ENTRY() do { \ + send_rs_str(__func__); \ + send_rs_str(": entry\n"); \ + } while(0); + +static void wait_for_cmd(struct fsm *fsm, enum event event); +static void move(struct fsm *fsm, enum event event); +static void homing(struct fsm *fsm, enum event event); +static void stop(void); + + +static void homing(struct fsm *fsm, enum event event) +{ + static uint32_t time_start = 0; + + switch (event) { + case EVENT_ENTRY: + DBG_ENTRY(); + time_start = timer_msec; + break; + case EVENT_DO: + //homing with 8s timeout + engine_A_en(ENGINE_EN_ON); + engine_A_dir(ENGINE_DIR_FW); + engine_A_pwm(50); + + if(fsm->flags & CAN_LIFT_SWITCH_DOWN){ + engine_A_pwm(0); + fsm->act_pos = 0; + fsm->current_state = wait_for_cmd; + fsm->flags |= CAN_LIFT_HOMED; + fsm->trigger_can_send = 1; + fsm->can_req_homing = 0; + fsm->can_req_position = 0; + // fsm->can_req_position = 0x54E2; + } else if (timer_msec >= (time_start + 4000)) { + stop(); + fsm->current_state = wait_for_cmd; + fsm->flags |= CAN_LIFT_TIMEOUT; + fsm->flags &= ~CAN_LIFT_HOMED; + fsm->trigger_can_send = 1; + fsm->can_req_homing = 0; + } + break; + case EVENT_EXIT: + break; + } +} + +void fsm_lift_init(struct fsm *fsm, enum event event) +{ + switch (event) { + case EVENT_ENTRY: + DBG_ENTRY(); + fsm->flags |= CAN_LIFT_INITIALIZING; + break; + case EVENT_DO: + fsm->flags &= ~CAN_LIFT_INITIALIZING; + fsm->current_state = wait_for_cmd; + break; + case EVENT_EXIT: + break; + } +} + +static void stop() +{ + engine_A_pwm(0); + engine_A_en(ENGINE_EN_OFF); +} + + +#define DEAD_ZONE 10 +static bool do_control(struct fsm *fsm, int P) +{ + bool finished; + int e = fsm->req_pos - fsm->act_pos; + int action = (P*e) / 5; // ORIGINAL: int action = P*e; + + engine_A_dir(action > 0); + action = action > 100 ? 100 : action; + +//#define BANG_BANG +#ifdef BANG_BANG + action = action > 0 ? action : -action; + action = action > 40 ? 100 : 0; +#else + action = action > 0 ? action : -action; +#endif + + engine_A_pwm(action); + engine_A_en(ENGINE_EN_ON); + + if (fsm->req_target > fsm->start_pos) + finished = fsm->act_pos > fsm->req_target - DEAD_ZONE; + else + finished = fsm->act_pos < fsm->req_target + DEAD_ZONE; + + return finished; +} + +static void wait_for_cmd(struct fsm *fsm, enum event event) +{ + static int last_can_req_pos = 0; + switch (event) { + case EVENT_ENTRY: + DBG_ENTRY(); + stop(); + break; + case EVENT_DO: + //homing if push home button or homing can msg. received + if ((fsm->flags & CAN_LIFT_SWITCH_HOME) || (fsm->can_req_homing)) { + fsm->can_req_homing = 0; + fsm->current_state = homing; + break; + } + /* if lift is not homed, do not allow movement and regulation */ + if (fsm->flags & CAN_LIFT_HOMED) { + do_control(fsm, 2); + + if (fsm->can_req_position != last_can_req_pos && + fsm->can_req_position != fsm->req_target) { + last_can_req_pos = fsm->can_req_position; + fsm->req_target = fsm->can_req_position; + fsm->req_spd = fsm->can_req_spd; + fsm->current_state = move; + } + } + break; + case EVENT_EXIT: + break; + } +} + +#define XMIN(a,b) ((a) < (b) ? (a) : (b)) +#define XMAX(a,b) ((a) > (b) ? (a) : (b)) +static void move(struct fsm *fsm, enum event event) +{ + static int counter; + static bool lift_stopped_on_end = false; + bool finished; + switch (event) { + case EVENT_ENTRY: + counter = 0; + DBG_ENTRY(); + fsm->time_start = timer_msec; + fsm->start_pos = fsm->act_pos; + + /* check if movement starts on end switch */ + if ((fsm->flags & CAN_LIFT_SWITCH_DOWN) || (fsm->flags & CAN_LIFT_SWITCH_UP)) + lift_stopped_on_end = true; + else + lift_stopped_on_end = false; + + if(fsm->req_spd == 0) + fsm->req_pos = fsm->req_target; + else + fsm->req_pos = fsm->start_pos; + break; + case EVENT_DO: + /* if movement starts on end switch, ignore this, else stop movement on act position */ + if ((fsm->flags & CAN_LIFT_SWITCH_UP) && !lift_stopped_on_end){ + fsm->can_response = fsm->req_target; + fsm->current_state = wait_for_cmd; + fsm->req_pos = fsm->act_pos; + lift_stopped_on_end = true; + } + /* if movement starts on end switch, ignore this, else stop movement on act position */ + if ((fsm->flags & CAN_LIFT_SWITCH_DOWN) && !lift_stopped_on_end) { + fsm->can_response = fsm->req_target; + fsm->current_state = wait_for_cmd; + fsm->act_pos = 0; + fsm->req_pos = fsm->act_pos; + lift_stopped_on_end = true; + } + + if (fsm->can_req_position != fsm->req_target) { + fsm->flags |= CAN_LIFT_TIMEOUT; + fsm->current_state = wait_for_cmd; + } + + if(fsm->req_spd != 0 && counter++ >= 10) + { + counter = 0; + if(fsm->req_target > fsm->start_pos) { + fsm->req_pos = XMIN(fsm->req_pos + fsm->req_spd,fsm->req_target); + } else { + fsm->req_pos = XMAX(fsm->req_pos - fsm->req_spd,fsm->req_target); + } + } + + if (timer_msec - fsm->time_start > (fsm->req_spd == 0 ? 2000 : 4000)) { + fsm->flags |= CAN_LIFT_TIMEOUT; + fsm->can_response = fsm->req_target; + fsm->current_state = wait_for_cmd; + fsm->req_pos = fsm->act_pos; + } + + finished = do_control(fsm, fsm->req_spd ? 2 : 2); + + if (finished && fsm->req_pos == fsm->req_target) { + fsm->can_response = fsm->req_target; + fsm->current_state = wait_for_cmd; + } + break; + case EVENT_EXIT: + stop(); + fsm->trigger_can_send = true;; + break; + } +} diff --git a/src/eb_lift/main.c b/src/eb_lift/main.c new file mode 100644 index 00000000..3b696c01 --- /dev/null +++ b/src/eb_lift/main.c @@ -0,0 +1,462 @@ + +/** + * @file main.c + * + * + * @author Bc. Jiri Kubias, jiri.kubias@gmail.com + * @author Michal Sojka + * + * @addtogroup fork + */ + + +/** + * @defgroup fork Vidle (fork) application + */ +/** + * @ingroup fork + * @{ + */ + + +#include /* LPC21xx definitions */ +#include +#include +#include +#include +#include +#include +#include +#include "engine.h" +#include "uar.h" +#include +#include "fsm.h" +#include "def.h" +#include + +#define CAN_SPEED 1000000 //< CAN bus speed +#define CAN_ISR 0 + +#define ADC_ISR 1 + +#define TIMER_IRQ_PRIORITY 5 + + +struct fsm fsm_lift; + +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +#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 SWITCH_STRATEGY_PIN 17 //pin 8, 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<= 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_up >= 100) { + cnt_up = 0; + cnt_down = 0; + deb_led_change(LEDB); + return ++position; + } + } + return position; +} + +void init_motors(void){ + + init_engine_A(); // initialization of PWM unit + engine_A_en(ENGINE_EN_ON); //enable motor A + engine_A_dir(ENGINE_DIR_FW); //set direction + engine_A_pwm(0); // STOP pwm is in percent, range 0~100~200 + +} + +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) { + /* set interrupt vector */ + ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler; + ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source; + /* enable interrupt */ + VICIntEnable = 1<= next_send) { + msg.id = CAN_ROBOT_CMD; + msg.flags = 0; + msg.dlc = 1; + msg.data[0] = start_condition; + +// send_rs_str("start\n"); + + /*while*/ (can_tx_msg(&msg)); + + if (count < START_SEND_FAST_COUNT) { + count++; + next_send = timer_msec + START_SEND_PRIOD_FAST; + } else + next_send = timer_msec + START_SEND_PRIOD_SLOW; + } + + +} + + +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +void CAN_rx(can_msg_t *msg) { + uint32_t spd; + can_msg_t rx_msg; + uint32_t req =0; + memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message + + + deb_led_on(LEDB); + + switch (rx_msg.id) + { + case CAN_LIFT_CMD: + deb_led_on(LEDB); + req = ((rx_msg.data[0]<<8) | (rx_msg.data[1])); + spd = rx_msg.data[2]; + fsm_lift.can_req_homing=rx_msg.data[3]; + // range 0 - A9C5 + if (req >= LIFT_IRC_VAL_MIN && req <= LIFT_IRC_VAL_MAX) { + fsm_lift.flags &= ~CAN_LIFT_OUT_OF_BOUNDS; + fsm_lift.can_req_position = req;// save new req position of lift + fsm_lift.can_req_spd = spd;// save new req spd of lift + } else + fsm_lift.flags |= CAN_LIFT_OUT_OF_BOUNDS; + break; + default:break; + } + + deb_led_off(LEDB); +} + + +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +void init_periphery(void){ + + can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus + init_motors(); + + /* init timer0 */ + init_timer0(1, CPU_APB_HZ/100000); + set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq); + + init_uart(); +// init_adc(ADC_ISR); +} +/*********************************************************/ +void can_send_status(void) +{ + can_msg_t tx_msg; + tx_msg.id = CAN_LIFT_STATUS; + tx_msg.dlc = 5; + tx_msg.flags = 0; + tx_msg.data[0] = (fsm_lift.act_pos >> 8) & 0xFF; + tx_msg.data[1] = fsm_lift.act_pos & 0xFF; + tx_msg.data[2] = (fsm_lift.can_response >> 8) & 0xFF; + tx_msg.data[3] = fsm_lift.can_response & 0xFF; + tx_msg.data[4] = fsm_lift.flags; + /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */ +} + +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +void dbg_print_time() +{ + char str[10]; + unsigned t = timer_usec, i; + memset(str, ' ', sizeof(str)); + str[9] = 0; + str[8] = '\n'; + for (i=7; t > 0; i--) { + str[i] = t%10 + '0'; + t /= 10; + } + send_rs_str(str); +} + +void fsm_lift_init(struct fsm *fsm, enum event event); + + +void blink_led() +{ + static uint32_t led_time = 0; + + if(timer_msec >= led_time + 500) + { + led_time = timer_msec; +// static int up; +// if (up == 0) +// fsm_lift.can_req_position = 0x380; +// if (up == 6) +// fsm_lift.can_req_position = 0x1e0; +// up = (up+1)%12; + + deb_led_change(LEDG); + + send_rs_str("ACT_POS\t"); + send_rs_int(fsm_lift.act_pos); + send_rs_str("\t"); + send_rs_str("CAN_FLAGS\t"); + send_rs_int(fsm_lift.flags); + send_rs_str("\n"); + } +} + +void robot_switches_handler() +{ + static uint32_t color_time = 0; + char sw = 0; + + if (timer_msec >= color_time + 100) + { + can_msg_t tx_msg; + + color_time = timer_msec; + + if (IO0PIN & (1<= main_time + 1000) + { + main_time = timer_usec; + + //dbg_print_time(); + +// fsm_lift.act_pos = adc_val[0]; + + run_fsm(&fsm_lift); + } + + if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms + fsm_lift.trigger_can_send) { //or when something important happen + fsm_lift.trigger_can_send = false; + status_time = timer_msec; //save new time, when message was sent + can_send_status(); + } + + fsm_lift.act_pos = irc_read_tick(); + + start_button(); + robot_switches_handler(); + lift_switches_handler(); + blink_led(); + } +} + +/** @} */ diff --git a/src/eb_lift/uar.c b/src/eb_lift/uar.c new file mode 100644 index 00000000..f30a0223 --- /dev/null +++ b/src/eb_lift/uar.c @@ -0,0 +1,82 @@ +#include + + + + +/** + * Send one char to uart. + */ +void uart_send_char(char val) +{ + uart0SendCh(val); +} + +/** + * Read one char from uart. + */ +char uart_get_char(void) +{ + return uart0GetCh(); +} + +/** + * Initialize UART - platform dependent + */ +void init_uart(void) +{ + init_uart0((int)9600, UART_BITS_8, UART_STOP_BIT_1, UART_PARIT_OFF, 0 ); +} + + +/** + * Send string to serial output in ASCII code. . + * @param data[] string to print + */ +void send_rs_str(const char data[]) +{ + + int i = 0; + int j = 0; + + for (j = 0; j < 255; j++) + { + if(data[j] == 0) break; + } + + for (i= 0 ; i < j; i++) + { + uart_send_char(data[i]); + } +} + +/** + * Send int value to serial output in ASCII code. Removes unused zeros. + * @param val value to print + */ +void send_rs_int(int val) +{ + char dat[8]; + int i; + int pom = 0; + + for(i = 0; i < 8; i++) + { + dat[i] = (val & 0xF) + 0x30; + if(dat[i] > '9') + dat[i] += 7; + val >>= 4; + } + + for(i = 0; i < 8; i++) + { + if((pom == 0) & (dat[7-i] == '0')) + { + if((i == 6) | (i == 7)) + uart_send_char('0'); + continue; + } + pom = 1; + uart_send_char(dat[7-i]); + } + +} diff --git a/src/eb_lift/uar.h b/src/eb_lift/uar.h new file mode 100644 index 00000000..3f9f41c1 --- /dev/null +++ b/src/eb_lift/uar.h @@ -0,0 +1,13 @@ +#ifndef _UAR_H +#define _UAR_H + + +void send_rs_int(int val); +void send_rs_str(const char data[]); +void init_uart(void); +char uart_get_char(void); +void uart_send_char(char val); + + +#endif + diff --git a/src/eb_lift_11/Makefile.omk b/src/eb_lift_11/Makefile.omk index c6d13ce9..40af9134 100644 --- a/src/eb_lift_11/Makefile.omk +++ b/src/eb_lift_11/Makefile.omk @@ -1,8 +1,8 @@ # -*- makefile -*- -bin_PROGRAMS = eb_lift +bin_PROGRAMS = eb_lift_11 -eb_lift_SOURCES = main.c fsm.c fsm_lift.c uar.c -eb_lift_LIBS = can ebb +eb_lift_11_SOURCES = main.c fsm.c fsm_lift.c uar.c +eb_lift_11_LIBS = can ebb link_VARIANTS = flash -- 2.39.2