-// CAN_BUMPER 1st BYTE (bite oriented)
-#define LEFT_BUMPER 1
-#define RIGHT_BUMPER 2
-
-#define LIFT_LIFT_INIT 0x01 //< Lift is initializing
-#define LIFT_LIFT_ENDSW_UP 0x02 //< End switch during normal movement
-#define LIFT_LIFT_ENDSW_DOWN 0x04 //< End switch during normal movement
-#define LIFT_LIFT_ERR_POS 0x08 //< Lift position cannot be reached
-#define LIFT_PUSHER_INIT 0x10 //< Pusher is initializing
-#define LIFT_PUSHER_ENDSW_FW 0x20 //< End switch during normal movement
-#define LIFT_PUSHER_ENDSW_BK 0x40 //< End switch during normal movement
-#define LIFT_PUSHER_ERR_POS 0x80 //< Pusher position cannot be reached
-#define LIFT_ENDSW_MASK (LIFT_LIFT_ENDSW_UP|LIFT_LIFT_ENDSW_DOWN|LIFT_PUSHER_ENDSW_FW|LIFT_PUSHER_ENDSW_BK)
+/* Flags sent in CAN_VIDLE_STATUS message */
+#define CAN_VIDLE_INITIALIZING 0x01
+#define CAN_VIDLE_TIMEOUT 0x02
+#define CAN_VIDLE_ERROR 0x80
bin_PROGRAMS = eb_vidle
-eb_vidle_SOURCES = main.c lift.c uar.c irc.c vhn.c
+eb_vidle_SOURCES = main.c fsm.c fsm_vidle.c uar.c
eb_vidle_LIBS = can ebb
link_VARIANTS = flash
#ifndef DEFH
#define DEFH
-#include <stdbool.h>
-
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- //~~~~~~~~~~ definition of interrupt priority ~~~~~~~~~~~
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- #define SERVO_PRIORITY 7 // 0 is the biggest priority
- #define IRC_PRIORITY 5 // higher priority than servo
- #define CAN_ISR 0
-
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- //~~~~~~~ definition of macros for bit operations ~~~~~~~
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- #define sbit(WORD,BIT) (WORD|=(1<<BIT)) //set bit
- #define cbit(WORD,BIT) (WORD&=~(1<<BIT)) //clear bit
- #define tbit(WORD,BIT) (WORD&(1<<BIT)) //test bit
-
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- //~~~~~~~~~ definition of CAN bus cominication ~~~~~~~~~~
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- #define CAN_SPEED 1000000 //< CAN bus speed
-
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- //~~~~~~~~~~~~ definition of some constants ~~~~~~~~~~~~~
- //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-
- #define SERVO_DOOR 0 //< position of servo conector
- #define CHELAE_R 1 //< position of right chelae
-
- #define SERVO_DOOR_MAX 0xA0 //< maximum HW servo value of DOOR
- #define SERVO_DOOR_MIN 0x00 //< minimum HW servo value of DOOR
- #define CHELAE_R_MAX 0xE0 //< maximum HW servo value of RIGHT CHELAE
- #define CHELAE_R_MIN 0x30 //< minimum HW servo value of RIGHT CHELAE
-
-
-
- typedef enum {//mark which unit we want controlled
- FSM_LIFT,
- FSM_PUSHER,
- }fsm_type;
-
- typedef enum {// events of each state of state machine
- EVENT_ENTRY,
- EVENT_DO,
- EVENT_EXIT
- } events;
-
- struct fsm;
-
- typedef void (*state_fcn)(struct fsm *fsm, events my_event);//pointer to function returning void and two input parametr
- typedef void (*move_eng)(uint8_t direct,uint8_t velocity);//pointer to function returning void and two input parametr
-
- /// fsm structure for pusher and lift
- 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 last_irc; // last IRC position
- volatile uint32_t can_req_position; // next requested position
- volatile int32_t * irc; // pointer to IRC value
- uint32_t timeout_glob; // global timeout
- uint32_t timeout_switch; // switch inactive timeout
- uint8_t type; // fsm typedef
- uint8_t motor_dir; // motor direction
- move_eng set_engine; // engine set function
- uint32_t lenght; // total length fs lift/pusher
- uint32_t sw_pin; // engine sensor
- uint16_t ans_can; // CAN position answer
- uint8_t speed_home; // homing speed
- uint8_t speed_normal; // normal speed
- uint8_t speed_slow; // normal speed
- bool switch_front; // up or front end switch
- bool switch_back; // down or back end switch
- uint8_t init_flag; // init status flag
- uint32_t p; ///< PI controller constants * 1000
- uint32_t last_move_time; ///< Last timer_msec when act_pos changed
- bool enable_reg; ///< enable regulator
- };
-
- extern volatile uint32_t timer_msec;
- extern volatile uint32_t timer_usec; /* incremented by 10 @100kHz */
-
- void set_holder(unsigned char range);
-
-#define LIFT_LIFT_PROTECT_PUSHER_TOP 0x03C0 //< maximum Lift height where we must protect pusher
-#define LIFT_PUSHER_PROTECT_TOP 0x0C5 //< maximum pusher extrusion at protected area
-
- volatile uint8_t can_flags; //< can flags bit
-
- void can_send_status(void);
-
+extern volatile uint32_t timer_msec;
+extern volatile uint32_t timer_usec; /* incremented by 10 @100kHz */
#endif
--- /dev/null
+#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
+ }
+}
+
+
--- /dev/null
+#ifndef FSM_H
+#define FSM_H
+
+#include <types.h>
+#include <stdbool.h>
+
+// 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
+ volatile uint32_t can_req_position; // next requested position
+ 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
--- /dev/null
+#include <lpc21xx.h>
+#include <deb_led.h>
+#include <system_def.h>
+#include <string.h>
+#include <can_msg_def.h>
+#include "uar.h"
+#include "fsm.h"
+#include <engine.h>
+#include <stdbool.h>
+#include "def.h"
+
+#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->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);
+}
+
+static bool do_control(struct fsm *fsm)
+{
+ int e = fsm->req_pos - fsm->act_pos;
+ int P = 1;
+ int action = P*e;
+
+
+ engine_A_dir(action < 0);
+ engine_A_pwm(action > 0 ? action : -action);
+ engine_A_en(ENGINE_EN_ON);
+
+ return false; /* TODO: Determine end of move */
+}
+
+static void wait_for_cmd(struct fsm *fsm, enum event event)
+{
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ stop();
+ break;
+ case EVENT_DO:
+ if (fsm->can_req_position != fsm->req_pos) {
+ fsm->req_pos = fsm->can_req_position;
+ fsm->current_state = move;
+ }
+ break;
+ case EVENT_EXIT:
+ break;
+ }
+}
+
+static void move(struct fsm *fsm, enum event event)
+{
+ bool finished;
+ switch (event) {
+ case EVENT_ENTRY:
+ DBG_ENTRY();
+ fsm->time_start = timer_msec;
+ fsm->flags = CAN_VIDLE_TIMEOUT;
+ break;
+ case EVENT_DO:
+ if (timer_msec - fsm->time_start > 1000) {
+ fsm->flags = CAN_VIDLE_TIMEOUT;
+ fsm->current_state = wait_for_cmd;
+ }
+
+ finished = do_control(fsm);
+ if (finished) {
+ fsm->can_response = fsm->req_pos;
+ fsm->current_state = wait_for_cmd;
+ }
+ break;
+ case EVENT_EXIT:
+ stop();
+ fsm->trigger_can_send = true;;
+ break;
+ }
+}
+++ /dev/null
-#include "irc.h"
-
-#define DEGLITCH_MEDIAN_LEN 31 /* max. 31 for uint32_t */
-#define DEGLITCH_PULSE_LEN 00 /*16*/
-
-typedef uint32_t deglitch_buf_t;
-
-typedef struct deglitcher_t {
- deglitch_buf_t z;
- unsigned sum, cnt;
- char out;
-} deglitcher_t;
-
-void irc_deglitch_init(deglitcher_t *dg) {
- dg->z = -1;
- dg->sum = DEGLITCH_MEDIAN_LEN;
- dg->cnt = 0;
- dg->out = 0;
-}
-
-deglitcher_t dg1, dg2;
-int32_t volatile irc1_count = 0, irc2_count = 0;
-
-int irc_deglitch(deglitcher_t *dg, char inp) {
- deglitch_buf_t z = dg->z;
- unsigned sum = dg->sum, cnt = dg->cnt;
- char med, change = 0;
-
- /* median filter */
- z <<= 1;
- if (inp) {
- z |= 1;
- ++sum;
- }
- if (z & (1<<DEGLITCH_MEDIAN_LEN))
- --sum;
- med = (sum > DEGLITCH_MEDIAN_LEN/2);
-
- /* too short pulse supression */
- if (med != dg->out) {
- if ((change = (++cnt > DEGLITCH_PULSE_LEN))) {
- dg->out = med;
- cnt = 0;
- }
- }
- else
- cnt = 0;
-
- dg->z = z; dg->sum = sum; dg->cnt = cnt;
-
- return change;
-}
-
-/**
- * update IRC1, IRC2 deglitchers' state structs,
- * initialize IRC1,2 as GPIO inputs
- */
-void irc_init() {
- irc_deglitch_init(&dg1);
- irc_deglitch_init(&dg2);
- IRC1DIR &= ~(1<<IRC1BIT);
- IRC2DIR &= ~(1<<IRC2BIT);
- IRC1PINSEL &= ~(0x3<<IRC1PINBIT);
- IRC2PINSEL &= ~(0x3<<IRC2PINBIT);
-}
-
-/**
- * -!- to be called @ approx. 100kHz fixed period -!-
- *
- * read IRC1,2 inputs, deglitch signal, update irc*_count variables
- * increments on falling edge
- * @return r; r==0: no change, r&1: IRC1 increment, r&2: IRC2 increment
- */
-int irc_read() {
- char inp;
- int rc = 0;
-
- inp = !(!(IRC1PIN & (1<<IRC1BIT)));
- if (irc_deglitch(&dg1, inp) && (inp == 0)) {
- ++irc1_count;
- rc |= 1;
- }
- inp = !(!(IRC2PIN & (1<<IRC2BIT)));
- if (irc_deglitch(&dg2, inp) && (inp == 0)) {
- ++irc2_count;
- rc |= 2;
- }
-
- return rc;
-}
-
+++ /dev/null
-#include <lpc21xx.h>
-#include <types.h>
-
-#define IRC1PIN IO0PIN
-#define IRC1DIR IO0DIR
-#define IRC1BIT 15
-#define IRC1PINSEL PINSEL0
-#define IRC1PINBIT (2*IRC1BIT)
-
-#define IRC2PIN IO0PIN
-#define IRC2DIR IO0DIR
-#define IRC2BIT 18
-#define IRC2PINSEL PINSEL1
-#define IRC2PINBIT (2*2) // FIXME possible error
-
-/* IRC number counts, incremented on falling edges */
- volatile int32_t irc1_count, irc2_count;
-
-void irc_init();
-int irc_read();
+++ /dev/null
-#include <lpc21xx.h> /* LPC21xx definitions */
-#include <deb_led.h>
-#include <system_def.h>
-#include <can_ids.h>
-#include <periph/can.h>
-#include <string.h>
-#include <deb_led.h>
-#include "servo.h"
-#include "engine.h"
-#include "lift.h"
-#include "uar.h"
-#include "def.h"
-#include "irc.h"
-#include "vhn.h"
-#include <can_msg_def.h>
-
-#define ENG_STOP 0
-#define ENG_LIFT_HOME_SLOW_SPEED 15
-#define ENG_LIFT_HOME_SPEED 40
-#define ENG_LIFT_FULL_SPEED 100
-#define ENG_PUSHER_HOME_SPEED 70
-#define ENG_PUSHER_FULL_SPEED 100
-#define ENG_PUSHER_HOME_SLOW_SPEED 40
-#define TIME_WAIT 300
-#define TIME_HOMING_WAIT 7000
-
-
-
-#define PRINT_STR_VAL(fsm,str, val) print_str_val(fsm, str, val)
-#define PRINT_STR(fsm, str) print_str(fsm, str)
-
-
-static uint8_t fsm_homing_block;
-
-
-void print_str(uint8_t type, uint8_t text[])
-{
- if(type == FSM_LIFT)
- send_rs_str("FSM_LIFT ");
- else
- send_rs_str("FSM_PUSHER ");
-
- send_rs_str(text);
- uart_send_char('\n');
-}
-
-
-void print_str_val(uint8_t type, uint8_t text[], uint32_t val)
-{
- if(type == FSM_LIFT)
- send_rs_str("FSM_LIFT ");
- else
- send_rs_str("FSM_PUSHER ");
-
- send_rs_str(text);
- uart_send_char(' ');
- send_rs_int(val);
- uart_send_char('\n');
-}
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void move_pusher(unsigned char direct,unsigned char velocity){//how many pulzes
- engine_A_dir(direct);//set direction for lift
- engine_A_pwm(velocity); // set velocity, range 0~100~200
- engine_A_en(ENGINE_EN_ON);//enable motor of pusher
-}
-
-void move_lift(uint8_t direct,uint8_t velocity){ //how many pulzes
-// engine_B_dir(direct); //set direction for lift
-// engine_B_pwm(velocity); // set velocity, range 0~100~200
-// engine_B_en(ENGINE_EN_ON); //enable motor of pusher
-
-// vhn_speed(50, 0);
- vhn_speed(velocity , direct);
-// PRINT_STR_VAL(FSM_LIFT,"VHN speed:", velocity);
-// PRINT_STR_VAL(FSM_LIFT,"VHN direct:", direct);
-}
-
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void fsm_irc(struct fsm *fsm)
-{
- int32_t irc = *fsm->irc ;
-
- if(fsm->motor_dir == ENGINE_DIR_FW)
- fsm->act_pos += irc - fsm->last_irc;
- else
- fsm->act_pos -= irc - fsm->last_irc;
-
- if (irc != fsm->last_irc)
- fsm->last_move_time = timer_msec;
-
-
- fsm->last_irc = irc;
-
-}
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void fsm_init(struct fsm *fsm, events my_event)
-{
-
-
- switch(my_event){
- case EVENT_ENTRY:
- PRINT_STR(fsm->type,"fsm_init: EVENT_ENTRY");
-
- fsm->act_pos = 0;
- fsm->req_pos = 0;
- fsm->can_req_position = 0;
- fsm->timeout_glob = 0; // ms
- fsm->timeout_switch = 0; // ms
- fsm->motor_dir = ENGINE_DIR_FW;
- fsm->lenght = 0;
- fsm->ans_can = 0;
- fsm->p = 1000;
-
- if(fsm->type == FSM_LIFT)
- {
- fsm->irc = &irc2_count;
- fsm->set_engine = &move_lift;
- fsm->sw_pin = LIFT_END_SWITCH; //LIFT_END_SWITCH;
- fsm->speed_home = ENG_LIFT_HOME_SPEED;
- fsm->speed_normal= ENG_LIFT_FULL_SPEED;
- fsm->init_flag = LIFT_LIFT_INIT;
- fsm->speed_slow = ENG_LIFT_HOME_SLOW_SPEED;
- fsm_homing_block = FSM_LIFT;
-
- }
- if(fsm->type == FSM_PUSHER)
- {
- fsm->irc = &irc1_count;
- fsm->set_engine = &move_pusher;
- fsm->sw_pin = PUSH_END_SWITCH;
- fsm->speed_home = ENG_PUSHER_HOME_SPEED;
- fsm->speed_normal= ENG_PUSHER_FULL_SPEED;
- fsm->init_flag = LIFT_PUSHER_INIT;
- fsm->speed_slow = ENG_PUSHER_HOME_SLOW_SPEED;
- }
-
- fsm->last_irc = *fsm->irc;
- can_flags |= fsm->init_flag;
- break;
-
- case EVENT_DO:
-
- if(fsm_homing_block == fsm->type) // block lift until release fsm_homing_block
- fsm->current_state = &fsm_homing_bw;
- break;
-
- case EVENT_EXIT:
-
- break;
- default:break;
- }
-}
-
-
-
-
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void fsm_homing_fw(struct fsm *fsm, events my_event){
-
-
- switch(my_event){
- case EVENT_ENTRY:
- PRINT_STR(fsm->type,"fsm_homing_fw: EVENT_ENTRY");
- fsm->motor_dir = ENGINE_DIR_FW;
- fsm->set_engine(fsm->motor_dir, fsm->speed_home);
- fsm->timeout_glob = timer_msec;
-
- break;
-
- case EVENT_DO:
-
- if(fsm->switch_front) // || (fsm->timeout_glob + TIME_HOMING_WAIT < timer_msec))
- {
- fsm->set_engine(fsm->motor_dir, ENG_STOP);
- fsm->act_pos = 0; // reset position
- fsm->current_state = &fsm_homing_bw; // move to fsm_homing_bw
- PRINT_STR(fsm->type,"fsm_homing_fw: EVENT_DO reached endswitch");
- PRINT_STR_VAL(fsm->type, "Measured IRC: ", *fsm->irc);
- }
-
- break;
-
- case EVENT_EXIT:
- break;
- default:break;
- }
-}
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void fsm_homing_bw(struct fsm *fsm, events my_event){
-
- switch(my_event){
- case EVENT_ENTRY:
- PRINT_STR(fsm->type,"fsm_homing_bw: EVENT_ENTRY");
- fsm->motor_dir = ENGINE_DIR_BW;
- fsm->set_engine(ENGINE_DIR_BW, fsm->speed_home);
- fsm->timeout_switch = timer_msec;
- fsm->timeout_glob = timer_msec; // FIXME nevim jestli je potreba
- break;
-
- case EVENT_DO:
-
- if(fsm->switch_back)
- {
- fsm->set_engine(fsm->motor_dir, ENG_STOP);
- fsm->lenght = -1 * fsm->act_pos; // store measured lenght
- fsm->act_pos = 0; // reset position
- fsm->current_state = &fsm_homing_fw_up; // move to fsm_stop
- fsm->ans_can = 0;
- PRINT_STR(fsm->type,"fsm_homing_bw: EVENT_DO reached endswitch");
-
- }
- break;
-
- case EVENT_EXIT:
- break;
- default:break;
- }
-}
-
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void fsm_homing_fw_up(struct fsm *fsm, events my_event){
-
-
- switch(my_event){
- case EVENT_ENTRY:
- can_flags |= fsm->init_flag;
- PRINT_STR(fsm->type,"fsm_homing_fw_up: EVENT_ENTRY");
- fsm->motor_dir = ENGINE_DIR_FW;
- fsm->set_engine(fsm->motor_dir, fsm->speed_home);
- fsm->timeout_glob = timer_msec;
-
- break;
-
- case EVENT_DO:
-
- if(fsm->switch_front || (fsm->timeout_glob + 1000 < timer_msec))
- {
- fsm->set_engine(fsm->motor_dir, ENG_STOP);
- fsm->current_state = &fsm_homing_bw_zero; // move to fsm_homing_bw
-
- PRINT_STR(fsm->type,"fsm_homing_fw_up: EVENT_DO reached endswitch");
- }
-
- break;
-
- case EVENT_EXIT:
- break;
- default:break;
- }
-}
-
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void fsm_homing_bw_zero(struct fsm *fsm, events my_event){
-
- switch(my_event){
- case EVENT_ENTRY:
- PRINT_STR(fsm->type,"fsm_homing_bw_zero: EVENT_ENTRY");
- fsm->motor_dir = ENGINE_DIR_BW;
- fsm->set_engine(ENGINE_DIR_BW, fsm->speed_slow);
- fsm->timeout_switch = timer_msec;
- fsm->timeout_glob = timer_msec; // FIXME nevim jestli je potreba
- break;
-
- case EVENT_DO:
-
- if(fsm->switch_back)
- {
- fsm->set_engine(fsm->motor_dir, ENG_STOP);
- fsm->act_pos = 0;
-
- if(fsm->type == FSM_LIFT)
- {
- fsm->current_state = &fsm_homing_fw_5;
- }
- else
- { /* PUSHER */
- fsm->current_state = &fsm_stop; // move to fsm_stop
- PRINT_STR(fsm->type,"fsm_homing_bw_zero: EVENT_DO reached endswitch");
- }
- }
- break;
-
- case EVENT_EXIT:
-
- break;
- default:break;
- }
-}
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void fsm_wait_for_pusher_init(struct fsm *fsm, events my_event);
-void fsm_homing_fw_5(struct fsm *fsm, events my_event){
-
-
- switch(my_event){
- case EVENT_ENTRY:
-
- PRINT_STR(fsm->type,"fsm_homing_fw_5: EVENT_ENTRY");
- fsm->motor_dir = ENGINE_DIR_FW;
- fsm->set_engine(fsm->motor_dir, fsm->speed_slow);
- fsm->timeout_glob = timer_msec;
-
- break;
-
- case EVENT_DO:
-
- if(fsm->switch_front || (fsm->act_pos >= (5*8)))
- {
- fsm->set_engine(fsm->motor_dir, ENG_STOP);
- fsm->current_state = fsm_wait_for_pusher_init;
- PRINT_STR_VAL(fsm->type,"fsm_homing_fw_5: EVENT_DO reached endswitch", *fsm->irc);
- fsm_homing_block = FSM_PUSHER; // release pusher
- }
-
- break;
-
- case EVENT_EXIT:
- break;
- default:break;
- }
-}
-
-void fsm_wait_for_pusher_init(struct fsm *fsm, events my_event){
- switch (my_event) {
- case EVENT_ENTRY:
- can_flags &= ~fsm->init_flag;
- break;
- case EVENT_DO:
- if ((can_flags & LIFT_PUSHER_INIT) == 0) {
- fsm->current_state = &fsm_stop; // move to fsm_homing_bw
- }
- break;
- default:;
- }
-}
-
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void fsm_stop(struct fsm *fsm, events my_event){
- switch(my_event){
- case EVENT_ENTRY:
- can_flags &= ~fsm->init_flag;
- can_send_status();
- fsm->set_engine(fsm->motor_dir , ENG_STOP);
- PRINT_STR(fsm->type,"fsm_stop: EVENT_ENTRY");
- break;
-
- case EVENT_DO:
-
-/* if(fsm->type == FSM_LIFT) { */
-/* if ((req >= LIFT_LIFT_PROTECT_PUSHER_TOP) && */
-/* (fsm->act_pos >= LIFT_PUSHER_PROTECT_TOP )) */
-/* can_flags |= LIFT_LIFT_ERR_POS; */
-/* else */
-/* can_flags &= ~(LIFT_LIFT_ERR_POS); */
-/* } */
-/* if (fsm->type == FSM_PUSHER) { */
-/* if (fsm_pusher.act_pos < 0) { */
-/* can_flags |= LIFT_PUSHER_ERR_POS; */
-
-/* } */
-/* else */
-/* can_flags &= ~(LIFT_PUSHER_ERR_POS); */
-/* } */
-
- if(fsm->req_pos != fsm->can_req_position)
- {
- fsm->req_pos = fsm->can_req_position;
- fsm->current_state = &fsm_move; // move to fsm_move
- PRINT_STR_VAL(fsm->type, "fsm_stop: new position recieved", fsm->req_pos);
- PRINT_STR_VAL(fsm->type, "fsm_stop: act pos: ", fsm->act_pos);
- }
-
- break;
- case EVENT_EXIT:
- break;
- }
-}
-
-
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void fsm_move(struct fsm *fsm, events my_event){
- unsigned u;
- int e;
-
- switch(my_event){
- PRINT_STR(fsm->type,"fsm_move: EVENT_ENTRY");
- case EVENT_ENTRY:
- if (fsm->act_pos > fsm->req_pos) {
- fsm->motor_dir = ENGINE_DIR_BW; //set the direction of pusher moving
- }
- else {
- fsm->motor_dir = ENGINE_DIR_FW;
- }
-
- fsm->last_move_time = timer_msec;
- fsm->enable_reg = 1; // enable re
- break;
-
- case EVENT_DO:
-
- /* Check end-switches */
-
- if((fsm->act_pos < fsm->req_pos) && (fsm->act_pos >= 0xF0))
- set_servo(SERVO_DOOR, SERVO_DOOR_MAX);
-
- if ((fsm->motor_dir == ENGINE_DIR_BW && fsm->switch_back) ||
- (fsm->motor_dir == ENGINE_DIR_FW && fsm->switch_front)) {
- fsm->set_engine(fsm->motor_dir, ENG_STOP);
- fsm->current_state = &fsm_wait; // move to fsm_homing_bw
- fsm->ans_can = fsm->req_pos;
- can_send_status();
- PRINT_STR_VAL(fsm->type, "fsm_move: reached endswitch", fsm->req_pos);
- // TODO FIXME TODO FIXME TODO FIXME nastavit act_pos =0?????
- break;
- }
-
- /* Normal movement */
- if (fsm->motor_dir == ENGINE_DIR_BW)
- e = fsm->act_pos - fsm->req_pos;
- else
- e = fsm->req_pos - fsm->act_pos;
-
-
- if(fsm->req_pos == 0) // musi dojet na koncak
- {
- if(e <= 0 || timer_msec - fsm->last_move_time > 90)
- {
- fsm->set_engine(fsm->motor_dir, 35 /*fsm->speed_normal*/);
- fsm->enable_reg = 0;
- return;
- }
- }
-
-
- if (e > 0 && fsm->enable_reg ) {
- /* Go there (P controller + constant to avoid timeout (see below))*/
- u = 20 + e*fsm->p/1000;
- if (u>100) u = 100;
- fsm->set_engine(fsm->motor_dir, u /*fsm->speed_normal*/);
- fsm->ans_can = fsm->act_pos;
- }
-
-
-
- if (e <= 0 || timer_msec - fsm->last_move_time > 100) {
- /* We are there */
- fsm->set_engine(fsm->motor_dir, ENG_STOP);
- if (e <= 0)
- fsm->current_state = &fsm_wait;
- else
- fsm->current_state = &fsm_stop;
- fsm->ans_can = fsm->req_pos;
- can_send_status();
- if (e <= 0)
- PRINT_STR(fsm->type,"fsm_move: reached target");
- else
- PRINT_STR_VAL(fsm->type,"fsm_move: move timeout at pos", fsm->act_pos);
- }
- break;
- case EVENT_EXIT:
- fsm->set_engine(fsm->motor_dir, ENG_STOP);
- break;
- default:break;
- }
-}
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void fsm_wait(struct fsm *fsm, events my_event){
- switch(my_event){
- case EVENT_ENTRY:
- fsm->timeout_glob = timer_msec;
- PRINT_STR(fsm->type,"fsm_wait: EVENT_ENTRY");
- break;
-
- case EVENT_DO:
-
- if((fsm->timeout_glob + TIME_WAIT) >+ timer_msec)
- {
- fsm->current_state = &fsm_stop; // move to fsm_homing_bw
- }
-
- break;
- case EVENT_EXIT:
- PRINT_STR_VAL(fsm->type,"fsm_wait: EXIT at pos", fsm->act_pos);
- break;
- default:break;
- }
-
-
-}
+++ /dev/null
-#ifndef __PUSHER_H
-#define __PUSHER_H
-
- #include "def.h"
-
- #define PUSH_END_SWITCH (1<<17)
- #define LIFT_END_SWITCH (1<<19)
-
-
- #define LIFT_FRONT_SWITCH 21
- #define LIFT_BACK_SWITCH 20
- #define PUSHER_FRONT_SWITCH 23
- #define PUSHER_BACK_SWITCH 22
-
-//************************************************************************************
-//****************** declaration of possible states **********************************
-//************************************************************************************
- void fsm_stop(struct fsm *fsm, events my_event);
- void fsm_move(struct fsm *fsm, events my_event);
- void fsm_init(struct fsm *fsm, events my_event);
- void fsm_homing_fw(struct fsm *fsm, events my_event);
- void fsm_homing_bw(struct fsm *fsm, events my_event);
- void fsm_irc(struct fsm *fsm);
- void fsm_wait(struct fsm *fsm, events my_event);
- void fsm_homing_bwf(struct fsm *fsm, events my_event);
- void fsm_homing_fw_up(struct fsm *fsm, events my_event);
- void fsm_homing_bw_zero(struct fsm *fsm, events my_event);
- void fsm_homing_fw_5(struct fsm *fsm, events my_event);
-
-#endif /*__PUSHER_H*/
*
*
* @author Bc. Jiri Kubias, jiri.kubias@gmail.com
+ * @author Michal Sojka <sojkam1@fel.cvut.cz>
*
- * @addtogroup lift
+ * @addtogroup fork
*/
/**
- * @defgroup lift Lift application
- *
- * Lift control application
- * - controlls the lift and pusher
- * - program recieves CAN commands about wanted lift/pusher position
- * - also controls Holded and right chelae
+ * @defgroup fork Vidle (fork) application
*/
/**
- * @ingroup lift
+ * @ingroup fork
* @{
*/
-////////////////////////////////////////////////////////////////////////////////
-//
-// Eurobot BLINK TEST (with LPC2129)
-//
-// Description
-// -----------
-// This software control mechanisms in eurobot. Use it with lpceurobot board
-// Author : Jarda Sach DCE CVUT
-//
-//
-////////////////////////////////////////////////////////////////////////////////
-
#include <lpc21xx.h> /* LPC21xx definitions */
#include <types.h>
#include <deb_led.h>
#include <periph/can.h>
#include <string.h>
#include <deb_led.h>
-#include "servo.h"
#include "engine.h"
-#include "lift.h"
#include "uar.h"
-#include "def.h"
-#include "irc.h"
#include "vhn.h"
#include <can_msg_def.h>
+#include "fsm.h"
+#include "def.h"
+#define CAN_SPEED 1000000 //< CAN bus speed
+#define CAN_ISR 0
+#define TIMER_IRQ_PRIORITY 5
-struct fsm fsm_lift;
-struct fsm fsm_pusher;
-
-
-void CAN_rx(can_msg_t *msg);
-
-
+struct fsm fsm_vidle;
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void init_motors(void){
init_engine_A(); // initialization of PWM unit
- init_engine_B();
engine_A_en(ENGINE_EN_ON); //enable motor A
- engine_B_en(ENGINE_EN_ON); // ----//----- B
engine_A_dir(ENGINE_DIR_FW); //set direction
- engine_B_dir(ENGINE_DIR_FW);
engine_A_pwm(0); // STOP pwm is in percent, range 0~100~200
- engine_B_pwm(0); // STOP pwm is in percent, range 0~100~200
- vhn_init();
+/* vhn_init(); */
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/* reset timer irq */
T0IR = -1;
- /* read IRCs @100kHz */
- irc_read();
/* increment timer_usec */
timer_usec += 10;
/* increment msec @1kHz */
VICVectAddr = 0;
}
+void CAN_rx(can_msg_t *msg) {
+ 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]));
+
+ fsm_vidle.can_req_position = req;// save new req position of lift
+ break;
+ default:break;
+ }
+
+ deb_led_off(LEDB);
+}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
init_motors();
- init_servo(SERVO_PRIORITY);//SERVO_PRIORITY is level of intrrupt, is defined in lift header file
- set_servo(SERVO_DOOR, SERVO_DOOR_MAX);
- set_servo(CHELAE_R, CHELAE_R_MAX);
- set_servo(2, 0x80);
- /* init timer0 */
+
+ /* init timer0 */
init_timer0(1, CPU_APB_HZ/100000);
- set_irq_handler(4 /*timer0*/, IRC_PRIORITY, timer0_irq);
+ set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
- /* init IRCs */
- irc_init();
init_uart();
- vhn_init();
+/* vhn_init(); */ // What is VHN? -MS
}
/*********************************************************/
void can_send_status(void)
tx_msg.id = CAN_VIDLE_STATUS;
tx_msg.dlc = 5;
tx_msg.flags = 0;
- tx_msg.data[0] = (fsm_pusher.ans_can >> 8) & 0xFF;
- tx_msg.data[1] = fsm_pusher.ans_can & 0xFF;
- tx_msg.data[2] = (fsm_lift.ans_can >> 8) & 0xFF;
- tx_msg.data[3] = fsm_lift.ans_can & 0xFF;
- tx_msg.data[4] = can_flags;
+ 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 */
}
-void can_send_len(void)
-{
- can_msg_t tx_msg;
- tx_msg.id = CAN_VIDLE_STATUS;
- tx_msg.dlc = 4;
- tx_msg.flags = 0;
- tx_msg.data[0] = (fsm_pusher.lenght >> 8) & 0xFF;
- tx_msg.data[1] = fsm_pusher.lenght & 0xFF;
- tx_msg.data[2] = (fsm_lift.lenght >> 8) & 0xFF;
- tx_msg.data[3] = fsm_lift.lenght & 0xFF;
- while(can_tx_msg(&tx_msg));
-
-}
-
-
-void can_send_db(void)
-{
- can_msg_t tx_msg;
- tx_msg.id = CAN_DB_LP;
- tx_msg.dlc = 4;
- tx_msg.flags = 0;
- tx_msg.data[0] = (fsm_pusher.act_pos >> 8) & 0xFF;
- tx_msg.data[1] = fsm_pusher.act_pos & 0xFF;
- tx_msg.data[2] = (fsm_lift.act_pos >> 8) & 0xFF;
- tx_msg.data[3] = fsm_lift.act_pos & 0xFF;
- while(can_tx_msg(&tx_msg));
-
-}
-
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void CAN_rx(can_msg_t *msg) {
- 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);
-#if 0
- switch (rx_msg.id)
- {
- case CAN_LIFT:
- deb_led_on(LEDB);
- req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
-
- fsm_lift.can_req_position = req;// save new req position of lift
- break;
- case CAN_PUSHER:
- deb_led_on(LEDB);
- req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
- fsm_pusher.can_req_position = req;// save new req position of lift
- break;
-
- case CAN_HOLDER:
- deb_led_on(LEDB);
- set_servo(SERVO_DOOR, SERVO_DOOR_MIN + (0xFF -rx_msg.data[0]) * (SERVO_DOOR_MAX - SERVO_DOOR_MIN)/255 ); //set servo position TODO: limits
- break;
-
- case CAN_CHELAE:
- deb_led_on(LEDB);
- set_servo(CHELAE_R , CHELAE_R_MIN + (0xFF - rx_msg.data[1]) * (CHELAE_R_MAX - CHELAE_R_MIN)/255 ); //set servo position TODO: limits
-
- default:break;
- }
-#endif
- deb_led_off(LEDB);
-}
-
-
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void run_fsm(struct fsm *fsm){
- fsm_irc(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
+
+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);
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-int main(void){
+int main(void)
+{
+ uint32_t main_time = timer_usec;
+ uint32_t led_time = timer_msec;
+ uint32_t CAN_time = timer_msec;
-// ~~~~~ initialization of periphery ~~~~~
- can_flags = 0;
init_periphery();
- fsm_pusher.type = FSM_PUSHER;
- fsm_lift.type = FSM_LIFT;
-
- fsm_lift.current_state = &fsm_init;
- fsm_pusher.current_state = &fsm_init;
- fsm_lift.current_state(&fsm_lift, EVENT_ENTRY);
- fsm_pusher.current_state(&fsm_pusher, EVENT_ENTRY);
+ init_fsm(&fsm_vidle, &fsm_vidle_init);
- uint32_t main_time = timer_usec;
- uint32_t led_time = timer_msec;
- uint32_t CAN_time = timer_msec;
-
+ /* TODO: Add comment */
SET_PIN(PINSEL1, 1, PINSEL_0);
SET_PIN(PINSEL1, 3, PINSEL_0);
-
IO1DIR &= ~(3<<20);
-// while(1)
-// {
-// if(IO1PIN & (1<<20))
-// deb_led_on(LEDY);
-// else
-// deb_led_off(LEDY);
-//
-//
-// if(IO1PIN & (1<<21))
-// deb_led_on(LEDR);
-// else
-// deb_led_off(LEDR);
-// }
-
-
- send_rs_str("list started\n");
-
+ send_rs_str("Vidle started\n");
while(1){
if(timer_usec >= main_time + 1000)
{
- main_time = timer_usec;
-
- 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);
-
- run_fsm(&fsm_lift);
- run_fsm(&fsm_pusher);
+ main_time = timer_usec;
-#if 0
- fsm_lift.switch_front = (0 != (IO1PIN & (1<<LIFT_FRONT_SWITCH)));
- fsm_lift.switch_back = (0 != (IO1PIN & (1<<LIFT_BACK_SWITCH)));
- {
- static int counter = 0;
- /* Histereze koncaku */
- if (0 != (IO1PIN & (1<<PUSHER_FRONT_SWITCH))) counter++;
- else counter--;
- if (counter > 10) {
- counter=10;
- fsm_pusher.switch_front = true;
- }
- if (counter < 0) {
- fsm_pusher.switch_front = false;
- counter = 0;
- }
- }
- fsm_pusher.switch_back = (0 != (IO1PIN & (1<<PUSHER_BACK_SWITCH)));
-
- if (fsm_lift.switch_front) can_flags |= LIFT_LIFT_ENDSW_UP;
- else can_flags &= ~LIFT_LIFT_ENDSW_UP;
- if (fsm_lift.switch_back) can_flags |= LIFT_LIFT_ENDSW_DOWN;
- else can_flags &= ~LIFT_LIFT_ENDSW_DOWN;
+ dbg_print_time();
+
+ /* TODO: Precti potak a uloz ho do fsm_vidle.act_pos */
- if (fsm_pusher.switch_front) can_flags |= LIFT_PUSHER_ENDSW_FW;
- else can_flags &= ~LIFT_PUSHER_ENDSW_FW;
- if (fsm_pusher.switch_back) can_flags |= LIFT_PUSHER_ENDSW_BK;
- else can_flags &= ~LIFT_PUSHER_ENDSW_BK;
-#endif
- // can_send_db();
+ run_fsm(&fsm_vidle);
}
#if 0
- if (timer_msec >= CAN_time + 100){ //repeat sending message every 100 ms
- CAN_time = timer_msec; //save new time, when message was sent
+ if (timer_msec >= CAN_time + 100 || //repeat sending message every 100 ms
+ fsm_vidle.trigger_can_send) { //or when something important happen
+ fsm_vidle.trigger_can_send = false;
+ CAN_time = timer_msec; //save new time, when message was sent
can_send_status();
+
}
#endif
if(timer_msec >= led_time + 500)
{
- led_time = timer_msec;
- deb_led_change(LEDG);
+ led_time = timer_msec;
+ deb_led_change(LEDG);
}
}
}
+++ /dev/null
-
-
-#include <lpc21xx.h> // LPC21XX Peripheral Registers
-#include <deb_led.h>
-#include <system_def.h>
-#include "serv.h"
-
-
-#define SERVO (1<<12)
-
-
-#define TIM_EMR_NOTHING 0
-#define TIM_EMR_CLEAR 1
-#define TIM_EMR_SET 2
-#define TIM_EMR_TOGLE 3
-
-#define TIM_EMR_PIN_ON 1
-#define TIM_EMR_PIN_OFF 0
-
-#define TIME20MS ((CPU_APB_HZ) / 50)
-#define SERVOTICK (((CPU_APB_HZ / 50) / 20) / 256)
-
-
-unsigned char servo;
-
-
-
-// ---------------- SERVO PART -------------------------------
-
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-//~~~~~ RUTINE FOR INTERRUPT SERVICE FROM T1 ~~~~~~~
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void tc1 (void) {
- time_ms +=20;
- T1EMR |= (TIM_EMR_PIN_ON<<0) | (TIM_EMR_PIN_ON<<1) | (TIM_EMR_PIN_ON<<3);
- T1MR3 = (servo+256) * SERVOTICK;
- if (T1IR != 4)
- {
- __deb_led_on(LEDR);
- }
- T1IR = 4; // Vynulovani priznaku preruseni
- VICVectAddr = 0; // Potvrzeni o obsluze preruseni
-}
-
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-void set_servo(char position)
-{
- servo = position;
-}
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-/* Setup the Timer Counter 1 Interrupt */
-void init_servo (unsigned rx_isr_vect)
-{
-
- IO0DIR|= SERVO; // set pin as output
- IO0SET|= SERVO; // sets to High level on pin P0.12
-
- PINSEL0 &= ~((PINSEL_3 << 24));
- PINSEL0 |= (PINSEL_2 << 24);//chose TMR1 interrupt for P0.12
- PINSEL1 &= ~(PINSEL_3 << 8);
- PINSEL1 |= (PINSEL_1 << 8);//chose TMR1 interrupt for P0.20
-
- servo = 0xff;
-
- T1PR = 0;
- T1MR2 = TIME20MS;
- T1MR0 = (servo+256)*SERVOTICK;
- T1MCR = (3<<6); // interrupt on MR1 nterrupt on MR2 an interrupt is generated when MR2 matches the value in the TC
-
- T1EMR = (TIM_EMR_PIN_ON<<0) | (TIM_EMR_PIN_ON<<1) | (TIM_EMR_PIN_ON<<3) \
- | (TIM_EMR_CLEAR << 4) | (TIM_EMR_CLEAR << 6) | (TIM_EMR_CLEAR << 10);
-
- T1TCR = 1; // Starts Timer 1
-
- ((uint32_t*)&VICVectAddr0)[rx_isr_vect] = (unsigned long)tc1; // Nastaveni adresy vektoru preruseni
- ((uint32_t*)&VICVectCntl0)[rx_isr_vect] = 0x20 | 0x5; // vyber casovece pro preruseni
- VICIntEnable = (1<<5); // Povoli obsluhu preruseni
-}
-
-
-// ---------------- powSitch PART -------------------------------
-
+++ /dev/null
-/**
- * @file servo.h
- *
- * This file provides simply how-to use eb_ebb library.
- * From main function is called init_perip function
- * where is initialized servos, engines, power switch,
- * CAN,ADC and serial port. After this initialization is shown
- * how to control each devices. This sample also include simply
- * sample of sending and receiving CAN message.
- *
- */
-
-
-
-#ifndef SERVO_H
-#define SERVO_H
-
-//registration of interrupt metod
-void tc1 (void) __attribute__ ((interrupt));
-
-
-/** gobal time
- * @note incremented twenty 20ms, overrun every 1194hours
- */
-volatile unsigned int time_ms;
-
-/** Initialize servos
- * @note All three servos - should be fixed FIXME
- */
-void init_servo(unsigned rx_isr_vect);
-
-
-/** Sets serv position
- * @return 0
- * @note VPB = APB - name conflict FIXME
- * @param servo define servo
- * @param position new position for servo
- */
-void set_servo(char position);
-
-#endif
* Send string to serial output in ASCII code. .
* @param data[] string to print
*/
-void send_rs_str(char data[])
+void send_rs_str(const char data[])
{
int i = 0;
void send_rs_int(int val);
-void send_rs_str(char data[]);
+void send_rs_str(const char data[]);
void init_uart(void);
char uart_get_char(void);
void uart_send_char(char val);
#ifndef __VHN_H
#define __VHN_H
- #include "def.h"
-
#define VHN_EN_A (1<<16)
#define VHN_EN_B (1<<17)
#define VHN_IN_A (1<<18)
projects / eurobot / public.git / commitdiff