From: Vojtech Date: Thu, 10 Nov 2011 11:40:56 +0000 (+0100) Subject: eb_test: add eb_test dir X-Git-Tag: start2012^0 X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/da9ea39fd0d4e902a16de492407fd6c3348239f9 eb_test: add eb_test dir --- diff --git a/build/lpceurobot/eb_test b/build/lpceurobot/eb_test new file mode 120000 index 00000000..7a83ebe1 --- /dev/null +++ b/build/lpceurobot/eb_test @@ -0,0 +1 @@ +../../src/eb_test \ No newline at end of file diff --git a/src/eb_test/Makefile b/src/eb_test/Makefile new file mode 100644 index 00000000..08cf5ff3 --- /dev/null +++ b/src/eb_test/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_test/Makefile.omk b/src/eb_test/Makefile.omk new file mode 100644 index 00000000..bf5cba43 --- /dev/null +++ b/src/eb_test/Makefile.omk @@ -0,0 +1,8 @@ +# -*- makefile -*- + +bin_PROGRAMS = eb_test + +eb_test_SOURCES = main.c uar.c +eb_test_LIBS = can ebb + +link_VARIANTS = flash diff --git a/src/eb_test/def.h b/src/eb_test/def.h new file mode 100644 index 00000000..9bd2d67f --- /dev/null +++ b/src/eb_test/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_test/fsm.c b/src/eb_test/fsm.c new file mode 100644 index 00000000..4a306a51 --- /dev/null +++ b/src/eb_test/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_test/fsm.h b/src/eb_test/fsm.h new file mode 100644 index 00000000..56e59de0 --- /dev/null +++ b/src/eb_test/fsm.h @@ -0,0 +1,37 @@ +#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; +}; + +void init_fsm(struct fsm *fsm, state_fcn initial_state); +void run_fsm(struct fsm *fsm); + +#endif diff --git a/src/eb_test/fsm_vidle.c b/src/eb_test/fsm_vidle.c new file mode 100644 index 00000000..c42a8220 --- /dev/null +++ b/src/eb_test/fsm_vidle.c @@ -0,0 +1,145 @@ +#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); + + +void fsm_vidle_init(struct fsm *fsm, enum event event) +{ + switch (event) { + case EVENT_ENTRY: + DBG_ENTRY(); + fsm->can_req_position = fsm->act_pos; +// fsm->flags |= CAN_VIDLE_INITIALIZING; + break; + case EVENT_DO: + /* TODO: Homing */ +// fsm->flags &= ~CAN_VIDLE_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) / 10; // ORIGINAL: int action = P*e; + + engine_A_dir(action < 0); +//#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: + 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; + bool finished; + switch (event) { + case EVENT_ENTRY: + counter = 0; + DBG_ENTRY(); + fsm->time_start = timer_msec; + fsm->start_pos = fsm->act_pos; + if(fsm->req_spd == 0) + fsm->req_pos = fsm->req_target; + else + fsm->req_pos = fsm->start_pos; + break; + case EVENT_DO: + if (fsm->can_req_position != fsm->req_target) { + fsm->flags |= CAN_VIDLE_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 ? 1000 : 3000)) { + fsm->flags |= CAN_VIDLE_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_test/main.c b/src/eb_test/main.c new file mode 100644 index 00000000..09fdc349 --- /dev/null +++ b/src/eb_test/main.c @@ -0,0 +1,430 @@ + +/** + * @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 +#include "vhn.h" + +#define CAN_SPEED 1000000 //< CAN bus speed +#define CAN_ISR 0 + +#define ADC_ISR 1 + +#define TIMER_IRQ_PRIORITY 5 + + +struct fsm fsm_vidle; + +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + + + + +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 +/* vhn_init(); */ +} + +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +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; + /*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; + } + + +} + +#endif +#if 0 +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +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_VIDLE_CMD: + deb_led_on(LEDB); + req = ((rx_msg.data[0]<<8) | (rx_msg.data[1])); + spd = rx_msg.data[2]; + + if (req >= 0x150 && req <= 0x3e0) { + fsm_vidle.flags &= ~CAN_VIDLE_OUT_OF_BOUNDS; + fsm_vidle.can_req_position = req;// save new req position of lift + fsm_vidle.can_req_spd = spd;// save new req spd of lift + } else + fsm_vidle.flags |= CAN_VIDLE_OUT_OF_BOUNDS; + break; + default:break; + } + + deb_led_off(LEDB); +} + +#endif +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +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); + + +} +/*********************************************************/ +#if 0 +void can_send_status(void) +{ + can_msg_t tx_msg; + tx_msg.id = CAN_VIDLE_STATUS; + tx_msg.dlc = 5; + tx_msg.flags = 0; + tx_msg.data[0] = (fsm_vidle.act_pos >> 8) & 0xFF; + tx_msg.data[1] = fsm_vidle.act_pos & 0xFF; + tx_msg.data[2] = (fsm_vidle.can_response >> 8) & 0xFF; + tx_msg.data[3] = fsm_vidle.can_response & 0xFF; + tx_msg.data[4] = fsm_vidle.flags; + /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */ +} +#endif +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +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_vidle_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_vidle.can_req_position = 0x380; + if (up == 6) + fsm_vidle.can_req_position = 0x1e0; + up = (up+1)%12; + */ + deb_led_change(LEDG); + send_rs_int(fsm_vidle.act_pos); + send_rs_str("\n"); + } +} + + +#define BUMPER_PIN 17 // bumper pin (SCK1/P0_17) +#define COLOR_PIN 3 // change color of dress pin (SDA1/P0_3) + +#define BUMPER_LEFT 19 // left bumper MOSI1/P0_19 +#define BUMPER_RIGHT 9 // right bumper RXD1/P0_9 + +#if 0 +void handle_bumper() +{ + static uint32_t bumper_time = 0; + char sw = 0; + + if (timer_msec >= bumper_time + 100) + { + can_msg_t tx_msg; + + bumper_time = timer_msec; + + + + if (IO0PIN & (1<= main_time + 1000) + { + main_time = timer_usec; + + //dbg_print_time(); + + // fsm_vidle.act_pos = adc_val[0]; + + + // run_fsm(&fsm_vidle); + } + + if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms + fsm_vidle.trigger_can_send) { //or when something important happen + fsm_vidle.trigger_can_send = false; + status_time = timer_msec; //save new time, when message was sent + // can_send_status(); + + } + + //start_button(); + //handle_bumper(); + + + delta=i; + i=adc_val[0]; + delta=i-delta; + suma+=delta; + engine_A_pwm(20); + if (j++>10000) { + if (abs(suma)<1) {obrat=!obrat;} + send_rs_int(suma); + send_rs_str("\n"); + j=0; + suma=0; + + } + if (obrat) + { + if (i>750) + engine_A_dir(ENGINE_DIR_BW); + else {engine_A_dir(ENGINE_DIR_FW); + obrat=false; + suma=20; + } + } + else + { + if(i<950) + engine_A_dir(ENGINE_DIR_FW); + else {engine_A_dir(ENGINE_DIR_BW); + obrat=true; + suma=20; + } + + } + + + + + + } +} + +/** @} */ diff --git a/src/eb_test/uar.c b/src/eb_test/uar.c new file mode 100644 index 00000000..f30a0223 --- /dev/null +++ b/src/eb_test/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_test/uar.h b/src/eb_test/uar.h new file mode 100644 index 00000000..3f9f41c1 --- /dev/null +++ b/src/eb_test/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 +